#include #include #include #include

<CMUcam4.h> <CMUcom4.h> <Servo.h> <AFMotor.h> // 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

Servo PingServo; AF_DCMotor motor1(1); at 1khz AF_DCMotor motor2(2); AF_DCMotor motor3(3); AF_DCMotor motor4(4);

int minSafeDist = 30; // 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; // Define variables center, left , right and back distance long duration, inches, cm; // Define variables for Ping sensor #define YUV_MODE false // Change this to true to switch to YUV mode. // Feed the color to track into these six variables. tracking red #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 bright lights. #define LED_BLINK 5 // 5 Hz #define WAIT_TIME 5000 // 5 seconds #define PIXELS_THRESHOLD 1 // The percent of tracked pixels needs to be greater than this 0=0% - 255=100%. #define CONFIDENCE_THRESHOLD 50 // 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. const int ledPin = 13; CMUcam4 cam(CMUCOM4_SERIAL3); void setup() { PingServo.attach(9); PingServo.write(90); motor1.setSpeed(155); 255. motor2.setSpeed(155); 255. motor3.setSpeed(155); 255. motor4.setSpeed(155); 255.

// Servo is attached to pin 9 in the motor shield // Center the Ping sensor (puts it at 90 degrees) // Sets the max speed of the first motor to 155 out of // Sets the max speed of the second motor to 155 out of // Sets the max speed of the third motor to 155 out of // Sets the max speed of the fourth motor to 155 out of

Serial.begin(19200); pinMode(ledPin, OUTPUT);

cam.begin(); // Wait for auto gain and auto white balance to run. 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.colorTracking(YUV_MODE); cam.noiseFilter(NOISE_FILTER_LEVEL); } //////////////////////////////////////////////////////////////////////////////// /////////////////////////// void loop(){ CMUcam4_tracking_data_t data; // Start streaming... cam.trackColor(RED_MIN, RED_MAX, GREEN_MIN, GREEN_MAX, BLUE_MIN, BLUE_MAX); for(;;) { cam.getTypeTDataPacket(&data); // Get a tracking packet at 30 FPS or every 3 3.33 ms. // Process the packet data safely here. if(data.pixels > PIXELS_THRESHOLD) // We see the color to track. { if(data.confidence > CONFIDENCE_THRESHOLD) // The pixels we are tracking a re in a dense clump - so maybe they are a single object. { //centroid position if(data.mx >= 0 && data.mx <= 40) { veerLeft(); } else if(data.mx >=140 && data.mx <=160) { veerRight(); } else { AllForward(); } // Okay, so we now know we are looking at a clump of pixels that are in the color bounds we wanted to track. digitalWrite(ledPin, HIGH); } else { // Turn the LED's off. AllStop(); digitalWrite(ledPin, LOW);

} } else { digitalWrite(ledPin, LOW); } } ping(); //check positioning if(inches >= minSafeDist) // If the inches in front of an object is greater than or equal to the minimum safe distance (30 inches) { AllForward(); // All wheels forward delay(200); // 0.2 sec delay AllStop(); }else { //otherwise AllStop(); // Stop all motors LookAround(); // Check your surroundings for best route if(rightDist > leftDist) // If the right distance is greater than the left distance , turn right { turnRight(); } else // If the left distance is greater than the right distance , turn lef t { turnLeft(); } } } /////////////////////////////////////////////////////////////////////////// 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 motor1.run(FORWARD); // Motor 1 motor4.run(FORWARD); // Motor 4 motor2.run(FORWARD); // Motor 2 motor3.run(FORWARD); // Motor 2 } robot go forward goes forward (left) goes forward (left) goes forward (right) goes forward (right) go right 2 backward 3 backward forward forward to turn right (1.5 seconds)

void turnRight() { // Makes the robot motor2.run(BACKWARD); // Turns motor motor3.run(BACKWARD); // Turns motor motor1.run(FORWARD); // Motor 1 goes motor4.run(FORWARD); // Motor 4 goes delay(200); // Estimate time required } void turnLeft() { // motor2.run(FORWARD); motor3.run(FORWARD); motor1.run(BACKWARD); motor4.run(BACKWARD);

Makes the robot go Left // Motor 2 goes forward // Motor 3 goes forward // Motor 1 goes backward // Motor 4 goes backward

delay(200); // Estimate time required to turn left (1.5 seconds) } void veerLeft() { // Makes the robot go Left motor2.run(FORWARD); // Motor 2 goes forward motor3.run(FORWARD); // Motor 3 goes forward motor1.run(BACKWARD); // Motor 1 goes backward motor4.run(BACKWARD); // Motor 4 goes backward delay(50); // Estimate time required to turn left (1.5 seconds) } void veerRight() { // Makes the robot go right motor2.run(BACKWARD); // Turns motor 2 backward motor3.run(BACKWARD); // Turns motor 3 backward motor1.run(FORWARD); // Motor 1 goes forward motor4.run(FORWARD); // Motor 4 goes forward delay(50); // Estimate time required to turn right (1.5 seconds) } void LookAround(){ PingServo.write(135); // 135 angle delay(320); // wait 0.32 seconds ping(); rightDist = inches; //get the right distance PingServo.write(45); // look to the other side at 45 angle delay(620); // wait 0.62 seconds ping(); leftDist = inches; // get the left distance PingServo.write(90); // 90 angle delay(320); // wait 0.32 seconds } 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); cm = microsecondsToCentimeters(duration); } long microsecondsToInches(long microseconds) // converts time to a distance { return microseconds / 74 / 2; } long microsecondsToCentimeters(long microseconds) // converts time to a distance { return microseconds / 29 / 2; }