Arduino Code:
  1. #include "NewPing.h"
  2.  
  3. int LeftMotorPlus = 9;
  4. int LeftMotorMinus = 8;
  5. int LeftMotorEnable = 12;
  6.  
  7. int RightMotorPlus = 11;
  8. int RightMotorMinus = 10;
  9. int RightMotorEnable = 13;
  10.  
  11. #define TRIGGER_PIN_RIGHT 6 // Arduino pin tied to trigger pin on the ultrasonic sensor.
  12. #define ECHO_PIN_RIGHT 7 // Arduino pin tied to echo pin on the ultrasonic sensor.
  13. int distance_left;
  14.  
  15. #define TRIGGER_PIN_LEFT 2 // Arduino pin tied to trigger pin on the ultrasonic sensor.
  16. #define ECHO_PIN_LEFT 3
  17. int distance_right;
  18.  
  19. #define TRIGGER_PIN_FRONT 4 // Arduino pin tied to trigger pin on the ultrasonic sensor.
  20. #define ECHO_PIN_FRONT 5 // Arduino pin tied to echo pin on the ultrasonic sensor.
  21. int distance_front;
  22.  
  23. #define MAX_DISTANCE 400 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
  24. int min_distance;
  25. NewPing sonar_right(TRIGGER_PIN_RIGHT, ECHO_PIN_RIGHT, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
  26. NewPing sonar_left(TRIGGER_PIN_LEFT, ECHO_PIN_LEFT, MAX_DISTANCE);
  27. NewPing sonar_front(TRIGGER_PIN_FRONT, ECHO_PIN_FRONT, MAX_DISTANCE);
  28.  
  29. void setup()
  30. {
  31. pinMode(LeftMotorPlus, OUTPUT);
  32. pinMode(LeftMotorMinus, OUTPUT);
  33. pinMode(LeftMotorEnable, OUTPUT);
  34.  
  35. pinMode(RightMotorEnable, OUTPUT);
  36. pinMode(RightMotorMinus , OUTPUT);
  37. pinMode(RightMotorPlus , OUTPUT);
  38. }
  39.  
  40. void move_forward() {
  41. digitalWrite(LeftMotorEnable, HIGH);
  42. digitalWrite(RightMotorEnable, HIGH);
  43.  
  44. digitalWrite(LeftMotorPlus, HIGH);
  45. digitalWrite(LeftMotorMinus, LOW);
  46.  
  47.  
  48. digitalWrite(RightMotorPlus, LOW);
  49. digitalWrite(RightMotorMinus, HIGH);
  50. }
  51.  
  52. void move_backward() {
  53. digitalWrite(LeftMotorEnable, HIGH);
  54. digitalWrite(RightMotorEnable, HIGH);
  55.  
  56. digitalWrite(LeftMotorPlus, LOW);
  57. digitalWrite(LeftMotorMinus, HIGH);
  58.  
  59.  
  60. digitalWrite(RightMotorPlus, HIGH);
  61. digitalWrite(RightMotorMinus, LOW);
  62. }
  63.  
  64. void turn_left() {
  65. digitalWrite(LeftMotorEnable, HIGH);
  66. digitalWrite(RightMotorEnable, HIGH);
  67.  
  68. digitalWrite(LeftMotorPlus, HIGH);
  69. digitalWrite(LeftMotorMinus, LOW);
  70.  
  71.  
  72. digitalWrite(RightMotorPlus, HIGH);
  73. digitalWrite(RightMotorMinus, LOW);
  74. }
  75.  
  76. void turn_right() {
  77. digitalWrite(LeftMotorEnable, HIGH);
  78. digitalWrite(RightMotorEnable, HIGH);
  79.  
  80. digitalWrite(LeftMotorPlus, LOW);
  81. digitalWrite(LeftMotorMinus, HIGH);
  82.  
  83.  
  84. digitalWrite(RightMotorPlus, LOW);
  85. digitalWrite(RightMotorMinus, HIGH);
  86. }
  87.  
  88. void loop() {
  89. //move_forward();
  90. // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
  91. unsigned int uS_left = sonar_left.ping(); // Send ping, get ping time in microseconds (uS).
  92. //Serial.print("Left Sensor Ping: ");
  93. distance_left = uS_left / US_ROUNDTRIP_CM;
  94. //Serial.print(distance_left); // Convert ping time to distance and print result (0 = outside set distance range, no ping echo)
  95. //Serial.println("cm");
  96. unsigned int uS_right = sonar_right.ping(); // Send ping, get ping time in microseconds (uS).
  97. //Serial.print("Right Sensor Ping: ");
  98. distance_right = uS_right / US_ROUNDTRIP_CM;
  99. //Serial.print(distance_right); // Convert ping time to distance and print result (0 = outside set distance range, no ping echo)
  100. //Serial.println("cm");
  101. //Serial.print("The Minimum distance of obstacle is");
  102. unsigned int uS_front = sonar_front.ping(); // Send ping, get ping time in microseconds (uS).
  103. //Serial.print("Right Sensor Ping: ");
  104. distance_front = uS_front / US_ROUNDTRIP_CM;
  105. if (distance_front > 30)
  106. {
  107. move_forward();
  108. }
  109. if (distance_front < 30)
  110. {
  111. if (distance_left > 30)
  112. {
  113. turn_left();
  114. delay(500);
  115. move_forward();
  116. }
  117. else if (distance_left < 30)
  118. {
  119. if (distance_right > 30)
  120. {
  121. turn_right();
  122. delay(500);
  123. move_forward();
  124. }
  125. else
  126. {
  127. move_backward();
  128. delay(2000);
  129. turn_left();
  130. delay(500);
  131. move_forward();
  132. }
  133. }
  134. }
  135. delay(50);
  136. }