Robot 1

#include <AFMotor.h>
#include <Servo.h>

// Define Servo Wheels
Servo servoLeft;
Servo servoRight;

// Define Ping Sensor Chassis Serco Motors (the two servos used to make the ping sensor look left/right and up/down)
Servo servoUpDown;
Servo servoLeftRight;

// Setup AM3 Accellerometer (used to idnetify the tilt of the robot)
int xAxis = A2;
int axisValue = 0;
int gravity;

// Setup Ping Sensor
int distSensor = 0;
const int pwPin = 8;
long pulse, inches, cm;

// Setup Hall Effect Sensor - i.e Wheel monitor
const int hallPin = 7;

void setup(){
  int i = 0;

  // Intitiate Serial connection for debugging
  Serial.begin(9600);

  // attach servo's to Pins and stop - Note: Motor shield uses Pin 9,10
  servoUpDown.attach(3);
  servoLeftRight.attach(4);
  servoLeft.attach(10);
  servoRight.attach(9);
  servoLeft.write(95); // 1 to 94 Backward, 96 to 200 Forward
  servoRight.write(95); // 1 to 94 Forward, 96 to 200 Backward

  // attach Hall Effewt Sensor
  pinMode(hallPin, INPUT);

  // Set sensor motors to front
  servoLeftRight.write(86);
  servoUpDown.write(1);

  // attach Ping Sensor
  pinMode(pwPin, INPUT);

  // set gravity level
  // average out reads to accommodate misreads and fluctuation
  for (i=0; i<10; i++) {
    axisValue = analogRead(xAxis);
    gravity += axisValue;
    delay(10);
  }
  gravity /= 10;

  // Give user 5 seconds before robot starts
  delay(5000);
}

// *********************************

void loop(){

  // Move forward
  servoLeft.write(200); // 1 to 94 Backward, 96 to 200 Forward
  servoRight.write(83); // 1 to 94 Forward, 96 to 200 Backward

  // Check Distance
  checkDistance();

  // if distance is < 30cm then check the direction to move in
  if (distSensor < 30) {checkDirection();}

  // Check vertical angle
  adjustPingVertical();

}

// *********************************

void checkDirection(){

  int leftDistance = 0;
  int rightDistance = 0;

  stopBot();

  // Check Left
  servoLeftRight.write(0);
  delay(1500);
  checkDistance();
  leftDistance = distSensor;

  // Check Right
  servoLeftRight.write(180);
  delay(2500);
  checkDistance();
  rightDistance = distSensor;

  // set Ping Sensor back to forward facing
  servoLeftRight.write(86);
  delay(2000);

  // Define direction
  if (leftDistance < rightDistance && rightDistance > 30){
    goRight();

  }
  else if (leftDistance > rightDistance && leftDistance > 30){
    goLeft();

  }
  else {

    turnAround();
  }
}

// *********************************

void checkDistance(){

  int i = 0;

  //Following code modified from original code by Author: Bruce Allen - 23/07/09
  //Used to read in the pulse that is being sent by the MaxSonar device.
  //Pulse Width representation with a scale factor of 147 uS per Inch.
  // Average 8 readings to accomodate noise and error

  for (i=0; i<8; i++) {
    pulse = pulseIn(pwPin, HIGH);
    //147uS per inch
    inches = pulse/147;
    //change inches to centimetres
    cm = inches * 2.54;
    distSensor += cm;
    delay(50);
  }

  distSensor /= 8;
}

// *********************************

void adjustPingVertical(){

  int angle = 0;
  int upDownAngle = 0;
  int i = 0;

  // read the angle - 400 is flat
  axisValue = analogRead(xAxis);

  // average out reads to accommodate misreads and fluctuation - divde by 2 to   accommodate angle
  for (i=0; i<8; i++) {
    angle = 400 - axisValue;
    angle /= 2;
    upDownAngle += angle;
    delay(10);
  }
  

  upDownAngle /= 8;
  upDownAngle = 1 + upDownAngle;

  // Don't tilt the sensor greater than 90 degrees or below 0
  if (upDownAngle > 90){upDownAngle = 90;}
  if (upDownAngle < 1){upDownAngle = 1;}

  servoUpDown.write(upDownAngle);
}

// *********************************

void goRight(){

  int i = 0;
  int hallState = 0;
  hallState = digitalRead(hallPin);

  // Set the wheels to turn the robot right
  servoLeft.write(200); // 1 to 94 Backward, 96 to 200 Forward
  servoRight.write(200); // 1 to 94 Forward, 96 to 200 Backward

  // If wheel is currently on a magnet, move off magnet
  while ( hallState == LOW){
   hallState = digitalRead(hallPin);
  } // end while

  // count 2 magnets on the wheel

  while (i != 2){
    hallState = digitalRead(hallPin);

    if (hallState == LOW){ // i.e on a magnet
       i = i + 1;
       while ( hallState == LOW){ // move off magnet
         hallState = digitalRead(hallPin);
       } // end while
    } // end if
  } // end while

} // end goRight

// *********************************

void goLeft(){

  int i = 0;
  int hallState = 0;
  hallState = digitalRead(hallPin);

  // Set the wheels to turn right
  servoLeft.write(1); // 1 to 94 Backward, 96 to 200 Forward
  servoRight.write(1); // 1 to 94 Forward, 96 to 200 Backward

  // move off magnet
  while ( hallState == LOW){
     hallState = digitalRead(hallPin);
  } // end while

  // count 2 magnets on the wheel

  while (i != 2){
    hallState = digitalRead(hallPin);

    if (hallState == LOW){
      i = i + 1;
      while ( hallState == LOW){
        hallState = digitalRead(hallPin);
      } // end while
   } // end if
  } // end while

} // end goLeft

// *********************************

void turnAround(){

  int i = 0;
  int hallState = 0;
  hallState = digitalRead(hallPin);

  // Set the wheels to turn right
  servoLeft.write(1); // 1 to 94 Backward, 96 to 200 Forward
  servoRight.write(1); // 1 to 94 Forward, 96 to 200 Backward

  // move off magnet
   while ( hallState == LOW){
     hallState = digitalRead(hallPin);
   } // end while

  // count 2 magnets on the wheel

  while (i != 4){
    hallState = digitalRead(hallPin);
    if (hallState == LOW){
      i = i + 1;
      while ( hallState == LOW){
         hallState = digitalRead(hallPin);
      } // end while
    } // end if
  } // end while

} // end turnAround

// *********************************

void stopBot(){

  int hallState = 0;
  hallState = digitalRead(hallPin);

  // Stop when at magnet
  while (hallState == HIGH){
    hallState = digitalRead(hallPin);
  }

  // Stop Servos
  servoLeft.write(95); // 1 to 94 Backward, 96 to 200 Forward
  servoRight.write(95); // 1 to 94 Forward, 96 to 200 Backward
}