safwan092

Untitled

Oct 19th, 2018
161
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
  6. 9 --> IN1
  7. 8 --> IN2
  8. 7 --> IN3
  9. 6 --> IN4
  10. 10 --> ENB
  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 = 100;//255 = max
  52. unsigned char Rpwm_val = 100;//255 = max
  53.  
  54. void M_Control_IO_config(void)
  55. {
  56. pinMode(trigPin, OUTPUT);
  57. pinMode(echoPin, INPUT);
  58. pinMode(SensorLeft, INPUT);
  59. pinMode(SensorMiddle, INPUT);
  60. pinMode(SensorRight, INPUT);
  61. pinMode(pinLB, OUTPUT);
  62. pinMode(pinLF, OUTPUT);
  63. pinMode(pinRB, OUTPUT);
  64. pinMode(pinRF, OUTPUT);
  65. pinMode(Lpwm_pin, OUTPUT); // pin 11 (PWM)
  66. pinMode(Rpwm_pin, OUTPUT); // pin 10 (PWM)
  67. }
  68. void Set_Speed(unsigned char Left, unsigned char Right)
  69. {
  70. analogWrite(Lpwm_pin, Left);
  71. analogWrite(Rpwm_pin, Right);
  72. }
  73.  
  74. void turnR()
  75. {
  76. digitalWrite(pinRB, LOW);
  77. digitalWrite(pinRF, HIGH);
  78. digitalWrite(pinLB, LOW);
  79. digitalWrite(pinLF, HIGH);
  80. Serial.println("RIGHT");
  81. }
  82. void back()
  83. {
  84. digitalWrite(pinRB, LOW);
  85. digitalWrite(pinRF, HIGH);
  86. digitalWrite(pinLB, HIGH);
  87. digitalWrite(pinLF, LOW);
  88. Serial.println("Back");
  89. }
  90. void advance()
  91. {
  92. digitalWrite(pinRB, HIGH);
  93. digitalWrite(pinRF, LOW );
  94. digitalWrite(pinLB, LOW);
  95. digitalWrite(pinLF, HIGH);
  96. Serial.println("Front");
  97. }
  98. void stopp()
  99. {
  100. digitalWrite(pinRB, HIGH);
  101. digitalWrite(pinRF, HIGH);
  102. digitalWrite(pinLB, HIGH);
  103. digitalWrite(pinLF, HIGH);
  104. Serial.println("STOP!!!!!!");
  105. }
  106. void turnL()
  107. {
  108. digitalWrite(pinRB, HIGH);
  109. digitalWrite(pinRF, LOW);
  110. digitalWrite(pinLB, HIGH);
  111. digitalWrite(pinLF, LOW);
  112. Serial.println("LEFT");
  113. }
  114.  
  115. void hornON() {
  116. digitalWrite(4, 1);
  117. }
  118. void hornOFF() {
  119. digitalWrite(4, 0);
  120. }
  121.  
  122. void setup()
  123. {
  124. //Serial.begin(9600);
  125. pinMode(4, OUTPUT);
  126. M_Control_IO_config();
  127. Set_Speed(Lpwm_val, Rpwm_val);
  128. stopp();
  129. delay(5000);
  130.  
  131. }
  132.  
  133. void Sensor_Scan(void)
  134. {
  135. digitalWrite(trigPin, LOW);
  136. delayMicroseconds(2);
  137. digitalWrite(trigPin, HIGH);
  138. delayMicroseconds(10);
  139. digitalWrite(trigPin, LOW);
  140. duration = pulseIn(echoPin, HIGH);
  141.  
  142. distance = duration * 0.034 / 2;
  143. SL = digitalRead(SensorLeft);
  144. SM = digitalRead(SensorMiddle);
  145. SR = digitalRead(SensorRight);
  146. //Serial.println(distance);
  147. //Serial.print(SL);
  148. //Serial.print(SM);
  149. //Serial.println(SR);
  150. }
  151.  
  152. void loop()
  153. {
  154. Sensor_Scan();
  155.  
  156. if (distance > 10) {
  157. hornOFF();
  158. ///////////////////////////////////////////////////////////////////////////////
  159. if (SM == HIGH)// middle sensor in black area
  160. {
  161. if (SL == LOW & SR == HIGH) // black on left, white on right, turn left
  162. {
  163. turnR();
  164. }
  165. else if (SR == LOW & SL == HIGH) // white on left, black on right, turn right
  166. {
  167. turnL();
  168. }
  169. else // white on both sides, going forward
  170. {
  171. advance();
  172.  
  173. }
  174. }
  175. ///////////////////////////////////////////////////////////////////////////////
  176. else // middle sensor on white area
  177. {
  178. if (SL == LOW & SR == HIGH) // black on left, white on right, turn left
  179. {
  180. turnR();
  181.  
  182. }
  183. else if (SR == LOW & SL == HIGH) // white on left, black on right, turn right
  184. {
  185. turnL();
  186.  
  187.  
  188. }
  189. else // all white, stop
  190. {
  191. // back();
  192. delay(100);
  193. stopp() ;
  194. }
  195. }
  196. ///////////////////////////////////////////////////////////////////////////////
  197. }
  198.  
  199. else {
  200. stopp();
  201. hornON();
  202. delay(1000);
  203. hornOFF();
  204. delay(1000);
  205. }
  206. // delay(1000);
  207. }//end of loop
RAW Paste Data