Advertisement
Guest User

Untitled

a guest
Jan 29th, 2020
97
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 3.72 KB | None | 0 0
  1. #include <Servo.h> //Servo motor library. This is standard library
  2. #include <NewPing.h> //Ultrasonic sensor function library. You must install this library
  3. #include <toneAC.h>
  4.  
  5. //our L298N control pins
  6. const int LeftMotorForward = 7;
  7. const int LeftMotorBackward = 6;
  8. const int RightMotorForward = 4;
  9. const int RightMotorBackward = 5;
  10. int red_light_pin= 11;
  11. int green_light_pin = 10;
  12. int blue_light_pin = 9;
  13.  
  14. //sensor pins
  15. #define trig_pin A1 //analog input 1
  16. #define echo_pin A2 //analog input 2
  17.  
  18. #define maximum_distance 200
  19. boolean goesForward = false;
  20. int distance = 100;
  21.  
  22. NewPing sonar(trig_pin, echo_pin, maximum_distance); //sensor function
  23. Servo servo_motor; //our servo name
  24.  
  25.  
  26. void setup(){
  27.  
  28. pinMode(RightMotorForward, OUTPUT);
  29. pinMode(LeftMotorForward, OUTPUT);
  30. pinMode(LeftMotorBackward, OUTPUT);
  31. pinMode(RightMotorBackward, OUTPUT);
  32. pinMode(red_light_pin, OUTPUT);
  33. pinMode(green_light_pin, OUTPUT);
  34. pinMode(blue_light_pin, OUTPUT);
  35.  
  36. servo_motor.attach(10); //our servo pin
  37.  
  38. servo_motor.write(115);
  39. delay(2000);
  40. distance = readPing();
  41. delay(100);
  42. distance = readPing();
  43. delay(100);
  44. distance = readPing();
  45. delay(100);
  46. distance = readPing();
  47. delay(100);
  48. }
  49.  
  50. void loop(){
  51.  
  52. int distanceRight = 0;
  53. int distanceLeft = 0;
  54. delay(50);
  55.  
  56. if (distance <= 20){
  57. moveStop();
  58. delay(300);
  59. moveBackward();
  60. delay(400);
  61. moveStop();
  62. delay(300);
  63. distanceRight = lookRight();
  64. delay(300);
  65. distanceLeft = lookLeft();
  66. delay(300);
  67.  
  68. if (distance >= distanceLeft){
  69. turnRight();
  70. moveStop();
  71. }
  72. else{
  73. turnLeft();
  74. moveStop();
  75. }
  76. }
  77. else{
  78. moveForward();
  79. }
  80. distance = readPing();
  81. }
  82.  
  83. int lookRight(){
  84. servo_motor.write(50);
  85. delay(500);
  86. int distance = readPing();
  87. delay(100);
  88. servo_motor.write(115);
  89. return distance;
  90. }
  91.  
  92. int lookLeft(){
  93. servo_motor.write(170);
  94. delay(500);
  95. int distance = readPing();
  96. delay(100);
  97. servo_motor.write(115);
  98. return distance;
  99. delay(100);
  100. }
  101.  
  102. int readPing(){
  103. delay(70);
  104. int cm = sonar.ping_cm();
  105. if (cm==0){
  106. cm=250;
  107. }
  108. return cm;
  109. }
  110.  
  111. void moveStop(){
  112.  
  113. RGB_color(255, 0, 0); // Red
  114. delay(1000);
  115. digitalWrite(RightMotorForward, LOW);
  116. digitalWrite(LeftMotorForward, LOW);
  117. digitalWrite(RightMotorBackward, LOW);
  118. digitalWrite(LeftMotorBackward, LOW);
  119. }
  120.  
  121. void moveForward(){
  122.  
  123. RGB_color(0, 255, 0); // Green
  124. delay(1000);
  125. if(!goesForward){
  126.  
  127. goesForward=true;
  128.  
  129. digitalWrite(LeftMotorForward, HIGH);
  130. digitalWrite(RightMotorForward, HIGH);
  131.  
  132. digitalWrite(LeftMotorBackward, LOW);
  133. digitalWrite(RightMotorBackward, LOW);
  134. }
  135. }
  136.  
  137. void moveBackward(){
  138.  
  139. goesForward=false;
  140.  
  141. digitalWrite(LeftMotorBackward, HIGH);
  142. digitalWrite(RightMotorBackward, HIGH);
  143.  
  144. digitalWrite(LeftMotorForward, LOW);
  145. digitalWrite(RightMotorForward, LOW);
  146.  
  147. }
  148.  
  149. void turnRight(){
  150.  
  151. digitalWrite(LeftMotorForward, HIGH);
  152. digitalWrite(RightMotorBackward, HIGH);
  153.  
  154. digitalWrite(LeftMotorBackward, LOW);
  155. digitalWrite(RightMotorForward, LOW);
  156.  
  157. delay(500);
  158.  
  159. digitalWrite(LeftMotorForward, HIGH);
  160. digitalWrite(RightMotorForward, HIGH);
  161.  
  162. digitalWrite(LeftMotorBackward, LOW);
  163. digitalWrite(RightMotorBackward, LOW);
  164.  
  165.  
  166.  
  167. }
  168.  
  169. void turnLeft(){
  170.  
  171. digitalWrite(LeftMotorBackward, HIGH);
  172. digitalWrite(RightMotorForward, HIGH);
  173.  
  174. digitalWrite(LeftMotorForward, LOW);
  175. digitalWrite(RightMotorBackward, LOW);
  176.  
  177. delay(500);
  178.  
  179. digitalWrite(LeftMotorForward, HIGH);
  180. digitalWrite(RightMotorForward, HIGH);
  181.  
  182. digitalWrite(LeftMotorBackward, LOW);
  183. digitalWrite(RightMotorBackward, LOW);
  184. }
  185.  
  186. void RGB_color(int red_light_value, int green_light_value, int blue_light_value)
  187. {
  188. analogWrite(red_light_pin, red_light_value);
  189. analogWrite(green_light_pin, green_light_value);
  190. analogWrite(blue_light_pin, blue_light_value);
  191. }
  192.  
  193. void song(){
  194. toneAC( frequency [, volume [, length [, background ]]] )
  195. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement