View difference between Paste ID: mUTqdC98 and FCp1drMW

 /*************************************************
  * Public Constants
  *************************************************/
 
 #define NOTE_B0  31
 #define NOTE_C1  33
 #define NOTE_CS1 35
 #define NOTE_D1  37
 #define NOTE_DS1 39
 #define NOTE_E1  41
 #define NOTE_F1  44
 #define NOTE_FS1 46
 #define NOTE_G1  49
 #define NOTE_GS1 52
 #define NOTE_A1  55
 #define NOTE_AS1 58
 #define NOTE_B1  62
 #define NOTE_C2  65
 #define NOTE_CS2 69
 #define NOTE_D2  73
 #define NOTE_DS2 78
 #define NOTE_E2  82
 #define NOTE_F2  87
 #define NOTE_FS2 93
 #define NOTE_G2  98
 #define NOTE_GS2 104
 #define NOTE_A2  110
 #define NOTE_AS2 117
 #define NOTE_B2  123
 #define NOTE_C3  131
 #define NOTE_CS3 139
 #define NOTE_D3  147
 #define NOTE_DS3 156
 #define NOTE_E3  165
 #define NOTE_F3  175
 #define NOTE_FS3 185
 #define NOTE_G3  196
 #define NOTE_GS3 208
 #define NOTE_A3  220
 #define NOTE_AS3 233
 #define NOTE_B3  247
 #define NOTE_C4  262
 #define NOTE_CS4 277
 #define NOTE_D4  294
 #define NOTE_DS4 311
 #define NOTE_E4  330
 #define NOTE_F4  349
 #define NOTE_FS4 370
 #define NOTE_G4  392
 #define NOTE_GS4 415
 #define NOTE_A4  440
 #define NOTE_AS4 466
 #define NOTE_B4  494
 #define NOTE_C5  523
 #define NOTE_CS5 554
 #define NOTE_D5  587
 #define NOTE_DS5 622
 #define NOTE_E5  659
 #define NOTE_F5  698
 #define NOTE_FS5 740
 #define NOTE_G5  784
 #define NOTE_GS5 831
 #define NOTE_A5  880
 #define NOTE_AS5 932
 #define NOTE_B5  988
 #define NOTE_C6  1047
 #define NOTE_CS6 1109
 #define NOTE_D6  1175
 #define NOTE_DS6 1245
 #define NOTE_E6  1319
 #define NOTE_F6  1397
 #define NOTE_FS6 1480
 #define NOTE_G6  1568
 #define NOTE_GS6 1661
 #define NOTE_A6  1760
 #define NOTE_AS6 1865
 #define NOTE_B6  1976
 #define NOTE_C7  2093
 #define NOTE_CS7 2217
 #define NOTE_D7  2349
 #define NOTE_DS7 2489
 #define NOTE_E7  2637
 #define NOTE_F7  2794
 #define NOTE_FS7 2960
 #define NOTE_G7  3136
 #define NOTE_GS7 3322
 #define NOTE_A7  3520
 #define NOTE_AS7 3729
 #define NOTE_B7  3951
 #define NOTE_C8  4186
 #define NOTE_CS8 4435
 #define NOTE_D8  4699
 #define NOTE_DS8 4978
 
 #include <AFMotor.h> // Enables the Motor library
 #include <Servo.h> // Enables the Servo library
 //#include "pitches.h"
 
 // notes in the melody:
 int melody[] = {
   NOTE_C4, NOTE_G3, NOTE_G3, NOTE_A3, NOTE_G3, 0, NOTE_B3, NOTE_C4
 };
 
 // note durations: 4 = quarter note, 8 = eighth note, etc.:
 int noteDurations[] = {
   4, 8, 8, 4, 4, 4, 4, 4
 };
 long randNumber;
 //Servo PingServo;
 AF_DCMotor motor1(1); // Motor 1 is connected to the port 1 on the motor shield
 AF_DCMotor motor2(2); // Motor 2 is connected to the port 2 on the motor shield
 AF_DCMotor motor3(3); // Motor 3 is connected to the port 1 on the motor shield
 AF_DCMotor motor4(4); // Motor 4 is connected to the port 2 on the motor shield
 int minSafeDist = 15 ; // Minimum distance for ping sensor to know when to turn
 //int pingPin = 30; // Ping sensor is connected to port A0
 int trigPin = 30;
 int echoPin = 31;
 float pingTime;
 float targetDistance;
 float speedOfSound = 776.5;
 int centerDist, leftDist, rightDist, backDist; // Define variables center, left, right and back distance
 long duration, inches, cm; // Define variables for Ping sensor
 
 int trigPinL = 34;
 int echoPinL = 35;
 float pingTimeL;
 float targetDistanceL;
 long inchesL;
 
 int beepPin = 36;
 
 void beep(unsigned char delayms){
   for (int thisNote = 0; thisNote < 8; thisNote++) {
 
     // to calculate the note duration, take one second
     // divided by the note type.
     //e.g. quarter note = 1000 / 4, eighth note = 1000/8, etc.
     int noteDuration = 1000 / noteDurations[thisNote];
     tone(beepPin, melody[thisNote], noteDuration);
 
     // to distinguish the notes, set a minimum time between them.
     // the note's duration + 30% seems to work well:
     int pauseBetweenNotes = noteDuration * 1.30;
     delay(pauseBetweenNotes);
     // stop the tone playing:
     noTone(beepPin);
   }
 }
 
 
 void setup() {
 //PingServo.attach(10); // Servo is attached to pin 10 in the motor shield
 //PingServo.write(90); // Center the Ping sensor (puts it at 90 degrees)
 pinMode(trigPin, OUTPUT);
 pinMode(echoPin, INPUT);
 pinMode(beepPin, OUTPUT);
 motor1.setSpeed(200); // Sets the speed of the first motor (At 0, the motors are turned off. 255 is the fastest setting that you are able to use, I used 215 to not push the motors too hard.)
 motor2.setSpeed(200); // Sets the speed of the second motor (At 0, the motors are turned off. 255 is the fastest setting that you are able to use, I used 215 to not push the motors too hard.)
 motor3.setSpeed(200); // Sets the speed of the first motor (At 0, the motors are turned off. 255 is the fastest setting that you are able to use, I used 215 to not push the motors too hard.)
 motor4.setSpeed(200);
 Serial.begin(9600); // Enables Serial monitor for debugging purposes
 Serial.println("Serial test!"); // Test the Serial communication
 
 }
 
 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);
 }
 void AllForward() { // Makes the robot go forward
 motor1.run(FORWARD); // Motor 1 goes forward
 motor2.run(FORWARD); // Motor 2 goes forward
 //delay(100);
 //if(inches >= minSafeDist && inchesL >= minSafeDist){
   motor3.run(FORWARD); // Motor 3 goes forward
   motor4.run(FORWARD); // Motor 4 goes forward
 //}
 Serial.println("Going forward"); // Prints a line in the serial monitor
 }
 void turnRight() { // Makes the robot go right
 motor2.run(BACKWARD); // Turns off motor 2
 motor1.run(FORWARD); // Motor 1 goes forward
 delay(1600); // Time required to turn right (1.6 seconds)
 Serial.println("Motors going Right"); // Prints a line in the serial monitor
 }
 void GoBack(){ // Makes the robot go back
 motor2.run(BACKWARD); // Motor 2 goes back
 motor1.run(BACKWARD); // Motor 1 goes back
 delay(1600); // Time Required to go back (1.6 seconds)
 Serial.println("Backward"); // Prints a line in the serial monitor
 }
 void turnLeft() { // Makes the robot go Left
 motor2.run(FORWARD); // Motor 2 goes forward
 motor1.run(BACKWARD); // turns off motor 1
 delay(1600); //Time Required to turn left (1.6)Seconds
 Serial.println("Motors going Left");// Prints a line in the serial monitor
 }
 
 // Starts the loop to decide what to do
 void loop()
 {
 LookAhead();
 Serial.print(inches);
 Serial.println(" inches"); // Prints a line in the serial monitor
 Serial.println(inches);
 if(inches >= minSafeDist || inchesL >= minSafeDist) /* If the inches in front of an object is greater than or equal to the minimum safe distance (11 inches), react*/
 {
 AllForward(); // All wheels forward
 //delay(110); // Wait 0.11 seconds
 }else if (inches < minSafeDist) // If not:
 {
   Serial.print("Turn right");
   beep(200);
   AllStop();
   //randNumber = random(10, 20);
   //if(randNumber < 10.) {
   //  turnLeft();
   //  }
   //else {
     turnRight();
   //}
   //turnLeft();
 } /*else if (inchesL < minSafeDist) {
   Serial.print("Turn right");
   AllStop();
   turnRight();
 }*/
 /*{
 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 (leftDist > rightDist) // If the left distance is greater than the right distance , turn left
 {
 turnLeft();
 }else if (leftDist&&rightDist<minSafeDist) // If the left and right distance is smaller than the min safe distance (11 inch) go back
 {
 GoBack();
 }
 }*/
 }
 
 unsigned long ping() {
   digitalWrite(trigPin, LOW);
   delayMicroseconds(2000);
   digitalWrite(trigPin, HIGH);
   delayMicroseconds(15);
   digitalWrite(trigPin, LOW);
   pingTime = pulseIn(echoPin, HIGH);
   pingTime = pingTime/1000000.;
   pingTime = pingTime/3600.;
   targetDistance = speedOfSound * pingTime;
   targetDistance = targetDistance/2;
   inches = targetDistance*63360; //Convert target distance to inches from mile;
   Serial.print("targetDistance");
 }
 
 unsigned long pingL() {
   digitalWrite(trigPinL, LOW);
   delayMicroseconds(2000);
   digitalWrite(trigPinL, HIGH);
   delayMicroseconds(15);
   digitalWrite(trigPinL, LOW);
   pingTimeL = pulseIn(echoPinL, HIGH);
   pingTimeL = pingTimeL/1000000.;
   pingTimeL = pingTimeL/3600.;
   targetDistanceL = speedOfSound * pingTimeL;
   targetDistanceL = targetDistanceL/2;
   inchesL = targetDistanceL*63360; //Convert target distance to inches from mile;
   Serial.print("targetDistanceL");
   Serial.print(inchesL);
 }
 
 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;
 }
 
 void LookAhead() {
 //PingServo.write(90);// angle to look forward
 delay(175); // wait 0.175 seconds
 ping();
 //if(inches < minSafeDist) {AllStop();}
 //pingL();
 //if(inchesL < minSafeDist) {AllStop();}
 }
 
 void LookAround(){
 //PingServo.write(180); // 180° angle
 //delay(320); // wait 0.32 seconds
 ping();
 pingL();
 //rightDist = inches; //get the right distance
 //PingServo.write(0); // look to the other side
 //delay(620); // wait 0.62 seconds
 //ping();
 //leftDist = inches; // get the left distance
 //PingServo.write(90); // 90° angle
 //delay(275); // wait 0.275 seconds
 
 // Prints a line in the serial monitor
 //Serial.print("RightDist: ");
 //Serial.println(rightDist);
 //Serial.print("LeftDist: ");
 //Serial.println(leftDist);
 //Serial.print("CenterDist: ");
 //Serial.println(centerDist);
 }