Adjusting Circle

// Robotics with the BOE Shield - RoamingWithWhiskers
// Go forward.  Back up and turn if whiskers indicate BOE Shield bot bumped
// into something.

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

Servo servoLeft;                             // Declare left and right servos
Servo servoRight;

int leftSpeed = 1700;
int rightSpeed = 1455;

void setup()                                 // Built-in initialization block
{
  pinMode(7, INPUT);                         // Set right whisker pin to input
  pinMode(5, INPUT);                         // Set left whisker pin to input
  pinMode(8, OUTPUT);                        // Left LED indicator -> output
  pinMode(2, OUTPUT);                        // Right LED indicator -> output

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

  servoLeft.attach(12);                      // Attach left signal to pin 13
  servoRight.attach(11);                     // Attach right signal to pin 12
}

void loop()                                  // Main loop auto-repeats
{
  byte wLeft = digitalRead(5);               // Copy left result to wLeft
  byte wRight = digitalRead(7);              // Copy right result to wRight
  if(wLeft == 0)                             // If left whisker contact
  {
    digitalWrite(8, HIGH);                   // Left LED on
  }
  else                                       // if no left whisker contact
  {
    digitalWrite(8, LOW);                    // Left LED off
  }

  if(wRight == 0)                            // If right whisker contact
  {
    digitalWrite(2, HIGH);                   // Right LED on
  }
  else                                       // If no right whisker contact
  {
    digitalWrite(2, LOW);                    // Right LED off
  }
  if((wLeft == 0) && (wRight == 0))          // If both whiskers contact
  {
    servoLeft.writeMicroseconds(1500);
    servoRight.writeMicroseconds(1500);
    song();
  }
  else if(wLeft == 0)                        // If only left whisker contact
  {
    rightSpeed-=5;
    delay(200);
  }
  else if(wRight == 0)                       // If only right whisker contact
  {
    rightSpeed+=5;
    delay(200);
  }
  else                                       // Otherwise, no whisker contact
  {
    servoLeft.writeMicroseconds(leftSpeed);
    servoRight.writeMicroseconds(rightSpeed);
    delay(200);
  }
}

void song(){
  //measure 1
  tone(4, 739.99, 500);
  delay(500);
  noTone(4);
  delay(500);
  tone(4, 739.99, 250);
  delay(250);
  tone(4, 783.99, 250);
  delay(250);
  tone(4, 739.99, 250);
  delay(250);
  tone(4, 739.99, 250);
  delay(250);
  //measure 2
  tone(4, 659.25, 500);
  delay(500);
  noTone(4);
  delay(500);
  delay(500);
  delay(250);
  tone(4, 739.99, 750); //measure 3 starts in here
  delay(750);
  delay(250);
  tone(4, 783.99, 750);
  delay(750);
  tone(4, 987.77, 250);
  delay(250);
  tone(4, 783.99, 250);
  delay(250);
  tone(4, 739.99, 250);
  delay(250);
  //measure 4
  tone(4, 659.25, 1000);
  delay(1000);
  tone(4, 987.77, 750);
  delay(750);
  tone(4, 739.99, 750);//measure 5 starts in here
  delay(750);
  noTone(4);
  delay(250);
  tone(4, 739.99, 500);
  delay(500);
  tone(4, 783.99, 250);
  delay(250);
  tone(4, 739.99, 250);
  delay(250);
  tone(4, 783.99, 250);
  delay(250);
  //measures 6 and 7
  tone(4, 987.77, 2000);
  delay(2000);
}