Here is the code. Thanks so much for helping me on this.
#include <Servo.h>
#define echoPinLeft 7 // Left Echo Pin
#define trigPinLeft 8 // Left Trigger Pin
#define echoPinRight 9 // Right Echo Pin
#define trigPinRight 10 // Right Trigger Pin
//long durationLeft, distanceLeft; // Duration used to calculate distance
//long durationRight, distanceRight; //Right front sensor duration used to calculate distance
//Declare Servos
Servo steerservo; //steering servo
Servo driveservo; //drive servo
const int turntime=0; //number of milliseconds to hold turn when turning
const int turnaroundtime=1000; // number of milliseconds to turn completely around
const int steerservopin=11; //pin number for steering servo
const int driveservopin=12; //pin number for drive servo
const int distancelimit=20; //threshold in inches to initiate reaction
const int pingdelay=50 // milliseconds delay added prior to a ping
//Setup function. Runs once when Arduino starts or is reset
void setup() {
Serial.begin (9600);
steerservo.attach(steerservopin); // attaches the steering servo to its pin 11
driveservo.attach(driveservopin); // attaches the drive servo to its pin 12
delay(1000); // delay one second
}
void loop(){
//go(); // proceed using the go function (disabled to implement only when if else enables)
int distanceleft=pingleft(); // invoke left sensor ping function to look for obstacles left of centre
int distanceright=pingright(); // invoke right sensor ping function to look for obstacles right of centre
Serial.print("Distance Left: ");
Serial.println(distanceleft);
Serial.print("Distance Right: ");
Serial.println(distanceright);
if(distanceleft<distancelimit || distanceright<distancelimit) {
char turndirection=scan(); // Goes to scan function to decide which way to turn based on l,r,s values.
switch (turndirection){
case ‘l’:
turnleft(turntime);
//delay(turntime);
break; // exits the case
case ‘r’:
turnright(turntime);
//delay(turntime);
break;
case ‘s’:
turnleft(turnaroundtime);
break;
}
}
else
{
go();
}
}
int pingleft(){
delay(pingdelay); // delay between sensor firing to avoid risk of interference
long durationLeft, inchesLeft, cmLeft;
//Send Left pulse
pinMode(echoPinLeft, INPUT);
pinMode(trigPinLeft, OUTPUT);
digitalWrite(trigPinLeft, LOW);
delayMicroseconds(2);
digitalWrite(trigPinLeft, HIGH);
delayMicroseconds(10);
digitalWrite(trigPinLeft, LOW);
//read left echo
durationLeft=pulseIn(echoPinLeft, HIGH);
// convert the time to distance
inchesLeft=microsecondsToInches(durationLeft);
cmLeft=microsecondsToCentimeters(durationLeft);
Serial.print("Ping Left: ");
Serial.println(inchesLeft);
return round (inchesLeft);
}
int pingright(){
delay(pingdelay); // delay between sensor firing to avoid risk of interference
long durationRight, inchesRight, cmRight;
//Send Right pulse
pinMode(echoPinRight, INPUT);
pinMode(trigPinRight, OUTPUT);
digitalWrite(trigPinRight, LOW);
delayMicroseconds(2);
digitalWrite(trigPinRight, HIGH);
delayMicroseconds(10);
digitalWrite(trigPinRight, LOW);
//read right echo
durationRight=pulseIn(echoPinRight, HIGH);
// convert the time to distance
inchesRight=microsecondsToInches(durationRight);
cmRight=microsecondsToCentimeters(durationRight);
Serial.print("Ping Right: ");
Serial.println(inchesRight);
return round (inchesRight);
} //Driving the servo motors
void go(){
steerservo.write(90); //for now, we don’t go anywhere, thus value is 90 for both which is stopped
driveservo.write(120);
Serial.println("Going forward ");
}
void turnleft(int t){
steerservo.write(150);
driveservo.write(90);
delay(turntime);
Serial.println("Turning Left ");
}
void turnright(int t){
steerservo.write(30);
driveservo.write(90);
delay(turntime);
Serial.println("Turning Right ");
}
void stopmotors(){
steerservo.write(90); // 90 is stop
driveservo.write(90);
Serial.println("STOPPED ");
}
char scan(){
int leftscanval, rightscanval;
char choice;
// read left
leftscanval=pingleft();
delay(50);
//read right
rightscanval=pingright();
delay(50);
// The turning decision is made here
if(leftscanval>rightscanval){
choice=‘l’;
}
else if(rightscanval>leftscanval){
choice=‘r’;
}
else{
choice=‘s’;
}
Serial.print("Choice: ");
Serial.println(choice);
return choice;
}
long microsecondsToInches(long microseconds){
return microseconds/74/2;
}
long microsecondsToCentimeters(long microseconds){
return microseconds/29/2;
}
