1. //this is a modified C++ for Arduino
  2.  
  3. //two slashes mean that what i am writing is a note, so it does not get executed, the notes are used
  4. //to tell you what I am doing
  5. //setting inputs on the arduino to their respective roles
  6. #define echoPin 3
  7. #define trigPin 2
  8. #define motor1Pin1 8
  9. #define motor1Pin2 9
  10. #define motor2Pin1 10
  11. #define motor2Pin2 11
  12. #define redLed 4
  13. #define greenLed 5
  14. #define blueLed 6
  15.  
  16.  
  17. void setup()//what the Arduino does before it runs its loop
  18. {
  19. //setting pins to either inputs or outputs
  20. pinMode(trigPin, OUTPUT);
  21. pinMode(echoPin, INPUT); //this is an input because the sensor is telling the Arduino how long the echo took, as
  22. //you will see below
  23. pinMode(motor1Pin1, OUTPUT); //this is an output because the Arduino is telling the motor to spin
  24. pinMode(motor1Pin2, OUTPUT);
  25. pinMode(motor2Pin1, OUTPUT);
  26. pinMode(motor2Pin2, OUTPUT);
  27. pinMode(redLed, OUTPUT);
  28. pinMode(greenLed, OUTPUT);
  29. pinMode(blueLed, OUTPUT);
  30. //set serial port to output data, so I can make sure the distance data is correct
  31. Serial.begin (9600);
  32. }
  33.  
  34. void loop() //begin loop, this is what the Arduino keeps doing indefinitely
  35. {
  36. //this gets a distance from the sensor
  37. //NOTE: I take two different readings of distance because sometimes intereference was causing abnormal distance
  38. //readings (e.g. -687 instead of 90), which messed things up.
  39. //I then test to make sure the two readings are not to different from each other, and then I average the two
  40. //readings together and use that as a final distance.
  41. int duration1, distance1, duration2, distance2, predistance1, predistance2; //create an variable that is an integer for duration and distance
  42. digitalWrite(trigPin, HIGH); //output a HIGH (full 5v) signal from trigpin
  43. delayMicroseconds(1000); //wait 1000 microseconds
  44. digitalWrite(trigPin, LOW);// stop outputting signal from trigpin
  45. duration1 = pulseIn(echoPin, HIGH); // set the variable duration to how long it took for the echoPin to receive 5v
  46. predistance1 = (duration1/2) / 29.1; //to find distance, divide duration by 2 and then by 29.1, to get centimeters
  47. distance1 = abs(predistance1);//take the abosolute value of the distance (to make sure I dont get negative numbers)
  48. Serial.print("distance1: "); //print the word distance1 in the serial port
  49. Serial.print(distance1); //print valua for distance1 in the serial port
  50. Serial.println(" cm"); //print cm next to it, then add new line
  51. delay(1); //delay for 1 millisecond
  52. digitalWrite(trigPin, HIGH);
  53. delayMicroseconds(1000);
  54. digitalWrite(trigPin, LOW);
  55. duration2 = pulseIn(echoPin, HIGH);
  56. predistance2 = (duration2/2) / 29.1;
  57. distance2 = abs(predistance2);
  58. Serial.print("distance2: ");
  59. Serial.print(distance2);
  60. Serial.println(" cm");
  61.  
  62.  
  63. //This tests to make sure the two different distance values are not more different than 20
  64. if ((distance2 < distance1 && distance1 - distance2 > 20) || (distance2 >= distance1 && distance2 - distance1 > 20))
  65. {
  66. //if they are a bigger difference than 20, make them both 30 (the robot will continue forward, since this is
  67. // greater than the 20 required to turn around)
  68. distance1 = 30;
  69. distance2 = 30;
  70. }
  71. //this makes a new variable distance which is the average of the two previous distance variables
  72. int distance = (distance1+distance2)/2;
  73.  
  74. // Serial.print("distance: "); //print distance in the serial port
  75. // Serial.print(distance); //print distance in the serial port
  76. // Serial.println(" cm"); //print cm next to it, then add new line
  77. delay(1);
  78. //this is what makes the robot autonomous
  79. if ((distance > 20)) //if the distance from the sensor to an object is greater than 20cm, do the following
  80. {
  81. GoForward(); // execute GoForward function (which is below)
  82. digitalWrite(redLed, LOW); // turn red LED off
  83. digitalWrite(greenLed, LOW); // turn green LED off
  84. digitalWrite(blueLed, HIGH); // turn blue LEd on
  85. //note: I can make any color by using various combinations of red, blue, and green
  86. }
  87. else
  88. {
  89.  
  90. GoBack(); // go back (
  91. digitalWrite(redLed, HIGH); // turn on red LED
  92. digitalWrite(greenLed, LOW);
  93. digitalWrite(blueLed, LOW);
  94. delay(750); //continue GoBack() for 1.5 seconds
  95.  
  96. TurnRight(); //then, turn right
  97. digitalWrite(redLed, LOW);
  98. digitalWrite(greenLed, HIGH); // turn on green LED
  99. digitalWrite(blueLed, LOW);
  100. delay(1250);
  101.  
  102. }
  103. }
  104.  
  105. void GoForward() // this is the GoForward function, void means that it doesnt get executed unless another function
  106. // calls it
  107. {
  108. //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
  109. digitalWrite(motor1Pin1, HIGH); //output signal on motor1pin1
  110. digitalWrite(motor1Pin2, LOW); //do not output signal on motor1pin2
  111. digitalWrite(motor2Pin1, LOW); //do not output signal on motor2pin1
  112. digitalWrite(motor2Pin2, HIGH); //output signal on motor2pin2
  113.  
  114. }
  115.  
  116. void GoBack()
  117. {
  118. digitalWrite(motor1Pin1, LOW);
  119. digitalWrite(motor1Pin2, HIGH);
  120. digitalWrite(motor2Pin1, HIGH);
  121. digitalWrite(motor2Pin2, LOW);
  122. }
  123.  
  124. void TurnRight()
  125. {
  126. digitalWrite(motor1Pin1, HIGH);
  127. digitalWrite(motor1Pin2, LOW);
  128. digitalWrite(motor2Pin1, HIGH);
  129. digitalWrite(motor2Pin2, LOW);
  130. }