Autonomous Sketch

Arduino programs are called sketches. Here is the Arduino sketch that controls the robot autonomously.

/* B4 Robot Program v0.4
   01/01/2013 S Rayner
 */

#include <Servo.h>
Servo servo; // Define our Servo

// Input/Output Pin configuration.
int ledPin     = 13;
int rightL3    = 12;
int rightL4    = 11;
int rightSpeed = 10; // pwm
int leftL1     =  8;
int leftL2     =  7;
int leftSpeed  =  6; // pwm
int pingPin    =  4;
int servoPin   =  3; // pwm
int echoPin    =  2;

// Speed.
int spd = 255;

// 90 Degree turn time.
int tt = 1500;

// Safe distance (cm)
long safeDistance = 14.0; // cm

void setup()
{
  // Configure LED pin.
  pinMode(ledPin, OUTPUT);

  // Configure motor pins.
  pinMode(leftL1, OUTPUT);
  pinMode(leftL2, OUTPUT);
  pinMode(leftSpeed, OUTPUT);
  pinMode(rightL3, OUTPUT);
  pinMode(rightL4, OUTPUT);
  pinMode(rightSpeed, OUTPUT);
  halt(); // Ensure stopped

  // Configure ultrasonic distance pins.
  pinMode(pingPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Configure the servo pin.
  servo.attach(servoPin);

  // Initialise serial
  Serial.begin(9600);

  // 5 Second initial pause.  
  delay(5000);
}

void loop()
{
  boolean ok = true;

  // Loop until front distance is less than safe distance.
  while (ok)
  {
    // Drive forward at 3/4 speed.
    forward(spd, 0);

    // Check distances.
    if (lookForward() < safeDistance)
    {
      ok = false;
    }
    else if (lookLeft(45) < (safeDistance * 1.5))
    {
      turnRight(spd, tt/2);
    }
    else if (lookForward() < safeDistance)
    {
      ok = false;
    }
    else if (lookRight(45) < (safeDistance * 1.5))
    {
      turnLeft(spd, tt/2);
    }
  }

  // Stop both motors.
  halt();

  // Full look around.
  long leftDist = lookLeft(90);
  long rightDist = lookRight(90);
  lookForward();

  if(leftDist > rightDist)
  {
    // Reverse, then turn left. 
    reverse(spd, 1000);
    turnLeft(spd, tt);
  }
  else
  {
    // Reverse, then turn right.
    reverse(spd, 1000);
    turnRight(255, tt);
  }
}

void forward(int velocity, int time)
{
  // Left motor forward at specified speed.
  digitalWrite(leftL1, LOW);
  digitalWrite(leftL2, HIGH);
  analogWrite(leftSpeed, velocity);

  // Right motor forward at specified speed.
  digitalWrite(rightL3, LOW);
  digitalWrite(rightL4, HIGH);
  analogWrite(rightSpeed, velocity);

  delay(500);
}

void reverse(int velocity, int time)
{
  // Left motor reverse at specified speed.
  digitalWrite(leftL1, HIGH);
  digitalWrite(leftL2, LOW);
  analogWrite(leftSpeed, velocity);

  // Right motor reverse at specified speed.
  digitalWrite(rightL3, HIGH);
  digitalWrite(rightL4, LOW);
  analogWrite(rightSpeed, velocity);

  // Delay for specified time.
  delay(time);

  // Stop both motors.
  halt;  
}

void turnLeft(int velocity, int time)
{
  Serial.println("Turning Left...");

  // Left motor reverse at specified speed.
  digitalWrite(leftL1, HIGH);
  digitalWrite(leftL2, LOW);
  analogWrite(leftSpeed, velocity);

  // Right motor forward at specified speed.
  digitalWrite(rightL3, LOW);
  digitalWrite(rightL4, HIGH);
  analogWrite(rightSpeed, velocity);

  // Delay for specified time.
  delay(time);

  // Stop both motors.
  halt;
}

void turnRight(int velocity, int time)
{
  Serial.println("Turning right...");

  // Left motor forward at specified speed.
  digitalWrite(leftL1, LOW);
  digitalWrite(leftL2, HIGH);
  analogWrite(leftSpeed, velocity);

  // Right motor reverse at specified speed.
  digitalWrite(rightL3, HIGH);
  digitalWrite(rightL4, LOW);
  analogWrite(rightSpeed, velocity);

  // Delay for specified time.
  delay(time);

  // Stop both motors.
  halt;
}

void halt()
{
  // Stop both motors.
  digitalWrite(leftL1, LOW);
  digitalWrite(leftL2, LOW);
  digitalWrite(rightL3, LOW);
  digitalWrite(rightL4, LOW);
  analogWrite(leftSpeed, 255);  // Breaking
  analogWrite(rightSpeed, 255);    // on both wheels.

  // pint reading to display
  Serial.println("Stopped motors");
}

long lookLeft(int angle)
{
  int curAngle = servo.read();
  Serial.print("Current angle: ");
  Serial.print(curAngle);
  Serial.println();

  int newAngle = (90 + angle);
  Serial.print("New angle: ");
  Serial.print(newAngle);
  Serial.println();

  int degDiff = abs(curAngle - newAngle);
  Serial.print("Degrees to move: ");
  Serial.print(degDiff);
  Serial.println();

  servo.write(newAngle);  // Turn servo left
  delay(1000 / 90 * degDiff); // Delay to ensure servo has time to turn
  return getDistance();
}

long lookRight(int angle)
{
  int curAngle = servo.read();
  Serial.print("Current angle: ");
  Serial.print(curAngle);
  Serial.println();

  int newAngle = (90 - angle);
  Serial.print("New angle: ");
  Serial.print(newAngle);
  Serial.println();

  int degDiff = abs(curAngle - newAngle);
  Serial.print("Degrees to move: ");
  Serial.print(degDiff);
  Serial.println();

  servo.write(newAngle);  // Turn Servo right
  delay(1000 / 90 * degDiff); // Delay to ensure servo has time to turn
  return getDistance();
}

long lookForward()
{
  servo.write(90); // Turn Servo to Center 90 degrees
  delay(1000);    // Wait 1/2 second
  return getDistance();
}

long getDistance()
{
  long duration;

  // Sending the signal, starting LOW for a clean signal
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);

  // Get echo and calculate distance
  duration = pulseIn(echoPin, HIGH);
  return microsecondsToCentimeters(duration);
}

long microsecondsToCentimeters(long microseconds)
{
  // speed of sound = 340 m/s or 29 microseconds per centimeter.
  return microseconds / 29 / 2;
}