safwan092

4wd obstical avodance for amer

Nov 26th, 2018
175
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. /****************************
  2. Connections:
  3. -----------------------------
  4. [Arduino Uno] --> [L298N Red Board]:
  5. 11 --> ENA (Black)/(أسود)
  6. 9 --> IN1 (White)/(أبيض)
  7. 8 --> IN2 (Gray)/(رمادي)
  8. 7 --> IN3 (Purple)/(أرجواني)
  9. 6 --> IN4 (Blue)/(أزرق)
  10. 10 --> ENB (Green)/(أخضر)
  11. [Arduino Uno] --> [Ultrasonic Sensor]:
  12. 5V --> Vcc (Purple)/(أرجواني)
  13. A1 --> Trig (Gray)/(رمادي)
  14. A0 --> Echo (White)/(أبيض)
  15. GND --> GND (Black)/(أسود)
  16. [Arduino Uno] --> [IR Sensor (Left)]:
  17. GND --> GND (Brown)/(بني)
  18. 5V --> VCC (Red)/(أحمر)
  19. 2 --> OUT (Orange)/(برتقالي)
  20. [Arduino Uno] --> [IR Sensor (Center)]:
  21. GND --> GND (Gray)/(رمادي)
  22. 5V --> VCC (White)/(أبيض)
  23. 3 --> OUT (Black)/(أسود)
  24. [Arduino Uno] --> [IR Sensor (Right)]:
  25. GND --> GND (Brown)/(بني)
  26. 5V --> VCC (Red)/(أحمر)
  27. 4 --> OUT (Orange)/(برتقالي)
  28. [Arduino Uno] --> [Buzzer]:
  29. GND --> - (Green)/(أخضر)
  30. 13 --> + (Yellow)/(أصفر)
  31. -----------------------------
  32. *****************************/
  33. const int trigPin = A1;
  34. const int echoPin = A0;
  35. // defines variables
  36. long duration;
  37. int distance;
  38.  
  39. #define SensorLeft 2 //input pin of left sensor
  40. #define SensorMiddle 3 //input pin of middle sensor
  41. #define SensorRight 4 //input pin of right sensor
  42. unsigned char SL; //state of left sensor
  43. unsigned char SM; //state of middle sensor
  44. unsigned char SR; //state of right sensor
  45.  
  46.  
  47.  
  48. #define Lpwm_pin 10 //ENB
  49. #define Rpwm_pin 11 //ENA
  50. int pinLB = 6; //IN4
  51. int pinLF = 7; //IN3
  52. int pinRB = 8; //IN2
  53. int pinRF = 9; //IN1
  54. unsigned char Lpwm_val = 100;//255 = max
  55. unsigned char Rpwm_val = 100;//255 = max
  56.  
  57. void M_Control_IO_config(void)
  58. {
  59. pinMode(trigPin, OUTPUT);
  60. pinMode(echoPin, INPUT);
  61. pinMode(SensorLeft, INPUT);
  62. pinMode(SensorMiddle, INPUT);
  63. pinMode(SensorRight, INPUT);
  64. pinMode(pinLB, OUTPUT);
  65. pinMode(pinLF, OUTPUT);
  66. pinMode(pinRB, OUTPUT);
  67. pinMode(pinRF, OUTPUT);
  68. pinMode(Lpwm_pin, OUTPUT); // pin 11 (PWM)
  69. pinMode(Rpwm_pin, OUTPUT); // pin 10 (PWM)
  70. }
  71. void Set_Speed(unsigned char Left, unsigned char Right)
  72. {
  73. analogWrite(Lpwm_pin, Left);
  74. analogWrite(Rpwm_pin, Right);
  75. }
  76.  
  77. void turnR()
  78. {
  79. digitalWrite(pinRB, LOW);
  80. digitalWrite(pinRF, HIGH);
  81. digitalWrite(pinLB, LOW);
  82. digitalWrite(pinLF, HIGH);
  83. delay(10);
  84. }
  85. void back()
  86. {
  87. digitalWrite(pinRB, LOW);
  88. digitalWrite(pinRF, HIGH);
  89. digitalWrite(pinLB, HIGH);
  90. digitalWrite(pinLF, LOW);
  91. delay(10);
  92. }
  93. void advance()
  94. {
  95. digitalWrite(pinRB, HIGH);
  96. digitalWrite(pinRF, LOW );
  97. digitalWrite(pinLB, LOW);
  98. digitalWrite(pinLF, HIGH);
  99. delay(10);
  100. }
  101. void stopp()
  102. {
  103. digitalWrite(pinRB, HIGH);
  104. digitalWrite(pinRF, HIGH);
  105. digitalWrite(pinLB, HIGH);
  106. digitalWrite(pinLF, HIGH);
  107. delay(10);
  108. }
  109. void turnL()
  110. {
  111. digitalWrite(pinRB, HIGH);
  112. digitalWrite(pinRF, LOW);
  113. digitalWrite(pinLB, HIGH);
  114. digitalWrite(pinLF, LOW);
  115. delay(10);
  116. }
  117.  
  118. void hornON() {
  119. digitalWrite(13, 1);
  120. }
  121. void hornOFF() {
  122. digitalWrite(13, 0);
  123. }
  124.  
  125. void setup()
  126. {
  127. pinMode(13, OUTPUT);
  128. M_Control_IO_config();
  129. Set_Speed(Lpwm_val, Rpwm_val);
  130. stopp();
  131. delay(5000);
  132.  
  133. }
  134.  
  135. void Sensor_Scan(void)
  136. {
  137. digitalWrite(trigPin, LOW);
  138. delayMicroseconds(2);
  139. digitalWrite(trigPin, HIGH);
  140. delayMicroseconds(10);
  141. digitalWrite(trigPin, LOW);
  142. duration = pulseIn(echoPin, HIGH);
  143.  
  144. distance = duration * 0.034 / 2;
  145. SL = digitalRead(SensorLeft);
  146. SM = digitalRead(SensorMiddle);
  147. SR = digitalRead(SensorRight);
  148. }
  149.  
  150. void loop()
  151. {
  152. Sensor_Scan();
  153.  
  154. if (distance > 10) {
  155. hornOFF();
  156. ///////////////////////////////////////////////////////////////////////////////
  157. if (SM == HIGH)// middle sensor in black area
  158. {
  159. if (SL == LOW & SR == HIGH) // black on left, white on right, turn left
  160. {
  161. turnR();
  162. }
  163. else if (SR == LOW & SL == HIGH) // white on left, black on right, turn right
  164. {
  165. turnL();
  166. }
  167. else // white on both sides, going forward
  168. {
  169. advance();
  170.  
  171. }
  172. }
  173. ///////////////////////////////////////////////////////////////////////////////
  174. else // middle sensor on white area
  175. {
  176. if (SL == LOW & SR == HIGH) // black on left, white on right, turn left
  177. {
  178. turnR();
  179.  
  180. }
  181. else if (SR == LOW & SL == HIGH) // white on left, black on right, turn right
  182. {
  183. turnL();
  184.  
  185.  
  186. }
  187. else // all white, stop
  188. {
  189. // back();
  190. delay(100);
  191. stopp() ;
  192. }
  193. }
  194. ///////////////////////////////////////////////////////////////////////////////
  195. }
  196.  
  197. else {
  198. stopp();
  199. hornON();
  200. delay(500);
  201. hornOFF();
  202. delay(500);
  203. }
  204. }//end of loop
RAW Paste Data