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; }