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

h> #include <CMUcom4.h> CMUcam4_tracking_data_t data; #define #define #define #define #define #define RED_MIN 75 RED_MAX 255 GREEN_MIN 20 GREEN_MAX 60 BLUE_MIN 20 BLUE_MAX 60

// The above tracking values currently track red. #define LED_BLINK 5 // 5 Hz #define WAIT_TIME 5000 // 5 seconds #define CONFIDENCE_THRESHOLD 1 // The percent of tracked pixels in the bounding box needs to be greater than this 0=0% - 255=100%. #define NOISE_FILTER_LEVEL 2 // Filter out runs of tracked pixels smaller than t his in length 0 - 255. Servo PingServo; AF_DCMotor motor1(1); at 1khz AF_DCMotor motor2(2); AF_DCMotor motor3(3); AF_DCMotor motor4(4); // Motor 1 is connected to the port 1 on the motor shield // Motor 2 is connected to the port 2 on the motor shield // Motor 3 is connected to the port 3 on the motor shield // Motor 4 is connected to the port 4 on the motor shield

int led = 50; int minSafeDist = 20; // Minimum distance for ping sensor to know when to turn (30in) int pingPin = A0; // Ping sensor is connected to pin 19 int centerDist, leftDist, rightDist, backDist, a, b; // Define variables center , left, right and back distance long duration, inches; // Define variables for Ping sensor CMUcam4 cam(CMUCOM4_SERIAL1); void setup() { PingServo.attach(10); // Servo is attached to pin 9 in the motor shield PingServo.write(90); // Center the Ping sensor (puts it at 90 degrees) Serial.begin(9600); pinMode(led, OUTPUT); cam.begin(); cam.LEDOn(LED_BLINK); delay(WAIT_TIME); // Turn auto gain and auto white balance off. cam.autoGainControl(false); cam.autoWhiteBalance(false); cam.LEDOn(CMUCAM4_LED_ON); cam.noiseFilter(NOISE_FILTER_LEVEL); cam.trackColor(RED_MIN, RED_MAX, GREEN_MIN, GREEN_MAX, BLUE_MIN, BLUE_MAX);

} void loop() { CMUcam4_tracking_data_t data; cam.getTypeTDataPacket(&data); // Get a tracking packet. a=data.mx; b=data.confidence; Serial.print("Confidence = "); Serial.print(data.confidence); Serial.print('\t'); Serial.print("PostionX = "); Serial.println(data.mx); Serial.print('\t'); Serial.print("Pixels = "); Serial.println(data.pixels); if(b >= CONFIDENCE_THRESHOLD) // We see the color to track. { chase(); digitalWrite(led, HIGH); } else { digitalWrite(led, LOW); CMUcam4_tracking_data_t data; cam.getTypeTDataPacket(&data); // Get a tracking packet. search(); } } void AllStop() { motor1.run(RELEASE); motor2.run(RELEASE); motor3.run(RELEASE); motor4.run(RELEASE); } void kill(){ motor1.setSpeed(240); 255. motor2.setSpeed(240); 255. motor3.setSpeed(240); 255. motor4.setSpeed(240); 255. motor1.run(FORWARD); motor4.run(FORWARD); motor2.run(FORWARD); motor3.run(FORWARD); } // // // // Turns Turns Turns Turns off off off off motor motor motor motor 1 2 3 4

// Sets the max speed of the first motor to 190 out of // Sets the max speed of the second motor to 190 out of // Sets the max speed of the third motor to 190 out of // Sets the max speed of the fourth motor to 190 out of // // // // Motor Motor Motor Motor 1 4 2 2 goes goes goes goes forward forward forward forward (left) (left) (right) (right)

void AllForward() { // Makes the motor1.setSpeed(200); // Sets 255. motor2.setSpeed(200); // Sets 255. motor3.setSpeed(200); // Sets

robot go forward the max speed of the first motor to 190 out of the max speed of the second motor to 190 out of the max speed of the third motor to 190 out of

255. motor4.setSpeed(200); 255. motor1.run(FORWARD); motor4.run(FORWARD); motor2.run(FORWARD); motor3.run(FORWARD); }

// Sets the max speed of the fourth motor to 190 out of // // // // Motor Motor Motor Motor 1 4 2 2 goes goes goes goes forward forward forward forward (left) (left) (right) (right)

void steerRight() { // Makes the robot go right motor1.setSpeed(210); // Sets the max speed of the first motor to 190 out of 255. motor2.setSpeed(210); // Sets the max speed of the second motor to 190 out of 255. motor3.setSpeed(210); // Sets the max speed of the third motor to 190 out of 255. motor4.setSpeed(210); // Sets the max speed of the fourth motor to 190 out of 255. 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(25); // Estimate time required to turn right (1.5 seconds) } void steerLeft() { // Makes the robot go Left motor1.setSpeed(210); // Sets the max speed of the first motor to 190 out of 255. motor2.setSpeed(210); // Sets the max speed of the second motor to 190 out of 255. motor3.setSpeed(210); // Sets the max speed of the third motor to 190 out of 255. motor4.setSpeed(210); // Sets the max speed of the fourth motor to 190 out of 255. 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(25); // Estimate time required to turn left (1.5 seconds) } void turnRight() { // Makes the robot go right motor1.setSpeed(210); // Sets the max speed of the 255. motor2.setSpeed(210); // Sets the max speed of the 255. motor3.setSpeed(210); // Sets the max speed of the 255. motor4.setSpeed(210); // Sets the max speed of the 255. 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(500); // Estimate time required to turn right } first motor to 190 out of second motor to 190 out of third motor to 190 out of fourth motor to 190 out of

(1.5 seconds)

void turnLeft() { // Makes the robot go Left motor1.setSpeed(210); // Sets the max speed of the first motor to 190 out of 255.

motor2.setSpeed(210); // Sets the max speed of the second motor to 190 out of 255. motor3.setSpeed(210); // Sets the max speed of the third motor to 190 out of 255. motor4.setSpeed(210); // Sets the max speed of the fourth motor to 190 out of 255. 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(500); // Estimate time required to turn left (1.5 seconds) } void LookAround(){ PingServo.write(10); // 135 angle delay(500); // wait 0.32 seconds ping(); rightDist = inches; //get the right distance PingServo.write(170); // look to the other side at 45 angle delay(1000); // wait 0.62 seconds ping(); leftDist = inches; // get the left distance PingServo.write(90); // 90 angle delay(500); // wait 0.32 seconds } void LookAhead() { PingServo.write(90);// angle to look forward delay(0100); // wait 0.175 seconds ping(); } void chase() { if(a <= 55) { steerLeft(); } else if(a >=105) { steerRight(); } else { kill(); } } void search() { LookAhead(); Serial.print(inches); Serial.println(" inches"); // Prints a line in the serial monitor and tells th e distance from an object if(inches >= minSafeDist) // If the inches in front of an object is greater t han or equal to the minimum safe distance (20 inches) { CMUcam4_tracking_data_t data; cam.getTypeTDataPacket(&data); // Get a tracking packet. AllForward(); // All wheels forward

} else // If not: { AllStop(); // Stop all motors CMUcam4_tracking_data_t data; cam.getTypeTDataPacket(&data); // Get a tracking packet. LookAround(); // Check your surroundings for best route if(rightDist > leftDist) // If the right distance is greater than the left d istance , turn right { CMUcam4_tracking_data_t data; cam.getTypeTDataPacket(&data); // Get a tracking packet. turnRight(); } else // If the left distance is greater than the right distance , turn left { CMUcam4_tracking_data_t data; cam.getTypeTDataPacket(&data); // Get a tracking packet. turnLeft(); } } } 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; }