API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Mar 5th, 2015
186
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 48.46 KB | None | 0 0
raw download clone embed print report
  1. /*
  2.  *
  3.  * Team Id            :  e-YRC#3105-FF
  4.  * Author List        :  Abhinav Sarkar, Shivam Bhardwaj, Tapan Khattar, Dhruv Gaba
  5.  * Filename           :  Final_Code
  6.  * Theme              :  Fire-Fighting
  7.  * Functions          :  motor_init, forward, backward, left, right, soft_left, soft_right, stopbot, linear_distance_mm, angle_rotate_degrees,forward_mm, backward_mm, left_degrees, right_degrees, soft_left_degrees, soft_right_degrees
  8.                          velocity, incrementright, incrementleft,catchline_left, catchline_right, followlinetillbbw,followlinetillwbb,followlinetillbbb,followlinetillbbworwbb,followlinetillbbworbbb, followlinetillwbborbbb,
  9.                          servod, servof, drop_mgnet, fire_detection, room_entrance_algo, room_traversal, setup, loop
  10.  * Global Variables   :  left_motor_1, right_motor_1,left_motor_2, right_motor_2,left_PWM, right_PWM, threshold, fire_detection_flag, room_entrance_flag, left_sensor, center_sensor, right_sensor
  11.                          leftshaftcount,rightshaftcount,m1,m2,m3, magnet_number, buzzer
  12.  *
  13.  *
  14.  */
  15.  
  16.  
  17. #include <LiquidCrystal.h>                 // Including Liquid crystal Library for displaying information on the Firebird V LCD
  18.  
  19. #define f5 255                             // Highest PWM value for speed of dc motors
  20. #define f4 240                             // Next PWM value for dc motor (mainly for inroom traversal)
  21.  
  22.  
  23. LiquidCrystal lcd(37, 35, 33, 32, 31, 30);
  24.  
  25. int left_motor_1 = 22;
  26. int left_motor_2 = 23;
  27. int right_motor_1 = 25;
  28. int right_motor_2 = 24;
  29. int left_PWM = 46;
  30. int right_PWM = 45;
  31. int threshold = 33;
  32. int fire_detection_flag;
  33. int magnet_number= 0;
  34. int left_sensor, center_sensor , right_sensor, left_sharp;
  35. volatile unsigned long int leftshaftcount = 0;
  36. volatile unsigned long int rightshaftcount = 0;
  37.  
  38. boolean m1, m2, m3;                                 // These variables display the surface detected by line sensors. They are 1 if white is detected and 0 if black is detected
  39. boolean room_entrance_flag;
  40.  
  41.  
  42. /*
  43. *
  44. * Function Name      :  motor_init
  45. * Input              :  None
  46. * Output             :  None
  47. * Logic              :  This function initialises all the motor pins as output
  48. * Example Call       :  motor_init();
  49. *
  50. */
  51. void motor_init()
  52. {
  53.   pinMode(left_motor_1, OUTPUT);
  54.   pinMode(left_motor_2, OUTPUT);
  55.   pinMode(right_motor_1, OUTPUT);
  56.   pinMode(right_motor_2, OUTPUT);
  57. }
  58.  
  59. /*
  60. *
  61. * Function Name      :  forward
  62. * Input              :  None
  63. * Output             :  None
  64. * Logic              :  This function makes both the motors rotate in forward direction hence allowing robot to move forward.
  65. * Example Call       :  forward();
  66. *
  67. */
  68. void forward()
  69. {
  70.   digitalWrite(left_motor_1, 0);
  71.   digitalWrite(left_motor_2, 1);
  72.   digitalWrite(right_motor_1, 0);
  73.   digitalWrite(right_motor_2, 1);
  74. }
  75.  
  76. /*
  77. *
  78. * Function Name      :  backward
  79. * Input              :  None
  80. * Output             :  None
  81. * Logic              :  This function makes both the motors rotate in backward direction hence allowing robot to move backward.
  82. * Example Call       :  backward();
  83. *
  84. */
  85. void backward()
  86. {
  87.   digitalWrite(left_motor_1, 1);
  88.   digitalWrite(left_motor_2, 0);
  89.   digitalWrite(right_motor_1, 1);
  90.   digitalWrite(right_motor_2, 0);
  91. }
  92.  
  93. /*
  94. *
  95. * Function Name      :  left
  96. * Input              :  None
  97. * Output             :  None
  98. * Logic              :  This function makes right motor rotate forward and left motor rotate backwards. Hence robot turns left.
  99. * Example Call       :  left();
  100. *
  101. */
  102. void left()
  103. {
  104.   digitalWrite(left_motor_1, 1);
  105.   digitalWrite(left_motor_2, 0);
  106.   digitalWrite(right_motor_1, 0);
  107.   digitalWrite(right_motor_2, 1);
  108. }
  109.  
  110. /*
  111. *
  112. * Function Name      :  right
  113. * Input              :  None
  114. * Output             :  None
  115. * Logic              :  This function makes left motor rotate forward and right motor rotate backwards. Hence robot turns right.
  116. * Example Call       :  right();
  117. *
  118. */  
  119. void right()
  120. {
  121.   digitalWrite(left_motor_1, 0);
  122.   digitalWrite(left_motor_2, 1);
  123.   digitalWrite(right_motor_1, 1);
  124.   digitalWrite(right_motor_2, 0);
  125. }
  126.  
  127. /*
  128. *
  129. * Function Name      :  soft_left
  130. * Input              :  None
  131. * Output             :  None
  132. * Logic              :  This function makes right motor rotate forward and left motor stationary. Hence robot turns left but along the axis of left wheel.
  133. * Example Call       :  soft_left();
  134. *
  135. */  
  136. void soft_left()
  137. {
  138.   digitalWrite(left_motor_1, 0);
  139.   digitalWrite(left_motor_2, 0);
  140.   digitalWrite(right_motor_1, 0);
  141.   digitalWrite(right_motor_2, 1);
  142. }
  143.  
  144. /*
  145. *
  146. * Function Name      :  soft_right
  147. * Input              :  None
  148. * Output             :  None
  149. * Logic              :  This function makes left motor rotate forward and right motor stationary. Hence robot turns right but along the axis of right wheel.
  150. * Example Call       :  soft_right();
  151. *
  152. */
  153. void soft_right()
  154. {
  155.   digitalWrite(left_motor_1, 0);
  156.   digitalWrite(left_motor_2, 1);
  157.   digitalWrite(right_motor_1, 0);
  158.   digitalWrite(right_motor_2, 0);
  159. }
  160.  
  161. /*
  162. *
  163. * Function Name      :  stopbot
  164. * Input              :  None
  165. * Output             :  None
  166. * Logic              :  This function makes both the motors stationary hence allowing robot to stop.
  167. * Example Call       :  stopbot();
  168. *
  169. */  
  170. void stopbot()
  171. {
  172.   digitalWrite(left_motor_1, 0);
  173.   digitalWrite(left_motor_2, 0);
  174.   digitalWrite(right_motor_1, 0);
  175.   digitalWrite(right_motor_2, 0);
  176. }
  177.  
  178. /*
  179. *
  180. * Function Name      :  velocity
  181. * Input              :  left_speed, right_speed
  182. * Output             :  None
  183. * Logic              :  This function sets the PWM values for both motors hence allowing us to control the rotation speed if each motor. The PWM values can change from 0-255 for 8-bit PWM
  184. * Example Call       :  velocity(150,150);
  185. *
  186. */  
  187. void velocity(int left_speed, int right_speed)
  188. {
  189.   analogWrite(left_PWM, left_speed);
  190.   analogWrite(right_PWM, right_speed);
  191. }
  192.  
  193. /*
  194. *
  195. * Function Name      :  incrementright
  196. * Input              :  None
  197. * Output             :  None
  198. * Logic              :  This is an interrupt service routine which increments a counter, rightshaftcount everytime the right motor position encoder detects a new pulse.
  199. * Example Call       :  This is an ISR so it is not called explicitly by us. Instead it is called implicitly by the program everytime interrupt occurs
  200. *
  201. */  
  202. void incrementright()
  203. {
  204.   rightshaftcount++;
  205. }
  206.  
  207. /*
  208. *
  209. * Function Name      :  incrementleft
  210. * Input              :  None
  211. * Output             :  None
  212. * Logic              :  This is an interrupt service routine which increments a counter, leftshaftcount everytime the left motor position encoder detects a new pulse.
  213. * Example Call       :  This is an ISR so it is not called explicitly by us. Instead it is called implicitly by the program everytime interrupt occurs
  214. *
  215. */  
  216. void incrementleft()
  217. {
  218.   leftshaftcount++;
  219. }
  220.  
  221. /*
  222. *
  223. * Function Name      :  linear_distance_mm
  224. * Input              :  distance_mm
  225. * Output             :  None
  226. * Logic              :  This function takes a distance value (in mm) and converts it into number of pulses required to cover that distance. It then sets right shaft count to zero
  227.                         and counts until it exceeds the required number of pulses.
  228. * Example Call       :  linear_mm(100);
  229. *
  230. */  
  231. void linear_distance_mm(unsigned int distance_mm)
  232. {
  233.   float reqdshaftcount = 0;
  234.   unsigned long int Reqdshaftcount = 0;
  235.   reqdshaftcount = distance_mm / 5.44;
  236.   Reqdshaftcount = (unsigned long int) reqdshaftcount;
  237.  
  238.   rightshaftcount = 0;
  239.   while (1) {
  240.     if (rightshaftcount > Reqdshaftcount)
  241.     {
  242.       break;
  243.     }
  244.   }
  245.   stopbot();
  246.  
  247. }
  248.  
  249. /*
  250. *
  251. * Function Name      :  angle_rotate_degrees
  252. * Input              :  degree
  253. * Output             :  None
  254. * Logic              :  This function takes angle to rotate in degrees and converts it into number of pulses required to cover that angle. It then sets both leftshaftcount and rightshaftcount
  255.                         to zero and counts until one of the two counters exceeds the required value.
  256. * Example Call       :  angle_rotate_degrees(90);
  257. *
  258. */  
  259. void angle_rotate_degrees(unsigned int degree)
  260. {
  261.   float reqdshaftcount = 0;
  262.   unsigned long int Reqdshaftcount = 0;
  263.   reqdshaftcount = (float) degree / 4.090;
  264.   Reqdshaftcount = (unsigned int) reqdshaftcount;
  265.  
  266.   rightshaftcount = 0;
  267.   leftshaftcount = 0;
  268.  
  269.   while (1)
  270.   {
  271.     if ((rightshaftcount >= Reqdshaftcount) | (leftshaftcount >= Reqdshaftcount))
  272.     {
  273.       break;
  274.     }
  275.   }
  276.   stopbot();
  277. }
  278.  
  279. /*
  280. *
  281. * Function Name      :  forward_mm
  282. * Input              :  distance_mm
  283. * Output             :  None
  284. * Logic              :  Moves the robot forward by a required distance by first setting motors in forward mode using forward() function and then counting till required number of pulses using linear_distance_mm()
  285. * Example Call       :  forward_mm(500);
  286. *
  287. */  
  288. void forward_mm(unsigned int distance_mm)
  289. {
  290.   forward();
  291.   linear_distance_mm(distance_mm);
  292. }
  293.  
  294. /*
  295. *
  296. * Function Name      :  backward_mm
  297. * Input              :  distance_mm
  298. * Output             :  None
  299. * Logic              :  Moves the robot backward by a required distance by first setting motors in backward mode using backward() function and then counting till required number of pulses using linear_distance_mm()
  300. * Example Call       :  backward_mm(500);
  301. *
  302. */  
  303. void backward_mm(unsigned int distance_mm)
  304. {
  305.   backward();
  306.   linear_distance_mm(distance_mm);
  307. }
  308.  
  309. /*
  310. *
  311. * Function Name      :  left_degrees
  312. * Input              :  degree
  313. * Output             :  None
  314. * Logic              :  Turns the robot left by required angle by first setting motors in left mode using left() function and then counting till required number of pulses using angle_rotate_degrees
  315. * Example Call       :  left_degrees(90);
  316. *
  317. */  
  318. void left_degrees(unsigned int degree)
  319. {
  320.   left();
  321.   angle_rotate_degrees(degree);
  322. }
  323.  
  324. /*
  325. *
  326. * Function Name      :  right_degrees
  327. * Input              :  degree
  328. * Output             :  None
  329. * Logic              :  Turns the robot right by required angle by first setting motors in right mode using right() function and then counting till required number of pulses using angle_rotate_degrees
  330. * Example Call       :  right_degrees(90);
  331. *
  332. */  
  333. void right_degrees(unsigned int degree)
  334. {
  335.   right();
  336.   angle_rotate_degrees(degree);
  337. }
  338.  
  339. /*
  340. *
  341. * Function Name      :  soft_left_degrees
  342. * Input              :  degree
  343. * Output             :  None
  344. * Logic              :  Turns the robot soft left by required angle by first setting motors in soft left mode using soft_left() function and then counting till required number of pulses using angle_rotate_degrees
  345. * Example Call       :  soft_left_degrees(180);
  346. *
  347. */  
  348. void soft_left_degrees(unsigned int degree)
  349. {
  350.   soft_left();
  351.   angle_rotate_degrees(degree);
  352. }
  353.  
  354. /*
  355. *
  356. * Function Name      :  soft_right_degrees
  357. * Input              :  degree
  358. * Output             :  None
  359. * Logic              :  Turns the robot soft right by required angle by first setting motors in soft right mode using soft_right() function and then counting till required number of pulses using angle_rotate_degrees
  360. * Example Call       :  soft_right_degrees(180);
  361. *
  362. */  
  363. void soft_right_degrees(unsigned int degree)
  364. {
  365.   soft_right();
  366.   angle_rotate_degrees(degree);
  367. }
  368.  
  369. /*
  370. *
  371. * Function Name      :  whitelineshow
  372. * Input              :  None
  373. * Output             :  None
  374. * Logic              :  This function first reads the analog value of the whiteline sensors for line following and stores them in variables left_sensor, center_sensor, right_sensor and then
  375.                         prints them on the lcd screen. It then compares each sensor value to the threshold and sets boolean variables m1, m2, m3 as 0 if sensor value is greater then threshold
  376.                         i.e black surface is detected and 1 if sensor value is less then threshold i.e white surface is detected. It also prints 'B' or 'W' on the lcd screen depending on value so as to make
  377.                         debugging easy.
  378. * Example Call       :  whitelineshow();
  379. *
  380. */  
  381. void whitelineshow()
  382. {
  383.   left_sensor = analogRead(A3);                        // Checks left sensor value
  384.   center_sensor = analogRead(A2);                      // Checks center sensor value
  385.   right_sensor = analogRead(A1);                       // Checks right sensor value
  386.   lcd.setCursor(0, 0);
  387.   lcd.print(left_sensor);
  388.   lcd.setCursor(0, 1);
  389.   if (left_sensor < threshold) {
  390.     lcd.print("W");
  391.     m1 = 1;                                            // Sets boolean variable m1=1 if white is detected
  392.   }
  393.   else
  394.   {
  395.     lcd.print("B");
  396.     m1 = 0;                                            // Sets boolean variable m1=0 if black is detected
  397.   }
  398.   lcd.setCursor(4, 1);
  399.   if (center_sensor < threshold) {
  400.     lcd.print("W");
  401.     m2 = 1;                                            // Sets boolean variable m2=1 if white is detected
  402.   }
  403.   else
  404.   {
  405.     lcd.print("B");
  406.     m2 = 0;                                            // Sets boolean variable m2=0 if black is detected
  407.   }
  408.   lcd.setCursor(4, 0);
  409.   lcd.print(center_sensor);
  410.   lcd.setCursor(8, 1);
  411.   if (right_sensor < threshold) {
  412.     lcd.print("W");
  413.     m3 = 1;                                            // Sets boolean variable m3=1 if white is detected
  414.   }
  415.   else
  416.   {
  417.     lcd.print("B");
  418.     m3 = 0;                                            // Sets boolean variable m3=0 if black is detected
  419.   }
  420.   lcd.setCursor(8, 0);
  421.   lcd.print(right_sensor);
  422.  
  423. }
  424.  
  425. /*
  426. *
  427. * Function Name      :  catchline_left
  428. * Input              :  None
  429. * Output             :  None
  430. * Logic              :  Turns the robot soft left until the center sensors detect black surface. This function is generally used to catch the line again if the robot strays to the right
  431.                         side of the line
  432. * Example Call       :  catchline_left();
  433. *
  434. */  
  435. void catchline_left()
  436. {
  437.   whitelineshow();
  438.   while (!(m2 == 0 ))              // Loop goes on until center sensor detects black
  439.   {
  440.     soft_left();
  441.     whitelineshow();
  442.   }
  443.   stopbot();
  444. }
  445.  
  446. /*
  447. *
  448. * Function Name      :  catchline_right
  449. * Input              :  None
  450. * Output             :  None
  451. * Logic              :  Turns the robot soft right until the center sensors detect black surface. This function is generally used to catch the line again if the robot strays to the left
  452.                         side of the line
  453. * Example Call       :  catchline_left();
  454. *
  455. */
  456. void catchline_right()
  457. {
  458.   whitelineshow();
  459.   while (!(m2 == 0 ))               // Loop goes on until center sensor detects black
  460.   {
  461.     soft_right();
  462.     whitelineshow();
  463.   }
  464.   stopbot();
  465. }
  466.  
  467. /*
  468. *
  469. * Function Name      :  followlinetillbbw
  470. * Input              :  None
  471. * Output             :  None
  472. * Logic              :  This function makes the robot follow the line until the left and center sensors detect black surface and right sensor detects white surface. This condition can also
  473.                         be called BBW condition.
  474. * Example Call       :  followlinetillbbw();
  475. *
  476. */
  477. void followlinetillbbw()
  478. {
  479.   whitelineshow();
  480.   while (!(m1 == 0 && m2 == 0 && m3 == 1))                                   // Loop goes on until BBW condition is met
  481.   {
  482.     if ((m1 == 1 && m2 == 0 && m3 == 1) || m1 == 0 && m2 == 1 && m3 == 0)    // WBW or BWB
  483.     {
  484.       forward();
  485.     }
  486.     else if (m1 == 0 && m2 == 1 && m3 == 1 )                                 // BWW
  487.     {
  488.       left();
  489.  
  490.     }
  491.     else if (m1 == 1 && m2 == 1 && m3 == 0)                                  // WBB
  492.     {
  493.       right();
  494.     }
  495.     else if ((m1 == 0 && m2 == 0 && m3 == 0) || (m1 == 1 && m2 == 1 && m3 == 1) )  // BBB or WWW
  496.     {
  497.       forward_mm(5);
  498.     }
  499.     whitelineshow();
  500.   }
  501.   forward_mm(5);                                                              // Robot moves 5mm ahead
  502.   stopbot();
  503.  
  504. }
  505.  
  506. /*
  507. *
  508. * Function Name      :  followlinetillwbb
  509. * Input              :  None
  510. * Output             :  None
  511. * Logic              :  This function makes the robot follow the line until the right and center sensors detect black surface and left sensor detects white surface. This condition can also
  512.                         be called WBB condition.
  513. * Example Call       :  followlinetillwbb();
  514. *
  515. */  
  516. void followlinetillwbb()                                                      
  517. {
  518.   whitelineshow();
  519.   while (!(m1 == 1 && m2 == 0 && m3 == 0))                             // Loop goes on until WBB condition is met
  520.   {
  521.     if ((m1 == 1 && m2 == 0 && m3 == 1) || m1 == 0 && m2 == 1 && m3 == 0)     // WBW or BWB
  522.     {
  523.       forward();
  524.     }
  525.     else if (m1 == 0 && m2 == 1 && m3 == 1 )                            // BWW
  526.     {
  527.       left();
  528.  
  529.     }
  530.     else if (m1 == 1 && m2 == 1 && m3 == 0)                             // WWB
  531.     {
  532.       right();
  533.     }
  534.     else if ((m1 == 0 && m2 == 0 && m3 == 0) || (m1 == 1 && m2 == 1 && m3 == 1) )   // BBB or WWW
  535.     {
  536.       forward_mm(5);
  537.     }
  538.     whitelineshow();
  539.   }
  540.   forward_mm(5);
  541.   stopbot();
  542.  
  543. }
  544.  
  545. /*
  546. *
  547. * Function Name      :  followlinetillbbb
  548. * Input              :  None
  549. * Output             :  None
  550. * Logic              :  This function makes the robot follow the line until the left and center sensors detect black surface and right sensor detects white surface. This condition can also
  551.                         be called BBB condition.
  552. * Example Call       :  followlinetillbbb();
  553. *
  554. */  
  555. void followlinetillbbb()
  556. {
  557.   whitelineshow();
  558.   while (!(m1 == 0 && m2 == 0 && m3 == 0))               // Loop goes on until BBB condition is met
  559.   {
  560.     if ((m1 == 1 && m2 == 0 && m3 == 1) || m1 == 0 && m2 == 1 && m3 == 0)    // WBW or BWB
  561.     {
  562.       forward();
  563.     }
  564.     else if (m1 == 0 && m2 == 1 && m3 == 1 )              // BWW
  565.     {
  566.       left();
  567.  
  568.     }
  569.     else if (m1 == 1 && m2 == 1 && m3 == 0)               // WWB
  570.     {
  571.       right();
  572.     }
  573.     else if (m1 == 1 && m2 == 1 && m3 == 1)               // WWW
  574.     {
  575.       forward_mm(5);
  576.     }
  577.  
  578.     whitelineshow();
  579.   }
  580.   forward_mm(5);
  581.   stopbot();
  582.  
  583. }
  584.  
  585. /*
  586. *
  587. * Function Name      :  followlinetillwbborbbb
  588. * Input              :  None
  589. * Output             :  None
  590. * Logic              :  This function makes the robot follow the line till either WBB condition or BBB condition is detected by the sensors. This kind of function is useful when there is
  591.                         ambiguity on the kind of condition arising
  592. * Example Call       :  followlinetillwbborbbb();
  593. *
  594. */  
  595. void followlinetillwbborbbb()
  596. {
  597.   whitelineshow();
  598.   while (!((m1 == 1 && m2 == 0 && m3 == 0) || (m1 == 0 && m2 == 0 && m3 == 0)))   // Loop goes on until either WBB or BBB condition is met
  599. {
  600.   if ((m1 == 1 && m2 == 0 && m3 == 1) || m1 == 0 && m2 == 1 && m3 == 0)   // WBW or BWB
  601.     {
  602.       forward();
  603.     }
  604.     else if (m1 == 0 && m2 == 1 && m3 == 1 )                              // BWW
  605.     {
  606.       left();
  607.  
  608.     }
  609.     else if (m1 == 1 && m2 == 1 && m3 == 0)                               // WWB
  610.     {
  611.       right();
  612.     }
  613.     else if (m1 == 1 && m2 == 1 && m3 == 1)                               // WWW
  614.     {
  615.       forward_mm(5);
  616.     }
  617.     whitelineshow();
  618.   }
  619.   forward_mm(5);
  620.   stopbot();
  621. }
  622.  
  623. /*
  624. *
  625. * Function Name      :  followlinetillbbworbbb
  626. * Input              :  None
  627. * Output             :  None
  628. * Logic              :  This function makes the robot follow the line till either BBW condition or BBB condition is detected by the sensors. This kind of function is useful when there is
  629.                         ambiguity on the kind of condition arising
  630. * Example Call       :  followlinetillbbworbbb();
  631. *
  632. */  
  633. void followlinetillbbworbbb()
  634. {
  635.   whitelineshow();
  636.   while (!((m1 == 1 && m2 == 0 && m3 == 0) || (m1 == 0 && m2 == 0 && m3 == 0)))    // Loop goes on until either BBW or BBB condition is met
  637. {
  638.   if ((m1 == 1 && m2 == 0 && m3 == 1) || m1 == 0 && m2 == 1 && m3 == 0)            // WBW or BWB
  639.     {
  640.       forward();
  641.     }
  642.     else if (m1 == 0 && m2 == 1 && m3 == 1 )                                       // BWW
  643.     {
  644.       left();
  645.  
  646.     }
  647.     else if (m1 == 1 && m2 == 1 && m3 == 0)                                        // WWB
  648.     {
  649.       right();
  650.     }
  651.     else if (m1 == 1 && m2 == 1 && m3 == 1)                                       // BBB
  652.     {
  653.       forward_mm(5);
  654.     }
  655.     whitelineshow();
  656.   }
  657.   forward_mm(5);
  658.   stopbot();
  659. }
  660.  
  661. /*
  662. *
  663. * Function Name      :  followlinetillbbworwbb
  664. * Input              :  None
  665. * Output             :  None
  666. * Logic              :  This function makes the robot follow the line till either BBW condition or WBB condition is detected by the sensors. This kind of function is useful when there is
  667.                         ambiguity on the kind of condition arising. Eg. Node
  668. * Example Call       :  followlinetillbbworwbb();
  669. *
  670. */  
  671. void followlinetillbbworwbb()
  672. {
  673.   whitelineshow();
  674.   while (!((m1 == 0 && m2 == 0 && m3 == 1) || (m1 == 1 && m2 == 0 && m3 == 0)))      // Loop goes on untill BBW or WBB condition is met.
  675.   {
  676.     if ((m1 == 1 && m2 == 0 && m3 == 1) || m1 == 0 && m2 == 1 && m3 == 0)            // WBW or BWB
  677.     {
  678.       forward();
  679.     }
  680.     else if (m1 == 0 && m2 == 1 && m3 == 1 )                                         // BWW
  681.     {
  682.       left();
  683.  
  684.     }
  685.     else if (m1 == 1 && m2 == 1 && m3 == 0)                                           // WWB
  686.     {
  687.       right();
  688.     }
  689.     else if ((m1 == 0 && m2 == 0 && m3 == 0) || (m1 == 1 && m2 == 1 && m3 == 1) )     // BBB or WWW
  690.     {
  691.       forward_mm(5);
  692.     }
  693.     whitelineshow();
  694.   }
  695.   forward_mm(5);
  696.   stopbot();
  697.  
  698. }
  699.  
  700. /*
  701. *
  702. * Function Name      :  servod
  703. * Input              :  degree
  704. * Output             :  None
  705. * Logic              :  This function generates a signal to direct the magnet dispenser servo to move to a certain angle.(Note:- We had problem using the arduino servo library and the motor
  706.                         functions together. So we created our own servo function)
  707. * Example Call       :  servod(90);
  708. *
  709. */  
  710. void servod(int degree)
  711. {
  712.  
  713.   int t = 10.05 * degree + 530;     // Mapping function for magnet dispenser servo. If we put the number of degrees in the equation, it generates the microsecond delay required for the servo
  714.   for (int i = 0; i < 5; i++)
  715.   {
  716.     digitalWrite(12, HIGH) ;
  717.     delayMicroseconds(t);
  718.     digitalWrite(12, LOW) ;
  719.     delay(20);
  720.   }
  721.  
  722. }
  723.  
  724. /*
  725. *
  726. * Function Name      :  servof
  727. * Input              :  degree
  728. * Output             :  None
  729. * Logic              :  This function generates a signal to direct the fire sensor servo to move to a certain angle.(Note:- We had problem using the arduino servo library and the motor
  730.                         functions together. So we created our own servo function)
  731. * Example Call       :  servof(90);
  732. *
  733. */  
  734. void servof(int n)
  735. {
  736.   int angle = 8.33 * n + 560;     // Mapping function for fire sensor servo. If we put the number of degrees in the equation, it generates the microsecond delay required for the servo
  737.   for (int i = 0; i < 2; i++)
  738.   {
  739.     digitalWrite(11, HIGH);
  740.     delayMicroseconds(angle);
  741.     digitalWrite(11, LOW);
  742.     delay(20);
  743.   }
  744. }
  745.  
  746. /*
  747. *
  748. * Function Name      :  drop_magnet
  749. * Input              :  magnet_number
  750. * Output             :  None
  751. * Logic              :  Upto four magnets are loaded in the magnet dispenser. Putting the magnet_number makes the servo rotate to a certain angle, hence dropping the magnet.
  752. * Example Call       :  drop_magnet(2);
  753. *
  754. */  
  755. void drop_magnet(int magnet_number)
  756. {
  757.  
  758.   switch (magnet_number)
  759.   {
  760.  
  761.     case 1: {                                              // Drop magnet 1
  762.         for (int i = 0; i < 40; i += 5)
  763.         {
  764.           servod(i);
  765.  
  766.         }
  767.         break;
  768.       }
  769.     case 2:  {                                             // Drop magnet 2
  770.         for (int i = 33; i <= 80; i += 5)
  771.         {
  772.           servod(i);
  773.  
  774.         }
  775.         break;
  776.       }
  777.     case 3:  {                                              // Drop magnet 3
  778.         for (int i = 77; i <= 120; i += 5)
  779.         {
  780.           servod(i);
  781.  
  782.         }
  783.         break;
  784.       }
  785.     case 4:  {                                              // Drop magnet 4
  786.         for (int i = 113; i <= 160; i += 5)
  787.         {
  788.           servod(i);
  789.  
  790.         }
  791.         break;
  792.       }
  793.     default: servod(0);                                      // Reset magnet dispenser servo to original position.
  794.   }
  795. }
  796.  
  797. /*
  798. *
  799. * Function Name      :  fire_detection
  800. * Input              :  room entrance
  801. * Output             :  None
  802. * Logic              :  This function first checks the value of room entrance flag. If room entrance flag is 0, then it means door position is d2 and fire servo sweeps clockwise from 80 to 180
  803.                         degrees searching for fire.When a fire is detected, it breaks out of the for loop and checks the position of fire by running a counter through a sequence of if-else conditions
  804.                         and returns a number according to position of fire with respect to the robots position. In case the room entrance flag = 1, the fire servo sweeps anticlockwise from 100 to 0
  805.                         degrees. In this case also the same process is followed.
  806. * Example Call       :  fire_detection(0);
  807. *
  808. */  
  809. int fire_detection( boolean room_entrance)
  810. {
  811.   int counter;
  812.   if( room_entrance == 0)                                                    // If door position is d2
  813.   {
  814.      servof(70);                                                             // Sets initial Servo angle
  815.      for( counter = 70 ; counter <= 180 ; counter++)
  816.      {
  817.        if( analogRead(A12) < 800 )                                           // A12 is pin on which fire sensor is connected. 800 is a threshold value under normal room lighting conditions. If analog value is less then 800, fire has been detected.
  818.        {
  819.          buzzer(1000);                                                       // Buzzer beep for 1 second
  820.          break;
  821.        }
  822.      }
  823.      
  824.      if( counter > 80 && counter <=115)
  825.      {
  826.        return 1;                                                             // Fire in front of robot.
  827.    }
  828.      else if( counter > 115 && counter <= 155)
  829.      {
  830.        return 2;                                                             // Fire is placed diagonally to the right of robot
  831.      }
  832.      else if( counter > 155 and counter < 180)
  833.      {
  834.        return 3;                                                             // Fire is placed on the right side of the robot
  835.      }
  836.      else
  837.      {
  838.        return 0;                                                             // No fire present
  839.      }
  840.      
  841.   }
  842.   else                                                                       // If door position is d1
  843.   {
  844.      servof(100);                                                            
  845.      for( counter = 100 ; counter >= 0 ; counter--)
  846.      {
  847.        if( analogRead(A12) < 800 )                                           // If fire is detected
  848.        {
  849.          buzzer(1000);                                                      // Buzzer beep for 1 second
  850.          break;
  851.        }
  852.      }
  853.      
  854.      if( counter > 0 && counter <=30)
  855.      {
  856.        return 3;                                                             // Fire is placed on the left side of the robot        
  857.      }
  858.      else if( counter > 30 && counter <= 60)
  859.      {
  860.        return 2;                                                             // Fire is placed diagonally to the left of robot
  861.      }
  862.      else if( counter > 60 and counter < 100)
  863.      {
  864.        return 1;                                                             // Fire in front of robot.
  865.      }
  866.      else
  867.      {
  868.        return 0;                                                             // No fire present
  869.      }
  870.   }
  871. }
  872.  
  873. /*
  874. *
  875. * Function Name      :  room_entrance_algo
  876. * Input              :  None
  877. * Output             :  None
  878. * Logic              :  Room_entrance_algo is called when the robot is at home. The robot moves forward and checks if there is a door. Depending on the condition, it either turns left and enters room
  879.                         in case of d2 or moves forward , turns left and then enters room in case of d1. In both cases the robot, moves till the first T-point in the room and then calls the fire detection
  880.                         algorithm and stores value returned in fire detection flag. Based on this the room traversal algorithm takes place                    
  881.                         degrees. In this case also the same process is followed.
  882. * Example Call       :  fire_detection(0);
  883. *
  884. */  
  885. void room_entrance_algo()
  886. {
  887.   forward_mm(550);                                    // Move forward 55cm
  888.   left_sharp = analogRead(A9);                        // The left sharp sensor is connected on pin A9. This line stores the value in the variable left sharp
  889.   if(left_sharp < 500)                                // If door is detected
  890.   {
  891.     delay(250);
  892.     room_entrance_flag = 0;                           // door is D2
  893.     left_degrees(90);                                 // Turn left 90 degrees.
  894.     forward_mm(120);                                  // Move forward 12 cm
  895.     catchline_left();                                 // Move soft left until robot catches line i.e center sensors detect black
  896.     followlinetillwbb();                              // Follow line until the first T-point in room is detected. The sensor should detect WBB when they are on this T-point.
  897.     fire_detection_flag = fire_detection(room_entrance_flag);  // Call fire detection algorithm and store returned value in fire detection flag.
  898.    
  899.   }
  900.   else                                                 // door is D1
  901.   {
  902.     delay(250);
  903.     forward_mm(400);                                   // Move forward 40cm more
  904.     left_degrees(100);                                 // Turn left 100 degrees
  905.     forward_mm(180);                                   // Move forward 18 cm
  906.     catchline_left();                                  // Move soft left until robot catches line
  907.     followlinetillbbw();                               // Follow line until first T-point in room is detected. The sensors should detect BBW condition here.
  908.     fire_detection_flag = fire_detection(room_entrance_flag);  // Call fire detection algorithm and store returned value in fire detection flag.
  909.    
  910.   }
  911.     room_traversal(room_entrance_flag, fire_detection_flag);
  912. }
  913.  
  914. /*
  915. *
  916. * Function Name      :  room_traversal
  917. * Input              :  room_entrance, fire_Detection
  918. * Output             :  None
  919. * Logic              :  This function takes values of 2 flags and based on this values the robot traverses the whole room using a sequence of steps                
  920. * Example Call       :  room_traversal(0,0);
  921. *
  922. */  
  923. void room_traversal(boolean room_entrance, int fire_detection)
  924. {
  925.   if(room_entrance == 0 && fire_detection == 0)                   // door D2 and no fire
  926.   {
  927.     right_degrees(180);                                           // Turn right 180 degrees
  928.     backward_mm(80);                                              // Move backward 8cm
  929.     velocity(f2, f2 - 5);                                         // Set velocity to f2
  930.     for (int i = 0; i < 3; i++)                                   // Iterate this loop 3 times(this is used to set the bot in orientation with the line)
  931.     {
  932.       catchline_right();                                          // Turn right till robot catches line
  933.       forward_mm(20);                                             // Move forward 2cm
  934.     }
  935.     forward_mm(300);                                              // Move forward 30cm
  936.     right_degrees(90);                                            // Turn right 90 degrees
  937.     forward_mm(230);                                              // Move forward 23cm
  938.     catchline_right();                                            // Turn right till robot catches line
  939.     velocity(f3, f3 - 5);                                         // Set velocity to f3
  940.     followlinetillbbb();                                          // Follow line till the next node ( black square at home)
  941.   }
  942.   else if(room_entrance == 0 && fire_detection == 1)              // door D2 and fire on front
  943.   {
  944.     right_degrees(20);                                            // Turn right 20 degrees
  945.     forward_mm(60);                                               // Move forward 6cm
  946.     catchline_left();                                             // move left till robot catches line
  947.     followlinetillbbworwbb();                                     // Follow line till the next node ( black square)
  948.     backward_mm(10);                                              // Move back 1cm to position magnet dispenser over deposition slot
  949.     drop_magnet(magnet_number);                                   // Drop next magnet
  950.     magnet_number++;
  951.     right_degrees(180);                                           // Turn sharp right 180 degrees
  952.     backward_mm(70);                                              // Move back 7cm
  953.     velocity(f4, f4-5);                                           // Set velocity to f2
  954.     catchline_right();                                            // Move right till robot catches right
  955.     followlinetillbbworwbb();                                     // Follow line till next node
  956.     forward_mm(10);                                               // Move 1 cm forward
  957.     followlinetillbbworwbb();                                     // Followline till next node or T-point
  958.     velocity(f5, f5 - 5);                                         // Set velocity to f5
  959.     forward_mm(550);                                              // Move forward 55cm to exit room
  960.     right_degrees(90);                                            // Turn right 90 degrees
  961.     forward_mm(210);                                              // Move forward 21cm
  962.     catchline_right();                                            // Move right till robot catches line
  963.     velocity(f4, f4 - 5);                                         // Set velocity to f3
  964.     followlinetillbbb();                                          // Followline till robot returns to home/ center node
  965.   }
  966.   else if(room_entrance == 0 && fire_detection == 2)              // door D2 and fire diagonally to the right
  967.   {
  968.     right_degrees(20);                                            // Turn right 20 degrees
  969.     forward_mm(60);                                               // Move forward 6cm
  970.     catchline_left();                                             // Move left to catch line
  971.     velocity(f4, f4-5);                                           // Set velocity to f4
  972.     followlinetillbbworwbb();                                     // Follow line till next node
  973.     forward_mm(55);                                               // Move forward 5.5cm                          
  974.     soft_right_degrees(180);                                      // Turn soft right 180 degree
  975.     forward_mm(40);                                               // Move forward 4cm
  976.     catchline_left();                                             // Catch line left
  977.     followlinetillbbworwbb();                                     // Move till next node
  978.     backward_mm(10);                                              // Move back 1cm to position magnet dispenser
  979.     drop_magnet(magnet_number);                                               // Drop magnet
  980.     magnet_number++;
  981.     right_degrees(220);                                           // Turn right 220 degree
  982.     catchline_left();                                             // Catch line to the left
  983.     followlinetillbbworwbb();                                     // Follow line till next node
  984.     forward_mm(55);                                               // Forward 5.5cm
  985.     soft_left_degrees(200);                                       // Turn left
  986.     backward_mm(30);                                              // Move back 3cm
  987.     catchline_right();                                            
  988.     followlinetillbbworwbb();                                     // Follow line till next node
  989.     velocity(f5,f5-5 );                                           // Set velocity to f5
  990.     forward_mm(575);                                              // Move forward to exit room
  991.     right_degrees(90);                                            // Turn right 90 degrees
  992.     forward_mm(210);                                              // Forward 21cm
  993.     catchline_right();                                            // Catch line right
  994.     velocity(f4, f4-5 );                                          // Set velocity to f4
  995.     followlinetillbbb();                                          // Follow line till robot reaches home node
  996.   }
  997.   else if(room_entrance == 0 && fire_detection == 3)              // door D2 and fire on the right side
  998.   {
  999.     right_degrees(100);                                           // Turn right 100 degrees
  1000.     forward_mm(60);                                               // Move forward 6cm
  1001.     catchline_left();                                             // move left till robot catches line
  1002.     followlinetillbbworwbb();                                     // Follow line till the next node ( black square)
  1003.     backward_mm(10);                                              // Move back 1cm to position magnet dispenser over deposition slot
  1004.     drop_magnet(magnet_number);                                               // Drop next magnet
  1005.     magnet_number++;
  1006.     left_degrees(180);                                            // Turn sharp left 180 degrees
  1007.     backward_mm(70);                                              // Move back 7cm
  1008.     velocity(f2, f2 - 5);                                         // Set velocity to f2
  1009.     catchline_left();                                             // Move left till robot catches left
  1010.     followlinetillbbworwbb();                                     // Follow line till next node
  1011.     forward_mm(10);                                               // Move 1 cm forward
  1012.     followlinetillbbworwbb();                                     // Followline till next node or T-point
  1013.     left_degrees(90);                                             // Turn sharp right 90 degrees
  1014.     forward_mm(300);                                              // Move forward 30cm
  1015.     left_degrees(90);                                             // Turn left 90 degrees
  1016.     forward_mm(230);                                              // Move forward 23cm
  1017.     catchline_right();                                            // Turn right till robot catches line
  1018.     velocity(f3, f3 - 5);                                         // Set velocity to f3
  1019.     followlinetillbbb();                                          // Follow line till the next node ( black square at home)
  1020.   }
  1021.   else if(room_entrance == 1 && fire_detection == 0)              // door D1 and no fire      
  1022.   {
  1023.     left_degrees(180);                                            // Turn left 180 degrees
  1024.     backward_mm(80);                                              // Move backward 8cm
  1025.     velocity(f2, f2 - 5);                                         // Set velocity to f2
  1026.     for (int i = 0; i < 3; i++)                                   // Iterate this loop 3 times(this is used to set the bot in orientation with the line)
  1027.     {
  1028.       catchline_left();                                           // Turn left till robot catches line
  1029.       forward_mm(20);                                             // Move forward 2cm
  1030.     }
  1031.     forward_mm(300);                                              // Move forward 30cm
  1032.     right_degrees(90);                                            // Turn right 90 degrees
  1033.     forward_mm(760);                                              // Move forward 76cm
  1034.     catchline_right();                                            // Turn right till robot catches line
  1035.     velocity(f3, f3 - 5);                                         // Set velocity to f3
  1036.     followlinetillbbb();                                          // Follow line till the next node ( black square at home)
  1037.   }
  1038.   }
  1039.   else if(room_entrance == 1 && fire_detection == 1)              // door D1 and fire on front
  1040.   {
  1041.     left_degrees(20);                                             // Turn left 20 degrees
  1042.     forward_mm(60);                                               // Move forward 6cm
  1043.     catchline_right();                                            // Move right till robot catches line
  1044.     followlinetillbbworwbb();                                     // Follow line till the next node ( black square)
  1045.     backward_mm(10);                                              // Move back 1cm to position magnet dispenser over deposition slot
  1046.     drop_magnet(magnet_number);                                               // Drop next magnet
  1047.     magnet_number++
  1048.     left_degrees(180);                                            // Turn sharp left 180 degrees
  1049.     backward_mm(70);                                              // Move back 7cm
  1050.     velocity(f2, f2-5);                                           // Set velocity to f2
  1051.     catchline_left();                                             // Move left till robot catches right
  1052.     followlinetillbbworwbb();                                     // Follow line till next node
  1053.     forward_mm(10);                                               // Move 1 cm forward
  1054.     followlinetillbbworwbb();                                     // Followline till next node or T-point
  1055.     velocity(f5, f5 - 5);                                         // Set velocity to f5
  1056.     forward_mm(550);                                              // Move forward 55cm to exit room
  1057.     right_degrees(90);                                            // Turn right 90 degrees
  1058.     forward_mm(760);                                              // Move forward 95cm
  1059.     catchline_right();                                            // Move right till robot catches line
  1060.     velocity(f3, f3 - 5);                                         // Set velocity to f3
  1061.     followlinetillbbb();                                          // Followline till robot returns to home/ center node
  1062.   }
  1063.   else if(room_entrance == 1 && fire_detection == 2)              // door D1 and fire diagonally to the left
  1064.   {
  1065.     left_degrees(20);                                             // Turn left 20 degrees
  1066.     forward_mm(60);                                               // Move forward 6cm
  1067.     catchline_right();                                            // Move right to catch line
  1068.     velocity(f4, f4-5);                                           // Set velocity to f4
  1069.     followlinetillbbworwbb();                                     // Follow line till next node
  1070.     forward_mm(55);                                               // Move forward 5.5cm                          
  1071.     soft_left_degrees(180);                                       // Turn soft left 180 degree
  1072.     forward_mm(40);                                               // Move forward 4cm
  1073.     catchline_right();                                            // Catch line right
  1074.     followlinetillbbworwbb();                                     // Move till next node
  1075.     backward_mm(10);                                              // Move back 1cm to position magnet dispenser
  1076.     drop_magnet(a);                                               // Drop magnet
  1077.     left_degrees(220);                                            // Turn left 220 degree
  1078.     catchline_right();                                            // Catch line to the left
  1079.     followlinetillbbworwbb();                                     // Follow line till next node
  1080.     forward_mm(55);                                               // Forward 5.5cm
  1081.     soft_right_degrees(200);                                      // Turn right
  1082.     backward_mm(30);                                              // Move back 3cm
  1083.     catchline_right();                                            
  1084.     followlinetillbbworwbb();                                     // Follow line till next node
  1085.     velocity(f5,f5-5 );                                           // Set velocity to f5
  1086.     forward_mm(575);                                              // Move forward to exit room
  1087.     right_degrees(90);                                            // Turn right 90 degrees
  1088.     forward_mm(760);                                              // Forward 76cm
  1089.     catchline_right();                                            // Catch line right
  1090.     velocity(f4, f4-5 );                                          // Set velocity to f4
  1091.     followlinetillbbb();                                          // Follow line till robot reaches home node
  1092.   }
  1093.   else if(room_entrance == 1 && fire_detection == 3)              // door D1 and fire on the left side
  1094.   {
  1095.     left_degrees(100);                                            // Turn left 100 degrees
  1096.   forward_mm(60);                                               // Move forward 6cm
  1097.   catchline_right();                                            // move right till robot catches line
  1098.   followlinetillbbworwbb();                                     // Follow line till the next node ( black square)
  1099.   backward_mm(10);                                              // Move back 1cm to position magnet dispenser over deposition slot
  1100.   drop_magnet(magnet_number);                                               // Drop next magnet
  1101.   magnet_number++;
  1102.   right_degrees(180);                                           // Turn sharp right 180 degrees
  1103.   backward_mm(70);                                              // Move back 7cm
  1104.   velocity(f2, f2 - 5);                                         // Set velocity to f2
  1105.   catchline_right();                                            // Move right till robot catches right
  1106.   followlinetillbbworwbb();                                     // Follow line till next node
  1107.   forward_mm(10);                                               // Move 1 cm forward
  1108.   followlinetillbbworwbb();                                     // Followline till next node or T-point
  1109.   left_degrees(90);                                             // Turn sharp right 90 degrees
  1110.   forward_mm(300);                                              // Move forward 30cm
  1111.   right_degrees(90);                                            // Turn right 90 degrees
  1112.   forward_mm(760);                                              // Move forward 76cm
  1113.   catchline_right();                                            // Turn right till robot catches line
  1114.   velocity(f3, f3 - 5);                                         // Set velocity to f3
  1115.   followlinetillbbb();                                          // Follow line till the next node ( black square at home)
  1116.   }
  1117. }
  1118.  
  1119. /*
  1120. *
  1121. * Function Name      :  buzzer
  1122. * Input              :  n
  1123. * Output             :  None
  1124. * Logic              :  This function takes number of milliseconds and beeps buzzer for that duration          
  1125. * Example Call       :  buzzer(1000);
  1126. *
  1127. */  
  1128. void buzzer(int n)
  1129. {
  1130.   digitalWrite(34, HIGH);
  1131.   delay(n);
  1132.   digitalWrite(34, LOW);
  1133. }
  1134.  
  1135. /*
  1136. *
  1137. * Function Name      :  setup
  1138. * Input              :  None
  1139. * Output             :  None
  1140. * Logic              :  This function initialises all the pins and interrupts and the main code also takes place here                
  1141. * Example Call       :  N/A
  1142. *
  1143. */  
  1144. void setup()
  1145. {
  1146.   pinMode(36, OUTPUT);    
  1147.   digitalWrite(36, 0);                        //Pin on which LCD RW pin in connected. It is set as LOW
  1148.   lcd.begin(16, 2);
  1149.   pinMode(34, OUTPUT);
  1150.   pinMode(11, OUTPUT);                       // Pin on which fire servo is connected
  1151.   pinMode(12, OUTPUT);                       // Pin on which magnet dispenser servo is connected
  1152.   motor_init();
  1153.  
  1154.   attachInterrupt(1, incrementright , FALLING);    //Right encoder interrupt attach
  1155.   attachInterrupt(0, incrementleft, FALLING);      // Left encoder interupt attach
  1156.  
  1157.   servod(0);                                     // Reset both servos
  1158.   servof(0);
  1159.  
  1160.   /******************************************************************************************/
  1161.  
  1162.   room_entrance_algo();   // Traverse first room
  1163.   room_entrance_algo();   // Traverse second room
  1164.   forward_mm(60);
  1165.   right_degrees(100);
  1166.   room_entrance_algo();   // Traverse third room
  1167.   room_entrance_algo();   // Traverse fourth room
  1168.   buzzer(5000);
  1169. }
  1170.  
  1171. /*
  1172. *
  1173. * Function Name      :  loop
  1174. * Input              :  None
  1175. * Output             :  None
  1176. * Logic              :  No code is used here            
  1177. * Example Call       :  N/A
  1178. *
  1179. */
  1180. void loop()
  1181. {
  1182. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
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
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!