safwan092

Obstical_avoidance_with_line_following_4WD_car

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