#include <AFMotor.h> // Enables the Motor library #include <Servo.

h> // Enables the Servo library Servo PingServo; AF_DCMotor motor1(1); // Motor 1 is connected to the port 1 on the AF_DCMotor motor2(2); // Motor 2 is connected to the port 2 on the AF_DCMotor motor3(3); // Motor 3 is connected to the port 3 on the AF_DCMotor motor4(4); // Motor 4 is connected to the port 4 on the int minSafeDist = 20 ; // Minimum distance for ping sensor to know int pingPin = A0; // Ping sensor is connected to port A0 int centerDist, leftDist, rightDist, backDist; // Define variables right and back distance long duration, inches; // Define variables for Ping sensor

motor shield motor shield motor shield motor shield when to turn center, left,

void setup() { PingServo.attach(10); // Servo is attached to pin 10 in the motor shield PingServo.write(90); // Center the Ping sensor (puts it at 90 degrees) motor1.setSpeed(215); // Sets the speed of the first motor (At 0, the motors are turned off. 255 is the fastest setting that you are able to use, I used 215 to not push the motors too hard.) motor2.setSpeed(215); // Sets the speed of the second motor (At 0, the motors ar e turned off. 255 is the fastest setting that you are able to use, I used 215 to not push the motors too hard.) motor3.setSpeed(215); // Sets the speed of the third motor (At 0, the motors are turned off. 255 is the fastest setting that you are able to use, I used 215 to not push the motors too hard.) motor4.setSpeed(215); // Sets the speed of the fourth motor (At 0, the motors ar e turned off. 255 is the fastest setting that you are able to use, I used 215 to not push the motors too hard.) Serial.begin(9600); // Enables Serial monitor for debugging purposes } void AllStop() { motor1.run(RELEASE); // Turns off motor 1 motor2.run(RELEASE); // Turns off motor 2 motor3.run(RELEASE); // Turns off motor 3 motor4.run(RELEASE); // Turns off motor 4 } void AllForward() { // Makes the robot go forward motor1.run(FORWARD); // Motor 1 goes forward motor2.run(FORWARD); // Motor 2 goes forward motor3.run(FORWARD); // Motor 3 goes forward motor4.run(FORWARD); // Motor 4 goes forward } void turnRight() { // Makes the robot go right motor2.run(FORWARD); // Turns off motor 2 motor3.run(BACKWARD); // Turns off motor 3 motor1.run(FORWARD); // Motor 1 goes forward motor4.run(BACKWARD); // Motor 4 goes forward delay(2000); // Time required to turn right (1.6 seconds) } /* void GoBack(){ // Makes the robot go back motor2.run(BACKWARD); // Motor 2 goes back motor1.run(BACKWARD); // Motor 1 goes back motor4.run(BACKWARD); // Motor 4 goes back

motor3.run(BACKWARD); // Motor 3 goes back delay(1600); // Time Required to go back (1.6 seconds) } */ void turnLeft() { // Makes the robot go Left motor2.run(BACKWARD); // Motor 2 goes forward motor3.run(FORWARD); // Motor 3 goes forward motor1.run(BACKWARD); // turns off motor 1 motor4.run(FORWARD); // turns off motor 1 delay(2000); //Time Required to turn left (1.6)Seconds } // Starts the loop to decide what to do void loop() { LookAhead(); Serial.print(inches); Serial.println(" inches"); // Prints a line in the serial monitor if(inches >= minSafeDist) /* If the inches in front of an object is greater than or equal to the minimum safe distance (11 inches), react*/ { AllForward(); // All wheels forward delay(110); // Wait 0.11 seconds }else // If not: { AllStop(); // Stop all motors LookAround(); // Check your surroundings for best route if(rightDist > leftDist) // If the right distance is greater than the left dista nce , turn right { turnRight(); }else if (leftDist > rightDist) // If the left distance is greater than the righ t distance , turn left { turnLeft(); }/* else if (leftDist&&rightDist<minSafeDist) // If the left and right distance is s maller than the min safe distance (11 inch) go back { GoBack(); }*/ } } unsigned long ping() { pinMode(pingPin, OUTPUT); // Make the Pingpin to output digitalWrite(pingPin, LOW); //Send a low pulse delayMicroseconds(2); // wait for two microseconds digitalWrite(pingPin, HIGH); // Send a high pulse delayMicroseconds(5); // wait for 5 micro seconds digitalWrite(pingPin, LOW); // send a low pulse pinMode(pingPin,INPUT); // switch the Pingpin to input duration = pulseIn(pingPin, HIGH); //listen for echo /*Convert micro seconds to Inches -------------------------------------*/ inches = microsecondsToInches(duration); }

long microsecondsToInches(long microseconds) // converts time to a distance { return microseconds / 74 / 2; } void LookAhead() { PingServo.write(90);// angle to look forward delay(175); // wait 0.175 seconds ping(); } void LookAround(){ PingServo.write(180); // 180 angle delay(320); // wait 0.32 seconds ping(); rightDist = inches; //get the right distance PingServo.write(0); // look to the other side delay(620); // wait 0.62 seconds ping(); leftDist = inches; // get the left distance PingServo.write(90); // 90 angle delay(275); // wait 0.275 seconds }