//this is a modified C++ for Arduino
//two slashes mean that what i am writing is a note, so it does not get executed, the notes are used
//to tell you what I am doing
//setting inputs on the arduino to their respective roles
#define echoPin 3
#define trigPin 2
#define motor1Pin1 8
#define motor1Pin2 9
#define motor2Pin1 10
#define motor2Pin2 11
#define redLed 4
#define greenLed 5
#define blueLed 6
void setup()//what the Arduino does before it runs its loop
{
//setting pins to either inputs or outputs
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT); //this is an input because the sensor is telling the Arduino how long the echo took, as
//you will see below
pinMode(motor1Pin1, OUTPUT); //this is an output because the Arduino is telling the motor to spin
pinMode(motor1Pin2, OUTPUT);
pinMode(motor2Pin1, OUTPUT);
pinMode(motor2Pin2, OUTPUT);
pinMode(redLed, OUTPUT);
pinMode(greenLed, OUTPUT);
pinMode(blueLed, OUTPUT);
//set serial port to output data, so I can make sure the distance data is correct
Serial.begin (9600);
}
void loop() //begin loop, this is what the Arduino keeps doing indefinitely
{
//this gets a distance from the sensor
//NOTE: I take two different readings of distance because sometimes intereference was causing abnormal distance
//readings (e.g. -687 instead of 90), which messed things up.
//I then test to make sure the two readings are not to different from each other, and then I average the two
//readings together and use that as a final distance.
int duration1, distance1, duration2, distance2, predistance1, predistance2; //create an variable that is an integer for duration and distance
digitalWrite(trigPin, HIGH); //output a HIGH (full 5v) signal from trigpin
delayMicroseconds(1000); //wait 1000 microseconds
digitalWrite(trigPin, LOW);// stop outputting signal from trigpin
duration1 = pulseIn(echoPin, HIGH); // set the variable duration to how long it took for the echoPin to receive 5v
predistance1 = (duration1/2) / 29.1; //to find distance, divide duration by 2 and then by 29.1, to get centimeters
distance1 = abs(predistance1);//take the abosolute value of the distance (to make sure I dont get negative numbers)
Serial.print("distance1: "); //print the word distance1 in the serial port
Serial.print(distance1); //print valua for distance1 in the serial port
Serial.println(" cm"); //print cm next to it, then add new line
delay(1); //delay for 1 millisecond
digitalWrite(trigPin, HIGH);
delayMicroseconds(1000);
digitalWrite(trigPin, LOW);
duration2 = pulseIn(echoPin, HIGH);
predistance2 = (duration2/2) / 29.1;
distance2 = abs(predistance2);
Serial.print("distance2: ");
Serial.print(distance2);
Serial.println(" cm");
//This tests to make sure the two different distance values are not more different than 20
if ((distance2 < distance1 && distance1 - distance2 > 20) || (distance2 >= distance1 && distance2 - distance1 > 20))
{
//if they are a bigger difference than 20, make them both 30 (the robot will continue forward, since this is
// greater than the 20 required to turn around)
distance1 = 30;
distance2 = 30;
}
//this makes a new variable distance which is the average of the two previous distance variables
int distance = (distance1+distance2)/2;
// Serial.print("distance: "); //print distance in the serial port
// Serial.print(distance); //print distance in the serial port
// Serial.println(" cm"); //print cm next to it, then add new line
delay(1);
//this is what makes the robot autonomous
if ((distance > 20)) //if the distance from the sensor to an object is greater than 20cm, do the following
{
GoForward(); // execute GoForward function (which is below)
digitalWrite(redLed, LOW); // turn red LED off
digitalWrite(greenLed, LOW); // turn green LED off
digitalWrite(blueLed, HIGH); // turn blue LEd on
//note: I can make any color by using various combinations of red, blue, and green
}
else
{
GoBack(); // go back (
digitalWrite(redLed, HIGH); // turn on red LED
digitalWrite(greenLed, LOW);
digitalWrite(blueLed, LOW);
delay(750); //continue GoBack() for 1.5 seconds
TurnRight(); //then, turn right
digitalWrite(redLed, LOW);
digitalWrite(greenLed, HIGH); // turn on green LED
digitalWrite(blueLed, LOW);
delay(1250);
}
}
void GoForward() // this is the GoForward function, void means that it doesnt get executed unless another function
// calls it
{
//by changing which pins signal is outputed, the integrated circuit (this one is a L293D H-Bridge) changes which direction and which motor is spinning
digitalWrite(motor1Pin1, HIGH); //output signal on motor1pin1
digitalWrite(motor1Pin2, LOW); //do not output signal on motor1pin2
digitalWrite(motor2Pin1, LOW); //do not output signal on motor2pin1
digitalWrite(motor2Pin2, HIGH); //output signal on motor2pin2
}
void GoBack()
{
digitalWrite(motor1Pin1, LOW);
digitalWrite(motor1Pin2, HIGH);
digitalWrite(motor2Pin1, HIGH);
digitalWrite(motor2Pin2, LOW);
}
void TurnRight()
{
digitalWrite(motor1Pin1, HIGH);
digitalWrite(motor1Pin2, LOW);
digitalWrite(motor2Pin1, HIGH);
digitalWrite(motor2Pin2, LOW);
}