File content as of revision 13:bdb92f041eca:

#include "mbed.h"
#include "Servo.h"

#define DISTANCE 0.45
#define VBORDER1 0.7    //VBORDER being the LDR voltage value when going from the black line to the white surface and vice versa, assume >0.7 is white surfce
#define VBORDER2 0.3    // assume black surface <0.3



Serial pc (USBTX, USBRX); // USB serial interface

Servo ServoRight(p21); // continuous rotation hobby servo right
Servo ServoLeft(p22); // continuous rotation hobby servo left

PwmOut LEDleft(p23);
PwmOut LEDright(p24);

AnalogIn IRfront(p15);
AnalogIn IRleft(p16);
AnalogIn IRright(p17);
AnalogIn LDRleft(p18);
AnalogIn LDRright(p19);

void init_servo() {
    // calibrate the servos for +/-5ms over +/-45deg
    ServoRight.calibrate(0.0005,45);
    ServoLeft.calibrate(0.0005,45);
}

int main() {
    float front;
    float left;
//    float right;
    float LDRvalueleft;
    float LDRvalueright;

     // set the USB serial interface baud rate
     pc.baud(921600);
     init_servo();

 while(1) {
    
    // Auto drive : the car will avoid all obstacles aroiund it and get away from them
    front = IRfront.read();
    left  = IRleft.read();
//    right = IRright.read();
    LDRvalueright = LDRright.read();
    LDRvalueleft = LDRleft.read();       //output voltage of the LDR used for line following feature
    
   /*****************************************************************************
   / if (front > DISTANCE) {           Collision avoidance system only,the robot will move away from an obstacle/wall if getting to close to it, backup program if the following doesn't work
    /    if (left > DISTANCE) {
    /        ServoRight.write(0.5);   //turn right
    /        ServoLeft.write(0.4);
    /    } else {
    /        ServoRight.write(0.6);   // turn left
    /        ServoLeft.write(0.5);    
    /    }
    / } else if (left > DISTANCE) {
    /    if (right < DISTANCE) {
    /        ServoRight.write(0.5); // right
    /        ServoLeft.write(0.4);
    /    } else {
    /        ServoRight.write(0.6); // forward    
    /        ServoLeft.write(0.4);
    /    }
    / }
   *****************************************************************************/
   
    //Turn both LED's on for the line following feature
    LEDleft=1;
    LEDright=1;
    
    // send the servo command to the servos themselves
    //ServoRight.write(0.6); // write to the continuous rotation servo1
   // ServoLeft.write(0.4); // write to the continous servo2
   
   if (front > DISTANCE) {
       // turn right
       ServoRight.write(0.5); // stop servoright
       ServoLeft.write(0.4); // turn on servoleft 
       wait(2.0); //wait 2 seconds to allow the robot to turn approximately 90 degrees
       // go straight for a second to establish separation from the object
       ServoRight.write(0.6); 
       ServoLeft.write(0.4);
       wait(1.0);
       if (left > DISTANCE) {
           // keep going straight for 1 second
           ServoRight.write(0.6); // write to the continuous rotation servo1
           ServoLeft.write(0.4); // write to the continous servo2
           wait(1.0);
       } else {
           // turn left 90 degrees
           ServoRight.write(0.5);
           ServoLeft.write(0.4);
           wait(2.0); // wait 2 seconds to allow the robot to turn approx. 90 degrees
           ServoRight.write(0.6); //go straight for 1 second
           ServoLeft.write(0.4);
           if (left > DISTANCE) {
           //keep going straight for 1 second
           ServoRight.write(0.6);
           ServoLeft.write(0.4);
           wait(1.0);      //wait for 1 second
           } else {
               // turn left 90 degrees
               ServoRight.write(0.6);
               ServoLeft.write(0.5);
               wait(2.0);     //wait 2 seconds to allow robot to turn 90 degrees
               ServoRight.write(0.6);    //after turning 90 deg turn both servos on so the robot goes straight ahead
               ServoLeft.write(0.4);
               }     
           }
           }
    else if (front < DISTANCE) {
     //follow line
        if (LDRvalueright < VBORDER2 && LDRvalueleft > VBORDER1){ //ldr right is in the black and ldr left is in the white then turn right
            // turn right
             ServoRight.write(0.5);
             ServoLeft.write(0.4);
            }
        if (LDRvalueright > VBORDER1 && LDRvalueleft < VBORDER2){   //ldr right is in the white and ldr left is in the black
            //turn left  
            ServoRight.write(0.6);
            ServoLeft.write(0.5);
            }
        if (LDRvalueright < VBORDER2 && LDRvalueleft < VBORDER2) {    //both ldr right and ldr left are in the black, continue ahead
            //go straight
            ServoRight.write(0.6);
            ServoLeft.write(0.4);
            }
        
        
        
          
     }      
}
}