//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);
}