Chibajoss with assits

// All code learned and modded from Jayden "ChickenParmis" fantastic site http://www.duino-robotics.com/
// So Lets Make Robots! here's the place to do it http://letsmakerobots.com/
//many thanks to all I welcome all idea to improve ossipee


// add some libraries for ping and servo, OddBot recomends I just do a function, planning on it
#include <Servo.h>
#include <NewPing.h>

#define USTrigger 7       //picks ping trigger pin
#define USEcho 8          //picks ping recievor pin, signal in
#define MaxDistance 100   // sets pings maximum range
#define LED 13            // picks pin 13 for included led on board not used right now

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's it's 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(10);       //Right servo connected to pin 10
  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);                                    //position the ping servo to face ahead could have done above but like it here
  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(){                          //the sevos are opposite on bot thus forwardon 1 = reverse on other
  rightServo.write(180);                      //reverses right servo
  leftServo.write(0);                         //reverses left servo
}

void moveForward(){
  rightServo.write(0);                       //right servo rotating forward
  leftServo.write(180);                      //left servo rotating forward
}

void moveRight(){                           //turns robot right, might be worth trying just stopping a servo
  rightServo.write(0);                      //right servo turning forward direction
  leftServo.write(0);                       //left servo turning reverse direction
}

void moveLeft(){                           //turns robot right just opposite servo directions
  rightServo.write(180);
  leftServo.write(180);
}

void moveStop(){                           //90 is arduino servo central position, modded servos need to tweek
  rightServo.write(90);                   // center stop servo position
  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);                                       //delay(1000) = 1000 millaseconds, 1 second
    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 a second for the servo to get there
    scan();                                           //Go to the scan function called above
    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
    }
}