int micVal; int cdsVal; int irLval; // Left IR int irCval; // Center IR int irRval; // Right IR int i; // Generic Counter int x; // Generic Counter int PLval; // Pulse Width for Left Servo int PRval; // Pulse Width for Right Servo int cntr; // Generic Counter Used for Determining amt. of Object Detections int counter; // Generic Counter int clrpth; // amt. of Milliseconds Of Unobstructed Path int objdet; // Time an Object was Detected int task; // Routine to Follow for Clearest Path int pwm; // Pulse Width for Pan Servo boolean add; // Whether to Increment or Decrement PW Value for Pan Servo int distance; // Distance to Object Detected via Ultrasonic Ranger int oldDistance; // Previous Distance Value Read from Ultrasonic Ranger float scale = 1.9866666666666666666666666666667; // *Not Currently Used* int LeftPin = 6; // Left Servo int RightPin = 9; // Right Servo int PiezoPin = 11; // Piezo int PingServoPin = 5; // Pan Servo int irLPin = 0; // Analog 0; Left IR int irCPin = 1; // Analog 1; Center IR int irRPin = 2; // Analog 2; Right IR int ultraSoundSignal = 7; // Ultrasound signal pin int val = 0; // Used for Ultrasonic Ranger int ultrasoundValue = 0; // Raw Distance Val int oldUltrasoundValue; // *Not used* int pulseCount; // Generic Counter int timecount = 0; // Echo counter int ledPin = 13; // LED connected to digital pin 13 #define BAUD 9600 #define CmConstant 1/29.034 void setup() { Serial.begin(9600); pinMode(PiezoPin, OUTPUT); pinMode(ledPin, OUTPUT); pinMode(LeftPin, OUTPUT); pinMode(RightPin, OUTPUT); pinMode(PingServoPin, OUTPUT); pinMode(irLPin, INPUT); pinMode(irCPin, INPUT); pinMode(irRPin, INPUT); for(i = 0; i < 500; i++) { digitalWrite(PiezoPin, HIGH); delayMicroseconds(1000); digitalWrite(PiezoPin, LOW); delayMicroseconds(1000); } for(i = 0; i < 20; i++) { digitalWrite(PingServoPin, HIGH); delayMicroseconds(655 * 2); digitalWrite(PingServoPin, LOW); delay(20); } ultrasoundValue = 600; i = 0; } void loop() { //Scan(); Look(); Go(); } void Look() { irLval = analogRead(irLPin); irCval = analogRead(irCPin); irRval = analogRead(irRPin); //if(counter > 10) { //counter = 0; //readPing(); //} if(irLval > 200) { PLval = 820; PRval = 850; x = 5; cntr = cntr + 1; clrpth = 0; objdet = millis(); } else if(irCval > 200) { PLval = 820; PRval = 850; x = 10; cntr = cntr + 1; clrpth = 0; objdet = millis(); } else if(irRval > 200) { PLval = 620; PRval = 650; x = 5; cntr = cntr + 1; clrpth = 0; objdet = millis(); } else { x = 1; PLval = 620; PRval = 850; counter = counter + 1; clrpth = (millis() - objdet); if(add == true) { pwm = pwm + 50; } else if(add == false) { pwm = pwm - 50; } if(pwm < 400) { pwm = 400; add = true; } if(pwm > 950) { pwm = 950; add = false; } digitalWrite(PingServoPin, HIGH); delayMicroseconds(pwm * 2); digitalWrite(PingServoPin, LOW); delay(20); readPing(); if(ultrasoundValue < 500) { cntr = cntr + 1; switch(pwm) { case 400: x = 7; PLval = 650; PRval = 650; Go(); break; case 500: x = 10; PLval = 650; PRval = 650; Go(); break; case 600: x = 14; PLval = 850; PRval = 850; Go(); break; case 700: x = 10; PLval = 850; PRval = 850; Go(); break; case 950: x = 7; PLval = 850; PRval = 850; Go(); break; } } } //Serial.print("clrpth: "); //Serial.println(clrpth); //Serial.print("objdet: "); //Serial.println(objdet); //Serial.print("cntr: "); //Serial.println(cntr); if(cntr > 25 && clrpth < 2000) { clrpth = 0; cntr = 0; Scan(); } } void Go() { for(i = 0; i < x; i++) { digitalWrite(LeftPin, HIGH); delayMicroseconds(PLval * 2); digitalWrite(LeftPin, LOW); digitalWrite(RightPin, HIGH); delayMicroseconds(PRval * 2); digitalWrite(RightPin, LOW); delay(20); } } void readPing() { // Get Distance from Ultrasonic Ranger timecount = 0; val = 0; pinMode(ultraSoundSignal, OUTPUT); // Switch signalpin to output /* Send low-high-low pulse to activate the trigger pulse of the sensor * ------------------------------------------------------------------- */ digitalWrite(ultraSoundSignal, LOW); // Send low pulse delayMicroseconds(2); // Wait for 2 microseconds digitalWrite(ultraSoundSignal, HIGH); // Send high pulse delayMicroseconds(5); // Wait for 5 microseconds digitalWrite(ultraSoundSignal, LOW); // Holdoff /* Listening for echo pulse * ------------------------------------------------------------------- */ pinMode(ultraSoundSignal, INPUT); // Switch signalpin to input val = digitalRead(ultraSoundSignal); // Append signal value to val while(val == LOW) { // Loop until pin reads a high value val = digitalRead(ultraSoundSignal); } while(val == HIGH) { // Loop until pin reads a high value val = digitalRead(ultraSoundSignal); timecount = timecount +1; // Count echo pulse time } /* Writing out values to the serial port * ------------------------------------------------------------------- */ ultrasoundValue = timecount; // Append echo pulse time to ultrasoundValue //serialWrite('A'); // Example identifier for the sensor //printInteger(ultrasoundValue); //serialWrite(10); //serialWrite(13); /* Lite up LED if any value is passed by the echo pulse * ------------------------------------------------------------------- */ if(timecount > 0){ digitalWrite(ledPin, HIGH); } } void Scan() { // Scan for the Clearest Path oldDistance = 30; task = 0; for(i = 1; i < 5; i++) { switch(i) { case 1: //Serial.println("Pos. 1"); pwm = 1125; /// incr. by 100 from 1085 break; case 2: //Serial.println("Pos. 2"); pwm = 850; //// increased by 100 from 850 break; case 3: //Serial.println("Pos. 3"); pwm = 400; break; case 4: //Serial.println("Pos. 4"); pwm = 235; break; } for(pulseCount = 0; pulseCount < 20; pulseCount++) { // Adjust Pan Servo and Read USR digitalWrite(PingServoPin, HIGH); delayMicroseconds(pwm * 2); digitalWrite(PingServoPin, LOW); readPing(); delay(20); } distance = ((float)ultrasoundValue * CmConstant); // Calculate Distance in Cm if(distance > oldDistance) { // If the Newest distance is longer, replace previous reading with it oldDistance = distance; task = i; // Set task equal to Pan Servo Position } } //Serial.print("Task: "); //Serial.println(task); //Serial.print("distance: "); //Serial.println(distance); //Serial.print("oldDistance: "); //Serial.println(oldDistance); distance = 50; // Prevents Scan from Looping switch(task) { // Determine which task should be carried out case 0: // Center was clearest x = 28; PLval = (850); PRval = (850); Go(); break; case 1: // 90 degrees Left was Clearest x = 14; PLval = (650); PRval = (650); Go(); break; case 2: // 45 degrees left x = 7; PLval = (650); PRval = (650); Go(); break; case 3: // 45 degrees right x = 7; PLval = (850); PRval = (850); Go(); break; case 4: // 90 degrees right x = 14; PLval = (850); PRval = (850); Go(); break; } }