Chix_Oss

// Taken from ChickenParmis fantastic site http://www.duino-robotics.com/



// add some libraries for ping and servo
#include <Servo.h>
#include <NewPing.h>

#define USTrigger 10       //picks ping trigger pin
#define USEcho 11         //picks ping recievor pin, signal in
#define MaxDistance 100  // sets pings maximum range
#define LED 13           // picks pin 13 for included led on board

Servo leftServo;       //Create Left Servo object, lets arduino program know it has a left servo
Servo rightServo;      //Create Right Servo object, lets arduino program know it has right servo
Servo servo;          //Create sweep Servo object, lets arduino program know it has a servo to sweep ping
NewPing sonar(USTrigger, USEcho, MaxDistance); //Create ultrasonic object, arduino now know it got a ping

//Below we are creating unsigned integer variables which we will use later on in the code. They are unsigned as they will only have postive values
//if you sign them they can have positive and negitive values, thanks 6677, Birdmun also says that just using positive ones saves program space
unsigned int duration;                //this is going to be time between sending and getting signal back from ping, gonna use it to make a distance
unsigned int distance;                //this is going to be the distance in cm we get from ping
unsigned int FrontDistance;           //this is the number we get from a ping reading looking straight ahead
unsigned int LeftDistance;            //this is the number we get from a ping reading looking to the left, check servo is really looking left
unsigned int RightDistance;           // same as above but looking right
unsigned int Time   ;

void setup()                  //This part of the program runs 1 time at start up
{
  leftServo.attach(9);        //Left servo connected to pin 9
  leftServo.write(90) ;       //Write the neutral position to that servo
  rightServo.attach(7);       //Right servo connected to pin 7
  rightServo.write(90);       //Write the neutral position to that servo
  servo.attach (6);          // puts ping servo on pin 6
}


void loop()                                           //This block repeats itself while the Arduino is turned on
{
  servo.write(90);                                    //Rotate the servo to face the front
  scan();                                             //Go to the scan function
  FrontDistance = distance;                           //Set the variable FrontDistance to the value of the distance returned from the scan function
  Serial.println("Front distance = ");
  Serial.print(distance);
  if(FrontDistance > 20 || FrontDistance == 0){       //If there is nothing infront of the robot within 20cm or the distance value is 0 (which for the newping libary means no ping was returned) then.


   moveForward();                                     //Go to the moveForward function
  }
  else                                                //Else (if there is something infront of the robot within 40cm) then...
  {
    moveStop();                                       //Go to the moveStop function
    navigate();                                       //Go to navigate function
  }
}

void moveBackward(){
  rightServo.write(180);
  leftServo.write(0);
}

void moveForward(){
  rightServo.write(0);
  leftServo.write(180);
}

void moveRight(){
  rightServo.write(0);
  leftServo.write(0);
}

void moveLeft(){
  rightServo.write(180);
  leftServo.write(180);
}

void moveStop(){
  rightServo.write(90);
  leftServo.write(90);
}

void scan(){                                        //This function determines the distance things are away from the ultrasonic sensor{
  Serial.println("");
  Serial.println("Scanning");
  Time = sonar.ping();
  distance = Time / US_ROUNDTRIP_CM;
  delay(500);
}
void navigate()
{
    Serial.println("There's an obstacle!");
    servo.write(130);                                 //Move the servo to the left (my little servos didn't like going to 180 so I played around with the value until it worked nicely)
    delay(1000);                                       //Wait half a second for the servo to get there
    scan();                                           //Go to the scan function
    LeftDistance = distance;                          //Set the variable LeftDistance to the distance on the left
    Serial.println("Left distance = ");
    Serial.print(distance);
    servo.write(50);                                   //Move the servo to the right
    delay(1000);                                       //Wait half a second for the servo to get there
    scan();                                           //Go to the scan function
    RightDistance = distance;                         //Set the variable RightDistance to the distance on the right
    Serial.println("Right distance = ");
    Serial.print(distance);
    if(abs(RightDistance - LeftDistance) < 5)
    {
      moveBackward();                                  //Go to the moveBackward function
      delay(200);                                      //Pause the program for 200 milliseconds to let the robot reverse
      moveRight();                                     //Go to the moveRight function
      delay(100);                                      //Pause the program for 200 milliseconds to let the robot turn right
    }
    else if(RightDistance < LeftDistance)                  //If the distance on the right is less than that on the left then...
    {
     moveLeft();                                      //Go to the moveLeft function
     delay(100);                                      //Pause the program for half a second to let the robot turn
    }
    else if(LeftDistance < RightDistance)             //Else if the distance on the left is less than that on the right then...
    {
     moveRight();                                     //Go to the moveRight function
     delay(100);                                      //Pause the program for half a second to let the robot turn
    }
}