/******************************************************************************
  Project      :  TWEENBOT I - MECHATRONICS PROJECT
  Author       :  Cao Hoang Long - Tweenbot 1 Team
  Start date   :  Nov 19th 2011
  Hardware     :  TWEENBOT1 - v2.0
                  ~5   -> IN1_L
                  ~6   -> IN2_L
                  ~9   -> IN1_R
                  ~10  -> IN2_R
                   7   -> LED
                   3   -> SPEAKER
                   A0-A5-> SENSORS: A0:head-bottom, A1:tale-bottom, A2-A5:front sensors
  Contact      :   Mechatronics06@yahoogroups.com
******************************************************************************/
/* TESTING INFO
  19/11/11: Control motor speed
            Test with IR-sensor
  10/12/11: Program done, no test
  13/12/11: Test full program. Done!
  15/12/11: Modify warning1, robot run backward when detecting hole, then, keep warning. No test
            Modify prescale for ADC to encrease sampling time. No test
  16/12/11: Test 15/12 program. Done!
  20/12/11: Mofify referent values
  22/12/11: Test in public
*/

/*****************************************************************************
* DEFINES & CONSTANTS
*/

// Defines for setting and clearing register bits
#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif
#ifndef sbi
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#endif


// Pins and ports
int SPEAKER = 3;      // select the pin for the BUZZER
int LED = 7;          // select the pin for the LED

int IN1_L = 5;      // Input_1 of Left motor connected to digital pin 5
int IN2_L = 6;      // Input_2 of Left motor connected to digital pin 6
int IN1_R = 9;      // Input_1 of Right motor connected to digital pin 9
int IN2_R = 10;     // Input_2 of Right motor connected to digital pin 10

byte sensor0 = A0;   // IR sensor input0
byte sensor1 = A1;   // IR sensor input1
byte sensor2 = A2;   // IR sensor input2
byte sensor3 = A3;   // IR sensor input3
byte sensor4 = A4;   // IR sensor input4
byte sensor5 = A5;   // IR sensor input5

byte sensor_input[]= {sensor0,sensor1,sensor2,sensor3,sensor4,sensor5};
int  ref_value[]   = {600    ,600    ,300    ,300    ,300    ,300    };  // Reference value to detect obstacles, holes, stairs or being held by people
                  //  head    tale   // front sensors
// testing only
//int  ref_value[]   = {200    ,200    ,800    ,800    ,800    ,200    };  // Reference value to detect obstacles, holes, stairs or being held by people
                  //  head    tale   // front sensors

// Return values for read_sensor()
#define  NO_OBSTACLE  0
#define  HOLE         1
#define  PEOPLE       2
#define  OBSTACLE     3

/*****************************************************************************
* SETUP
*/
void setup()
{
  // Set prescale of ADC to 16
  sbi(ADCSRA,ADPS2) ;
  cbi(ADCSRA,ADPS1) ;
  cbi(ADCSRA,ADPS0) ;
  // Setup pins
  pinMode(LED, OUTPUT);  // sets the pin as output
  pinMode(SPEAKER, OUTPUT);  // sets the pin as output
  pinMode(IN1_L, OUTPUT);   // sets the pin as output
  pinMode(IN2_L, OUTPUT);   // sets the pin as output
  pinMode(IN1_R, OUTPUT);   // sets the pin as output
  pinMode(IN2_R, OUTPUT);   // sets the pin as output
}

/*****************************************************************************
* FUNCTIONS
*/

/* @fn      read_sensors
 * @brief   Read 6 sensors
 * @param   void
 * @return  byte: NO_OBSTACLE, HOLE, PEOPLE, OBSTACLE
 */
byte read_sensors(void)
 {
   int sensor_value[6]={0,0,0,0,0,0};
   boolean head_bottom = 0;
   boolean tale_bottom = 0;
   boolean front = 0; 
   
   // Read 2 bottom sensors
   sensor_value[0]= analogRead(sensor_input[0]);
   if (sensor_value[0] < ref_value[0])
   {
     delay(10);  // Delay 10ms to avoid noise
     //Read again
     sensor_value[0]= analogRead(sensor_input[0]);
     // Stable signal -> head-bottom
     if (sensor_value[0] < ref_value[0])
       head_bottom = 1;  // head-bottom is active  
     // noise -> Continue to read other sensors
   }
   
   sensor_value[1]= analogRead(sensor_input[1]);
   if (sensor_value[1] < ref_value[1])
   {
     delay(10);  // Delay 10ms to avoid noise
     //Read again
     sensor_value[1]= analogRead(sensor_input[1]);
     // Stable signal -> tale-bottom
     if (sensor_value[1] < ref_value[1])
       tale_bottom = 1;  // tale-bottom is active  
     // noise -> Continue to read other sensors
   }
   
   // Distinguish hole/stair and be lifted by people
   if ( (head_bottom==1)&&(tale_bottom==1) ) // both bottom sensors are active-> being lifted
     return PEOPLE;
   else if ( (head_bottom==1)&&(tale_bottom==0) )  // only head-bottom sensor is active-> hole/stair
     return HOLE;
     
   // Read 4 front sensors (if the case is not PEOPLE or HOLE)
   for (byte i=2;i<6;i++)
   {
     sensor_value[i]= analogRead(sensor_input[i]);
     if (sensor_value[i] > ref_value[i])
     {
       delay(200);  // Delay 100ms to avoid noise
       //Read again
       sensor_value[i]= analogRead(sensor_input[i]);
       // Stable signal -> Return: "Obstacle"
       if (sensor_value[i] > ref_value[i])
         return OBSTACLE;  // obstable(s) in front of robot  
       // noise -> Continue to read other sensors
     }
   }
   // No obstacle
   return NO_OBSTACLE;
 }   

 
/* @fn      l_motor_speed
 * @brief   Control speed of left motor
 * @param   int speed: 0% - 100%
 * @return  void
 */
void l_motor_speed(signed int speed)  
{
  if (speed>=0)
  {
    analogWrite(IN1_L, speed*2.55);  // Convert from percentage to pwm value
    analogWrite(IN2_L, 0);
  } 
  else
  {
    analogWrite(IN2_L, -speed*2.55);  // Convert from percentage to pwm value
    analogWrite(IN1_L, 0);
  } 
}

/* @fn      r_motor_speed
 * @brief   Control speed of right motor
 * @param   int speed: 0% - 100%
 * @return  void
 */
void r_motor_speed(signed int speed)  
{
  if (speed>=0)
  {
    analogWrite(IN1_R, speed*2.55);  // Convert from percentage to pwm value
    analogWrite(IN2_R, 0);
  } 
  else
  {
    analogWrite(IN2_R, -speed*2.55);  // Convert from percentage to pwm value
    analogWrite(IN1_R, 0);
  } 
}

/* @fn      speed
 * @brief   Control speed of 2 motors
 * @param   int l_speed: 0% - 100% speed of left motor
            int r_speed: 0% - 100% speed of right motor
 * @return  void
 */
void speed(signed int l_speed,signed int r_speed)  
{
  l_motor_speed(l_speed);
  r_motor_speed(r_speed);
}

/* @fn      warning1
 * @brief   Attract people
 * @param   void
 * @return  void
 */
void warning1(void)  
{
  speed(-50,-50);
  digitalWrite(LED, HIGH);
  digitalWrite(SPEAKER, HIGH);
  delay(300);
  
  speed(0,0);
  digitalWrite(LED, LOW);
  digitalWrite(SPEAKER, LOW);
  delay(1000);
  
  speed(50,50);
  digitalWrite(LED, HIGH);
  digitalWrite(SPEAKER, HIGH);
  delay(200);
  
  speed(0,0);
  digitalWrite(LED, LOW);
  digitalWrite(SPEAKER, LOW);
  delay(1000);
}

/* @fn      warning2
 * @brief   happy when being held (Pika Pikaaa)
 * @param   void
 * @return  void
 */
void warning2(void)  
{
  // Pi
  digitalWrite(LED, HIGH);
  digitalWrite(SPEAKER, HIGH);
  delay(50);
  digitalWrite(LED, LOW);
  digitalWrite(SPEAKER, LOW);
  delay(200);
  // Kaa
  digitalWrite(LED, HIGH);
  digitalWrite(SPEAKER, HIGH);
  delay(200);
  digitalWrite(LED, LOW);
  digitalWrite(SPEAKER, LOW);
  delay(200);
  // Pi 
  digitalWrite(LED, HIGH);
  digitalWrite(SPEAKER, HIGH);
  delay(50);
  digitalWrite(LED, LOW);
  digitalWrite(SPEAKER, LOW);
  delay(200);
  // Kaaaaaa
  digitalWrite(LED, HIGH);
  digitalWrite(SPEAKER, HIGH);
  delay(1000);
  digitalWrite(LED, LOW);
  digitalWrite(SPEAKER, LOW);
  delay(1000);
  
}

//void warning3(void)  
//{
//  digitalWrite(LED, HIGH);
//  digitalWrite(SPEAKER, HIGH);
//  delay(50);
//  digitalWrite(LED, LOW);
//  digitalWrite(SPEAKER, LOW);
//  delay(50);
//  digitalWrite(LED, HIGH);
//  digitalWrite(SPEAKER, HIGH);
//  delay(50);
//  digitalWrite(LED, LOW);
//  digitalWrite(SPEAKER, LOW);
//  delay(50);
//  digitalWrite(LED, HIGH);
//  digitalWrite(SPEAKER, HIGH);
//  delay(500);
//  digitalWrite(LED, LOW);
//  digitalWrite(SPEAKER, LOW);
//  delay(50);
//}
/*****************************************************************************
* SYSTEM INITIALIZATION
*/
void sys_init(void)
{
  digitalWrite(LED, HIGH); // Turn LEDs on
  digitalWrite(SPEAKER, LOW); // Turn buzzer off
  speed(0,0);  // Robot stops
}


/*****************************************************************************
* MAIN LOOP
*/
void loop()
{
  // Initial system
  sys_init();
  
  while(1) {
    
    switch (read_sensors())
    {
       // Hole is dectected
       case HOLE:
       {
         // Robot stops
         speed(0,0);  
         delay(50);
         // Robot runs backward 1 second
         speed(-100,-100);
         delay(1000);
         // Keep warning until robot is lifted
         while (read_sensors()!=PEOPLE) warning1(); // Warning 1 (attract people)
         break;
       }
      // Robot is being lifted by people
      case PEOPLE:
       {
         speed(0,0);  // Robot stops
         warning2();  // Warning 2 (pikapikaa happy)
         break;
       }
      // Obstacle(s) is detected in front of robot
      case OBSTACLE:
       {
         speed(0,0);  // Robot stops
         warning1();  // Warning 1 (attract people)
         break;
       }
      
      case NO_OBSTACLE:
       {
         speed(45,45);  // Robot runs forward 45% maxspeed
         digitalWrite(LED, HIGH);
         digitalWrite(SPEAKER, LOW);
         break;
       }
    }// end switch
  }// end while
} // end main