Final code for the Arduino maze robot in Year 1 of CS course.

maze-robot-y1.ino

// by Victor Azzam and Gerard O'Brien
//    B00099167        B00099300

#include <Servo.h>                           // Include servo library

Servo servoLeft;                             // Declare servos
Servo servoRight;

void setup()
{ 
  pinMode(7, INPUT);                         // Set whiskers as inputs
  pinMode(5, INPUT);

  pinMode(8, OUTPUT);                        // Set LEDs as outputs
  pinMode(2, OUTPUT);

  tone(4, 3000, 1000);                       // Play tone for 1 second
  delay(1000);                               // Delay to finish tone

  servoLeft.attach(13);                      // Left/right servos to pins 13/12 respectively
  servoRight.attach(12);
}

/* PSEUDO CODE AND EXPLANATION
 * ---------------------------
 *
 * LOOP:
 *     IF either whisker contacts:
 *         Turn on LEDs
 *         Move backwards slightly while beeping
 *         Rotate 90 degrees left
 *         FOR a period of 1.5 seconds, EVERY 0.1 seconds:
 *             IF either whisker contacts:
 *                 Turn on LEDs
 *                 Move backwards slightly while beeping
 *                 Rotate 180 degrees left (to prevent going backwards)
 *                 BREAK the FOR loop
 *     OTHERWISE:
 *         Turn off LEDs
 *         Move forward 0.02 seconds
 *
 *
 * The idea is that, assuming the robot runs into a wall twice immediately, the
 * second one will be no longer than 1.5 seconds away from the original turning
 * position, and within this time touching a wall would trigger a full rotation
 * in the opposite direction, preventing the robot from going back to the start
 * position of the maze. Then it leaves the 1.5 second loop, preventing further
 * unnecessary turns.
 *
 * If only one wall is met, a 90 degree turn will occur, which is sufficient to
 * allow the robot to escape it without turning too far.
 *
 */

void loop()
{
  if((digitalRead(5) == 0) || (digitalRead(7) == 0))
  {                                          // If either whisker contacts
    digitalWrite(8, HIGH);                   // Turn LEDs on
    digitalWrite(2, HIGH);
    backward(100);                           // Back up slightly
    turn(525);                               // Turn left about 90 degrees
    for(int i = 0; i < 15; i++)
    {
      delay(100);
      if((digitalRead(5) == 0) || (digitalRead(7) == 0))
      {
        digitalWrite(8, HIGH);
        digitalWrite(2, HIGH);
        backward(100);
        turn(1050);
        break;
      }
    }
  }
  else                                       // Otherwise, no whisker contact
  {
    digitalWrite(8, LOW);                    // Turn LEDs off
    digitalWrite(2, LOW);
    forward(20);                             // Forward 0.02 seconds
  }
}

void forward(int time)
{
  servoLeft.writeMicroseconds(1700);         // Left wheel counter-clockwise
  servoRight.writeMicroseconds(1300);        // Right wheel clockwise
  delay(time);                               // Time in ms
}

void turn(int time)
{
  servoLeft.writeMicroseconds(1300);         // Wheels turn clockwise
  servoRight.writeMicroseconds(1300);
  delay(time);                               // Time in ms
}

void backward(int time)
{
  servoLeft.writeMicroseconds(1400);         // Left wheel clockwise
  servoRight.writeMicroseconds(1600);        // Right wheel counter-clockwise
  back_noise();                              // Play an intermittent tone
  delay(time);                               // Time in ms
}

void back_noise()                            // Function to play a sound periodically
{
  for (int i = 0; i < 3; i++)                // Values 0, 1, 2
  {
    tone(4, 3000, 200);                      // Play tone for 0.2 seconds
    delay(300);                              // Time in ms
  }
}