Birdmun_teaches_oss

    // 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>

    //some programming etiquette suggests constants, which #defines are, should be
    //all CAPS. It assists in reading the code. Also, having values defined at/near
    //the top of your program lets you make adjustments without having to search for
    //the "magic numbers" hidden in your code.
    #define USTrigger 7       //picks ping trigger pin
    #define USEcho 8          //picks ping recievor pin, signal in
    #define MaxDistance 100   // sets pings maximum range
    #define MINDISTANCE 20    //sets min distance for avoidance
    #define LED 13            // pin 13 for included led on board unused right now
    #define LEFTREV 0         // left servo full reverse
    #define LEFTFWD 180       // left servo full forward
    #define LEFTSTOP 90       // left servo stop
    #define RIGHTREV 180      // right servo full reverse
    #define RIGHTFWD 0        // right servo full forward
    #define RIGHTSTOP 90      // right servo stop
    #define DEADBAND 5        // value above/below center that will back robot out of corners
    #define SWEEPLEFT 130     // sweep servo face left
    #define SWEEPRIGHT 50     // sweep servo face right
    #define SWEEPCENTER 90    // center sweep servo

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

    //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
    //this is going to be time between sending and getting signal back from ping,
    //gonna use it to make a distance
    //~ unsigned int duration; //no longer required
    //***variables moved to local functions
    //this is going to be the distance in cm we get from ping
    //~ unsigned int distance; //doesn't need to be global
    //~ //this is the number we get from a ping reading looking straight ahead
    //~ unsigned int FrontDistance;
    //~ //this is the number we get from a ping reading looking to the left,
    //~ //check servo is really looking left
    //~ unsigned int LeftDistance;
    //~ // same as above but looking right
    //~ unsigned int RightDistance;

    void setup() {          //This part of the program runs 1 time at start up
      Serial.begin(9600); //required if you are going to send data via serial
      leftServo.attach(9);   //Left servo connected to pin 9
      leftServo.write(LEFTSTOP);  //Write the neutral position to that servo
      rightServo.attach(10); //Right servo connected to pin 10
      rightServo.write(RIGHTSTOP);  //Write the neutral position to that servo
      servo.attach (6);      // puts ping servo on pin 6
    }

    //This block repeats itself while the Arduino is turned on
    void loop() {
      //this is the number we get from a ping reading looking straight ahead
      unsigned int FrontDistance;
      servo.write(SWEEPCENTER); //position the ping servo to face ahead could have done above but like it here
      //Set the variable FrontDistance to
      //the value of the distance returned from the scan function
      FrontDistance = scan();
      Serial.println("Front distance = ");
      Serial.print(FrontDistance);
      //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.
      if(FrontDistance > MINDISTANCE || FrontDistance == 0) {
         moveRobot(100, 100, 500); //move robot forward for 0.5 seconds
         //moveRobot(100, 100, 0); //delay may not be needed try both ways
      } else { //Else (if there is something infront of the robot within 40cm) then
        moveRobot(0, 0, 0); //Go to the moveStop function
        navigate(); //Go to navigate function
      }
    }

    //procedures can accept values. They must be declared properly.
    void moveRobot(int leftServoSpeed, int rightServoSpeed, int dlay) {
      //the sevos are opposite on bot thus forwardon 1 = reverse on other
      //~ rightServo.write(180);                      //reverses right servo
      //~ leftServo.write(0);                         //reverses left servo
      int leftSpeed, rightSpeed;
      if (leftServoSpeed == 0) {
        leftSpeed = LEFTSTOP; //stop
      } else if (leftServoSpeed < 0) {
        leftSpeed = map(leftServoSpeed, -100, -1, LEFTREV, LEFTSTOP - 1); //reverse
      } else {
        leftSpeed = map(leftServoSpeed, 1, 100, LEFTSTOP + 1, LEFTFWD); //forward
      }
      if (rightServoSpeed == 0) {
        rightSpeed = RIGHTSTOP; //stop
      } else if (rightServoSpeed < 0) {
        rightSpeed = map(rightServoSpeed, -100, -1, RIGHTREV, RIGHTSTOP -1); //rev
      } else {
        rightSpeed = map(rightServoSpeed, 1, 100, RIGHTSTOP +1, RIGHTFWD); //forward
      }
      rightServo.write(rightSpeed);
      leftServo.write(leftSpeed);
      delay(dlay);
    }

    //functions can accept values as well as return values. The unsigned int means
    //it will return an unsigned int to the calling function. void means it returns
    //nothing.
    unsigned int scan() { //This function determines the distance things are away from the ultrasonic sensor
      unsigned int distance; //moved here. Does not need to be a global variable.
      Serial.println("");
      Serial.println("Scanning");
      //~ Time = sonar.ping();
      //~ distance = Time / US_ROUNDTRIP_CM;
      //newping lib will return cm, in, mSec
      //http://playground.arduino.cc/Code/NewPing#Methods
      distance = sonar.ping_cm();
      delay(500); //why the delay?
      return distance;
    }
    void navigate() {
      //this is the number we get from a ping reading looking to the left,
      //check servo is really looking left
      unsigned int LeftDistance;
      // same as above but looking right
      unsigned int RightDistance;
        Serial.println("There's an obstacle!");
        servo.write(SWEEPLEFT); //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 = scan(); //Set the variable LeftDistance to the distance on the left
        Serial.println("Left distance = ");
        Serial.print(LeftDistance);
        servo.write(SWEEPRIGHT);                           //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 = scan();                         //Set the variable RightDistance to the distance on the right
        Serial.println("Right distance = ");
        Serial.print(RightDistance);
        if(abs(RightDistance - LeftDistance) < DEADBAND) { //DEADBAND can be changed above
          //~ 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
          moveRobot(-100, -100, 200); //reverse for 200 mS
          moveRobot(100, -100, 200); //rotate right for 200 mS
        } 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
          moveRobot(-100, 100, 500); //rotate left for 500 mS
        } else { // you have already checked for the other two possibilities
        //~ 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
          moveRobot(100, -100, 500); //rotate right for 500 mS
        }
    }