SHARE
TWEET

Untitled

a guest Oct 21st, 2019 70 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. //******************************
  2. #include <IRremote.h>
  3. #include <Servo.h>
  4. gt
  5. //*********************** define motor pin *************************
  6. int MotorRight1=5;
  7. int MotorRight2=6;
  8. int MotorLeft1=10;
  9. int MotorLeft2=11;
  10. int counter=0;
  11. const int irReceiverPin = 2; // set pin 2 as IR receiver signal OUTPUT
  12. char val;
  13. //***********************set the IRcode from the test result*************************
  14. long IRfront= 0x00FF629D; // code for going forward
  15. long IRback=0x00FFA857; // going backward
  16. long IRturnright=0x00FFC23D; // turn right
  17. long IRturnleft= 0x00FF22DD; // turn left
  18. long IRstop=0x00FF02FD; // stop
  19. long IRcny70=0x00FF6897; // CNY70 aoto-movingmode
  20. long IRAutorun=0x00FF9867; // ultrasonic aoto-movingmode
  21. long IRturnsmallleft= 0x00FFB04F;
  22. //************************* define pin CNY70************************************
  23. const int SensorLeft = 7; // input pin for left sensor
  24. const int SensorMiddle= 4 ; // input pin for middle sensor
  25. const int SensorRight = 3; // input pin for right sensor
  26. int SL; // left sensor status
  27. int SM; // middle sensor status
  28. int SR; // right sensor status
  29. IRrecv irrecv(irReceiverPin); // set IRrecv to receive IR signal
  30. decode_results results; // decode result will be put into the variable of the result in the
  31. //************************* define pin for ultrasonic ******************************
  32. int inputPin =13 ; // define ultrasonic signal receiving pin rx
  33. int outputPin =12; // define ultrasonic signal sending pin'tx
  34. int Fspeedd = 0; // distance upfront
  35. int Rspeedd = 0; // distance on the right
  36. int Lspeedd = 0; // distance on the left
  37. int directionn = 0; // F=8 B=2 L=4 R=6
  38. Servo myservo; // set myservo
  39. int delay_time = 250; // settling time for the servo motor moving backwards
  40. int Fgo = 8; // going forward
  41. int Rgo = 6; // going right
  42. int Lgo = 4;// going left
  43. int Bgo = 2;// going backwards
  44. //********************************************************************(SETUP)
  45. void setup()
  46. {
  47. Serial.begin(9600);
  48. pinMode(MotorRight1, OUTPUT); // pin 8 (PWM)
  49. pinMode(MotorRight2, OUTPUT); // pin 9 (PWM)
  50. pinMode(MotorLeft1, OUTPUT); // pin 10 (PWM)
  51. pinMode(MotorLeft2, OUTPUT); // pin 11 (PWM)
  52. irrecv.enableIRIn(); // start IR decoding
  53. pinMode(SensorLeft, INPUT); //define left sensor
  54. pinMode(SensorMiddle, INPUT);// define middle sensor
  55. pinMode(SensorRight, INPUT); // define right sensor
  56. digitalWrite(2,HIGH);
  57. pinMode(inputPin, INPUT); // define receiving pin for ultrasonic signal
  58. pinMode(outputPin, OUTPUT); // define sending pin for ultrasonic signal
  59. myservo.attach(9); // set servo motor output as pin 5(PWM)
  60. }
  61. //******************************************************************(Void)
  62. void advance(int a) // go forward
  63. {
  64. digitalWrite(MotorRight1,LOW);
  65. digitalWrite(MotorRight2,HIGH);
  66. digitalWrite(MotorLeft1,LOW);
  67. digitalWrite(MotorLeft2,HIGH);
  68. delay(a * 100);
  69. }
  70. void right(int b) //turn right(1 wheel)
  71. {
  72. digitalWrite(MotorLeft1,LOW);
  73. digitalWrite(MotorLeft2,HIGH);
  74. digitalWrite(MotorRight1,LOW);
  75. digitalWrite(MotorRight2,LOW);
  76. delay(b * 100);
  77. }
  78. void left(int c) // turn left(1 wheel)
  79. {
  80. digitalWrite(MotorRight1,LOW);
  81. digitalWrite(MotorRight2,HIGH);
  82. digitalWrite(MotorLeft1,LOW);
  83. digitalWrite(MotorLeft2,LOW);
  84. delay(c * 100);
  85. }
  86. void turnR(int d) // turn right(2 wheels)
  87. {
  88. digitalWrite(MotorRight1,HIGH);
  89. digitalWrite(MotorRight2,LOW);
  90. digitalWrite(MotorLeft1,LOW);
  91. digitalWrite(MotorLeft2,HIGH);
  92. delay(d * 100);
  93. }
  94. void turnL(int e) // turn left (2 wheels)
  95. {
  96. digitalWrite(MotorRight1,LOW);
  97. digitalWrite(MotorRight2,HIGH);
  98. digitalWrite(MotorLeft1,HIGH);
  99. digitalWrite(MotorLeft2,LOW);
  100. delay(e * 100);
  101. }
  102. void stopp(int f) // stop
  103. {
  104. digitalWrite(MotorRight1,LOW);
  105. digitalWrite(MotorRight2,LOW);
  106. digitalWrite(MotorLeft1,LOW);
  107. digitalWrite(MotorLeft2,LOW);
  108. delay(f * 100);
  109. }
  110. void back(int g) // go backwards
  111. {
  112. digitalWrite(MotorRight1,HIGH);
  113. digitalWrite(MotorRight2,LOW);
  114. digitalWrite(MotorLeft1,HIGH);
  115. digitalWrite(MotorLeft2,LOW);;
  116. delay(g * 100);
  117. }
  118. void detection() // measure 3 angles(0.90.179)
  119. {
  120. int delay_time = 250; // settling time for the servo motor moving backwards
  121. ask_pin_F(); // read the distance upfront
  122. if(Fspeedd < 10) // if distance less than 10cm
  123. {
  124. stopp(1); // clear output information
  125. back(2); // oing backwards for 0.2second
  126. }
  127. if(Fspeedd < 15) // if distance less than 25cm
  128. {
  129. stopp(1); // clear output information
  130. ask_pin_L(); // read the distance on the left
  131. delay(delay_time); // settling time for the servo
  132. ask_pin_R(); // read the distance on the right
  133. delay(delay_time); // settling time for the servo
  134. if(Lspeedd > Rspeedd) //if distance on the left is more than that on the right
  135. {
  136. directionn = Lgo; // go left
  137. }
  138. if(Lspeedd <= Rspeedd) // if distance on the left is less than that on the right
  139. {
  140. directionn = Rgo; // going right
  141. }
  142. if (Lspeedd < 10 && Rspeedd < 10) // if both distance are less than 10cm
  143. {
  144. directionn = Bgo; // going backwards
  145. }
  146. }
  147. else // if the distance upfront is more than 25cm
  148. {
  149. directionn = Fgo; // going forward
  150. }
  151. }
  152. //*****************************************************************************
  153. void ask_pin_F() // measure the distance upfront
  154. {
  155. myservo.write(90);
  156. delay(delay_time);
  157. digitalWrite(outputPin, LOW);// ultrasonic sends out low voltage 2μs
  158. delayMicroseconds(2);
  159. digitalWrite(outputPin, HIGH); // ultrasonic sends out high voltage 10μs, at least 10μs
  160. delayMicroseconds(10);
  161. digitalWrite(outputPin, LOW); // maintain low voltage sending
  162. float Fdistance = pulseIn(inputPin, HIGH); // read the time difference
  163. Fdistance= Fdistance/5.8/10; // convert time into distance (unit: cm)
  164. Serial.print("Fdistance:"); //output distance in cm
  165. Serial.println(Fdistance);// display distance
  166. Fspeedd = Fdistance; // read the distance data into Fspeedd
  167. }
  168. //*****************************************************************************
  169. void ask_pin_L() // measure the distance on the left
  170. {
  171. myservo.write(177);
  172. delay(delay_time);
  173. digitalWrite(outputPin, LOW); // ultrasonic sends out low voltage 2μs
  174. delayMicroseconds(2);
  175. digitalWrite(outputPin, HIGH); // ultrasonic sends out high voltage 10μs, at least 10μs
  176. delayMicroseconds(10);
  177. digitalWrite(outputPin, LOW); // maintain low voltage sending
  178. float Ldistance = pulseIn(inputPin, HIGH); // read the time difference
  179. Ldistance= Ldistance/5.8/10; //converttime intodistance(unit: cm)
  180. Serial.print("Ldistance:"); //output distance in cm
  181. Serial.println(Ldistance);//display distance
  182. Lspeedd = Ldistance; // read the distance data into Lspeedd
  183. }
  184. //*****************************************************************************
  185. void ask_pin_R() // measure the distance on the right
  186. {
  187. myservo.write(5);
  188. delay(delay_time);
  189. digitalWrite(outputPin, LOW);// ultrasonic sends out low voltage 2μs
  190. delayMicroseconds(2);
  191. digitalWrite(outputPin, HIGH); // ultrasonic sends out high voltage 10μs, at least 10μs
  192. delayMicroseconds(10);
  193. digitalWrite(outputPin, LOW); //maintain low voltage sending
  194. float Rdistance = pulseIn(inputPin, HIGH); //read the time difference
  195. Rdistance= Rdistance/5.8/10; //convert time into distance(unit: cm)
  196. Serial.print("Rdistance:"); //output distance in cm
  197. Serial.println(Rdistance);//display distance
  198. Rspeedd = Rdistance; // read the distance data into Rspeedd
  199. }
  200. //******************************************************************************(LOOP)
  201. void loop()
  202. {
  203. SL=digitalRead(SensorLeft);
  204. SM = digitalRead(SensorMiddle);
  205. SR = digitalRead(SensorRight);
  206. performCommand();
  207. //***************************************************normal remote control mode
  208. if(irrecv.decode(&results))
  209. { // finish decoding, receive IR signal
  210. /***********************************************************************/
  211. if (results.value == IRfront)// go forward
  212. {
  213. advance(10);// go forward
  214. }
  215. /***********************************************************************/
  216. if (results.value == IRback)// go backward
  217. {
  218. back(5);// go backward
  219. }
  220. /***********************************************************************/
  221. if (results.value == IRturnright)// turn right
  222. {
  223. right(5); // turn right
  224. }
  225. /***********************************************************************/
  226. if (results.value == IRturnleft)// turn left
  227. {
  228. left(5); // turn left);
  229. }
  230. /***********************************************************************/
  231. if (results.value == IRstop)// stop
  232. {
  233. digitalWrite(MotorRight1,LOW);
  234. digitalWrite(MotorRight2,LOW);
  235. digitalWrite(MotorLeft1,LOW);
  236. digitalWrite(MotorLeft2,LOW);
  237. }
  238. //****************************************************** black and white line mode
  239. if (results.value == IRcny70)
  240. {
  241. while(IRcny70)
  242. {
  243. SL= digitalRead(SensorLeft);
  244. SM = digitalRead(SensorMiddle);
  245. SR = digitalRead(SensorRight);
  246. if (SM == HIGH)// middle sensor in black area
  247. {
  248. if (SL == LOW & SR == HIGH) // black on left, white on right, turn left
  249. {
  250. digitalWrite(MotorRight1,LOW);
  251. digitalWrite(MotorRight2,HIGH);
  252. digitalWrite(MotorLeft1,LOW);
  253. digitalWrite(MotorLeft2,LOW);
  254. }
  255. else if (SR == LOW & SL == HIGH) // white on left, black on right, turn right
  256. {
  257. digitalWrite(MotorLeft1,LOW);
  258. digitalWrite(MotorLeft2,HIGH);
  259. digitalWrite(MotorRight1,LOW);
  260. digitalWrite(MotorRight2,LOW);
  261. }
  262. else // white on both sides, going forward
  263. {
  264. digitalWrite(MotorRight1,LOW);
  265. digitalWrite(MotorRight2,HIGH);
  266. digitalWrite(MotorLeft1,LOW);
  267. digitalWrite(MotorLeft2,HIGH);
  268. }
  269. }
  270. else // middle sensor on white area
  271. {
  272. if (SL== LOW & SR == HIGH)// black on left, white on right, turn left
  273. {
  274. digitalWrite(MotorRight1,LOW);
  275. digitalWrite(MotorRight2,HIGH);
  276. digitalWrite(MotorLeft1,LOW);
  277. digitalWrite(MotorLeft2,LOW);
  278. }
  279. else if (SR == LOW & SL == HIGH) // white on left, black on right, turn right
  280. {
  281. digitalWrite(MotorLeft1,LOW);
  282. digitalWrite(MotorLeft2,HIGH);
  283. digitalWrite(MotorRight1,LOW);
  284. digitalWrite(MotorRight2,LOW);
  285. }
  286. else // all white, stop
  287. {
  288. digitalWrite(MotorRight1,LOW);
  289. digitalWrite(MotorRight2,LOW);
  290. digitalWrite(MotorLeft1,LOW);
  291. digitalWrite(MotorLeft2,LOW);
  292. }
  293. }
  294. if(irrecv.decode(&results))
  295. {
  296. irrecv.resume();
  297. Serial.println(results.value,HEX);
  298. if(results.value==IRstop)
  299. {
  300. digitalWrite(MotorRight1,HIGH);
  301. digitalWrite(MotorRight2,HIGH);
  302. digitalWrite(MotorLeft1,HIGH);
  303. digitalWrite(MotorLeft2,HIGH);
  304. break;
  305. }
  306. }
  307. }
  308. results.value=0;
  309. }
  310. //******************************************************** ultrasonic auto-moving mode
  311. if (results.value ==IRAutorun )
  312. {
  313. while(IRAutorun)
  314. {
  315. myservo.write(90); // reset the servo motor and prepare it for the next measurement
  316. detection(); // measure the angle and decide which direction to move
  317. if(directionn == 8) // if directionn = 8
  318. {
  319. if(irrecv.decode(&results))
  320. {
  321. irrecv.resume();
  322. Serial.println(results.value,HEX);
  323. if(results.value==IRstop)
  324. {
  325. digitalWrite(MotorRight1,LOW);
  326. digitalWrite(MotorRight2,LOW);
  327. digitalWrite(MotorLeft1,LOW);
  328. digitalWrite(MotorLeft2,LOW);
  329. break;
  330. }
  331. }
  332. results.value=0; advance(1); // going forward
  333. Serial.print("Advance "); // display direction(forward)
  334. Serial.print(" ");
  335. }
  336. if(directionn == 2) // if directionn = 2
  337. {
  338. if(irrecv.decode(&results))
  339. {
  340. irrecv.resume();
  341. Serial.println(results.value,HEX);
  342. if(results.value==IRstop)
  343. {
  344. digitalWrite(MotorRight1,LOW);
  345. digitalWrite(MotorRight2,LOW);
  346. digitalWrite(MotorLeft1,LOW);
  347. digitalWrite(MotorLeft2,LOW);
  348. break;
  349. }
  350. }
  351. results.value=0;
  352. back(2); // going backwards
  353. turnL(3); // slightly move to the left to avoid stuck in the dead end
  354. Serial.print(" Reverse "); // display direction (backwards)
  355. }
  356. if(directionn == 6) // if direction = 6
  357. {
  358. if(irrecv.decode(&results))
  359. {
  360. irrecv.resume();
  361. Serial.println(results.value,HEX);
  362. if(results.value==IRstop)
  363. {
  364. digitalWrite(MotorRight1,LOW);
  365. digitalWrite(MotorRight2,LOW);
  366. digitalWrite(MotorLeft1,LOW);
  367. digitalWrite(MotorLeft2,LOW);
  368. break;
  369. }
  370. }
  371. results.value=0;
  372. back(2);
  373. turnR(3); // turn right
  374. Serial.print(" Right "); // display direction(right)
  375. }
  376. if(directionn == 4) // if direction = 4
  377. {
  378. if(irrecv.decode(&results))
  379. {
  380. irrecv.resume();
  381. Serial.println(results.value,HEX);
  382. if(results.value==IRstop)
  383. {
  384. digitalWrite(MotorRight1,LOW);
  385. digitalWrite(MotorRight2,LOW);
  386. digitalWrite(MotorLeft1,LOW);
  387. digitalWrite(MotorLeft2,LOW);
  388. break;
  389. }
  390. }
  391. results.value=0;
  392. back(2);
  393. turnL(3); // turn left
  394. Serial.print(" Left "); // display direction(left)
  395. }
  396. if(irrecv.decode(&results))
  397. {
  398. irrecv.resume();
  399. Serial.println(results.value,HEX);
  400. if(results.value==IRstop)
  401. {
  402. digitalWrite(MotorRight1,LOW);
  403. digitalWrite(MotorRight2,LOW);
  404. digitalWrite(MotorLeft1,LOW);
  405. digitalWrite(MotorLeft2,LOW);
  406. break;
  407. }
  408. }
  409. }
  410. results.value=0;
  411. }
  412. /***********************************************************************/
  413. else
  414. {
  415. digitalWrite(MotorRight1,LOW);
  416. digitalWrite(MotorRight2,LOW);
  417. digitalWrite(MotorLeft1,LOW);
  418. digitalWrite(MotorLeft2,LOW);
  419. }
  420. irrecv.resume(); // continue receiving IR signal coming next
  421. }
  422. }
  423. void performCommand()
  424. { if(Serial.available())
  425. { val = Serial.read();
  426. }
  427. if (val == 'U') { // Forward
  428. advance(1);
  429. } else if (val == 'D') { //Backward
  430. back(1);
  431. } else if (val == 'R') { // Right
  432. turnR(1);
  433. } else if (val == 'L') { // Left
  434. turnL(1);
  435. } else if (val== 'S') { // Stop
  436. stopp(1) ;
  437. }
  438. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top