Robot Rebels Firstbit Code

 // This is some example code to get your first robot, wandering around your house and not hopefully not damaging Mothers furniture
    // Lots more robots, building and making over at www.robotrebels.org
    // I welcome all ideas to improve ossipee
    // I have used the pins that corelate for my motor driver board, yours will be differant so adjust your pin numbers as needed
    // The blink/led 13 is commented out but it is there to orientate you a bit


    // add some libraries for ultra sonic sensor and sweep servo
    #include <Servo.h>
    #include <NewPing.h>

    // Lets label some arduino pins as inputs and outputs you will have to set up for the pins your robot uses/needs

    #define USTrigger 9       // picks ping trigger pin
    #define USEcho 10         // 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
    #define lMotor1 3         // left motor direction control #1 is on pin 3
    #define lMotor2 4         // left motor direction control #2 is on pin 4
    #define LPWM 5            // left motor pulsed width modulation, speed control
    #define RPWM 6            // the next 3 are the same but for the right motor
    #define rMotor1 7
    #define rMotor2 8

    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;
    byte lSpeed1=255;                     // use this number to adjust your left motor speed out of the box the 2 motor might not be the same speed
    byte rSpeed=255;                      // 255 is full speed

    void setup()                          // This part of the program runs 1 time at start up
    {
      pinMode(lMotor1,OUTPUT);            // sets our motor pins as outputs and low
      pinMode(lMotor2,OUTPUT);
      pinMode(LPWM, OUTPUT);
      pinMode(RPWM,OUTPUT);
      pinMode(rMotor1,OUTPUT);
      pinMode(rMotor2, OUTPUT);

      servo.attach (11);                 // puts ping servo on pin 11
    }


    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 motorSpeed(int thisLeft,int thisRight){             // sets our motors speed
      analogWrite(LPWM,thisLeft);
      analogWrite(RPWM,thisRight);
    }

    void moveBackward(){                                     // this will tell both motors to backup your robot, you will need to adjust this
      digitalWrite(lMotor1,LOW);
      digitalWrite(lMotor2,HIGH);
      digitalWrite(rMotor1,LOW);
      digitalWrite(rMotor2,HIGH);
    }

    void moveForward(){                                      // this will tell both motor to go forwards
      digitalWrite(lMotor1,HIGH);
      digitalWrite(lMotor2,LOW);
      digitalWrite(rMotor1,HIGH);
      digitalWrite(rMotor2,LOW);

    }

    void moveRight(){                                       // turns robot right, might be worth trying just stopping a motor as well
      digitalWrite(lMotor1,HIGH);
      digitalWrite(lMotor2,LOW);
      digitalWrite(rMotor1,LOW);
      digitalWrite(rMotor2,HIGH);
    }

    void moveLeft(){                                        // turns robot left just opposite motor directions adjust as needed
      digitalWrite(lMotor1,LOW);
      digitalWrite(lMotor2,HIGH);
      digitalWrite(rMotor1,HIGH);
      digitalWrite(rMotor2,LOW);
    }

    void moveStop(){                                        // all pins low stops both motors
      digitalWrite(lMotor1,LOW);
      digitalWrite(lMotor2,LOW);
      digitalWrite(rMotor1,LOW);
      digitalWrite(rMotor2,LOW);
    }

    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 servo 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 you can adjust the times to get the amount of reverse you need
          delay(200);                                      // Pause the program for 200 milliseconds to let the robot reverse
          moveRight();                                     // Go to the moveRight function you can adjust the time to get a 90 degree turn
          delay(100);                                      // Pause the program for 100 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
        }
    }