Advertisement
RichPyke

IR Roam V1.0.0

Jul 26th, 2013
173
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 9.54 KB | None | 0 0
  1. # IR Object Avoidance
  2. # Using IR Sensor on servo for avoidance
  3. # Based on Ping Roam V1.1.2
  4. # Version 1.0.0
  5. # Updated 2013-07-26
  6. # Author Rich
  7. # Software EZ-Builder 2013-07-24
  8. # www.EZ-Robot.com
  9.  
  10.  
  11. # Configuration Settings
  12. # This script assumes the following ports;
  13. # IR Sensor = ADC0
  14. # Sweep Servo = D2
  15.  
  16.  
  17. # Adjust values below for movement control
  18. $maxdistance = 30 # Change for closest distance to object before avoiding in units
  19. $boxedindistance = 50 # Change for closest distance to object for boxed in detection
  20. $turnamount = 500 # Change for how long to turn for in ms
  21. $reverseamount = 500 # Change for how long to reverse for in ms (if applicable)
  22. $movementspeed = 255 # Change for movement speed
  23. $slowturn = 127 # Change for slow turn speed
  24.  
  25.  
  26. # Adjust values below for sweep configuration
  27. $sweepmin = 10 # Change for min limit
  28. $sweepmax = 90 # Change for max limit
  29. $sweepservodelay = 500 # Change for delay between sweep servo movements and readings
  30.  
  31.  
  32. # --------------------- Do not adjust these values ---------------------
  33. # Calculate the centre position for the sweeping servo
  34. # Minimum position plus maximum position divided by 2
  35. # Round to 0 decimal places
  36. $sweepcenter = Round((($sweepmin+$sweepmax)/2),0)
  37. # Set the previous position to the max position for initial movement
  38. $sweepprevious = $sweepmax # Do not change
  39. # Set the current position to the centre position for initial movement
  40. $sweepcurrent = $sweepcenter # Do not change
  41. $isboxedin = 0 # Do not change
  42. # Set last and penultimate moves.
  43. $penultimatemove = "none"
  44. $lastmove = "none"
  45. # ----------------------------------------------------------------------
  46.  
  47.  
  48. # ------- The Script --------
  49. # Center the sweep servo
  50. Servo(D2, $sweepcenter)
  51.  
  52. # Start moving forwards
  53. FORWARD()
  54.  
  55.  
  56. # Detection code
  57. # Set a label for loops and gotos
  58. :detect
  59.  
  60. # Get the current distance
  61. $currentdistance = GetADC(ADC0)
  62.  
  63. # Check the current distance against the max allowed distance
  64. IF ($currentdistance >= $maxdistance)
  65. # If the current distance is above the max distance start avoiding
  66. GOTO(avoid)
  67.  
  68. # Set label for avoid return to avoid return errors
  69. :avoidreturn
  70. ENDIF
  71.  
  72. # Run the sweeping servo code
  73. GOTO(sweep)
  74.  
  75. # Wait
  76. SLEEP ($sweepservodelay)
  77.  
  78. # Loop back to the start of detection
  79. GOTO(detect)
  80.  
  81.  
  82.  
  83. # Avoidance code
  84.  
  85. # Set a label for loops and gotos
  86. :avoid
  87.  
  88. # First check if boxed in
  89. Goto(boxedin)
  90.  
  91. # If the robot is boxed in run the escape code
  92. IF ($isboxedin = 1)
  93. Goto(escape)
  94.  
  95. # Avoid return error after escape loop by setting a label for a goto
  96. :escapereturn
  97.  
  98. # Change to else to avoid reversing and turning after an escape
  99. ELSE
  100.  
  101. # Stop and reverse
  102. Stop()
  103. Sleep(200)
  104. Reverse($movementspeed,$reverseamount)
  105.  
  106. # Check the servo position
  107. # Check if it's to the left
  108. IF ($sweepcurrent = $sweepmin)
  109. # If the servo is in the lowest position (left) move right
  110. Goto(moveright)
  111. # Continue moving forwards
  112. # Add in a stop and sleep before movement
  113. Stop()
  114. Sleep(200)
  115. FORWARD()
  116.  
  117. # Else check if it's to the right
  118. ELSEIF ($sweepcurrent = $sweepmax)
  119. # If the servo is in the highest position (right) move left
  120. Goto(moveleft)
  121. # Continue moving forwards
  122. # Add in a stop and sleep before movement
  123. Stop()
  124. Sleep(200)
  125. FORWARD()
  126.  
  127. # Else assume it's in the middle
  128. ELSE
  129. # If the servo is in the center position check which side is closest to the object and move the other way
  130.  
  131. # Move and check the left side
  132. Servo(D2,$sweepmin)
  133.  
  134. # Get the current distance
  135. $irmin = GetADC(ADC0)
  136.  
  137. # Wait
  138. Sleep(400)
  139.  
  140. # Move and check the right side
  141. Servo(D2,$sweepmax)
  142.  
  143. # Get the current distance
  144. $irmax = GetADC(ADC0)
  145.  
  146. # Wait
  147. Sleep(400)
  148.  
  149. # Move and check the center
  150. Servo(D2,$sweepcenter)
  151. IF ($irmin < $irmax)
  152. Goto(moveright)
  153. # Add in a stop and sleep before movement
  154. Stop()
  155. Sleep(200)
  156. FORWARD()
  157. ELSE
  158. Goto(moveleft)
  159. # Add in a stop and sleep before movement
  160. Stop()
  161. Sleep(200)
  162. FORWARD()
  163. ENDIF
  164. ENDIF
  165. ENDIF
  166.  
  167. # Return to the main code
  168. Goto(avoidreturn)
  169.  
  170.  
  171.  
  172. # The sweep code
  173.  
  174. # Set a label for loops and gotos
  175. :sweep
  176.  
  177. # Move in the correct direction and store previous position
  178.  
  179. # Check what the current position is
  180. # Check if left
  181. IF ($sweepcurrent = $sweepmin)
  182.  
  183. # Save the current position as the previous
  184. $sweepprevious = $sweepcurrent
  185.  
  186. # Move to the next position
  187. Servo(D2, $sweepcenter)
  188.  
  189. # Save the current position
  190. $sweepcurrent = GetServo(D2)
  191.  
  192. # Else check if its center and where it was before
  193. # If it is center and was left before
  194. ELSEIF ($sweepcurrent = $sweepcenter and $sweepprevious = $sweepmin)
  195.  
  196. # Save the current position as the previous
  197. $sweepprevious = $sweepcurrent
  198.  
  199. # Move to the next position
  200. Servo(D2, $sweepmax)
  201.  
  202. # Save the current position
  203. $sweepcurrent = GetServo(D2)
  204.  
  205. # If it is center and was right before
  206. ELSEIF ($sweepcurrent = $sweepcenter and $sweepprevious = $sweepmax)
  207.  
  208. # Save the current position as the previous
  209. $sweepprevious = $sweepcurrent
  210.  
  211. # Move to the next position
  212. Servo(D2, $sweepmin)
  213.  
  214. # Save the current position
  215. $sweepcurrent = GetServo(D2)
  216.  
  217. # Else check if right
  218. ELSEIF ($sweepcurrent = $sweepmax)
  219.  
  220. # Save the current position as the previous
  221. $sweepprevious = $sweepcurrent
  222.  
  223. # Move to the next position
  224. Servo(D2, $sweepcenter)
  225.  
  226. # Save the current position
  227. $sweepcurrent = GetServo(D2)
  228.  
  229. ENDIF
  230.  
  231. # Return back to the main script
  232. Return()
  233.  
  234.  
  235.  
  236. # The sweep center code
  237.  
  238. # Set a label for loops and gotos
  239. :sweepcenter
  240.  
  241. # Move the servo to the left position
  242. Servo(D2,$sweepmin)
  243.  
  244. # Get the current distance
  245. $irmin = GetADC(ADC0)
  246.  
  247. # Wait
  248. Sleep(200)
  249.  
  250. # Move the servo to the right
  251. Servo(D2,$sweepmax)
  252.  
  253. # Get the current distance
  254. $irmax = GetADC(ADC0)
  255.  
  256. # Wait
  257. Sleep(200)
  258.  
  259. # Move the servo back to the center
  260. Servo(D2,$sweepcenter)
  261.  
  262. # Check which side has the closest object
  263. # If the object to the left is further away than the object to the right
  264. IF ($irmin < $irmax)
  265.  
  266. # Move to the right
  267. RIGHT($movementspeed,$turnamount)
  268.  
  269. # Move forwards again
  270. # Add in a stop and sleep before movement
  271. Stop()
  272. Sleep(200)
  273. FORWARD()
  274.  
  275. # Else if the object to the right is further away than the object to the left
  276. ELSEIF ($irmin > $irmax)
  277.  
  278. # Move to the left
  279. LEFT($movementspeed,$turnamount)
  280.  
  281. # Move forwards again
  282. # Add in a stop and sleep before movement
  283. Stop()
  284. Sleep(200)
  285. FORWARD()
  286.  
  287. # Else they are both the same
  288. ELSE
  289.  
  290. # So move left - this can be customised
  291. LEFT($movementspeed,$turnamount)
  292.  
  293. # And move forwards again
  294. # Add in a stop and sleep before movement
  295. Stop()
  296. Sleep(200)
  297. FORWARD()
  298. ENDIF
  299.  
  300. # Return to the main code
  301. Return()
  302.  
  303.  
  304.  
  305. # The boxed in code
  306.  
  307. # Set a label for loops and gotos
  308. :boxedin
  309.  
  310. # Get distance to the side
  311. # Move the servo to the left
  312. Servo(D2,$sweepmin)
  313.  
  314. # Get the current distance
  315. $side1scan = GetADC(ADC0)
  316.  
  317. # Get distance to the other side
  318. # Move the servo to the right
  319. Servo(D2,$sweepmax)
  320.  
  321. # Get the current distance
  322. $side2scan = GetADC(ADC0)
  323.  
  324. # Get distance to the front
  325. # Move the servo to the center
  326. Servo(D2,$sweepcenter)
  327.  
  328. # Get the current distance
  329. $centerscan = GetADC(ADC0)
  330.  
  331. # Check if boxed in by compairing the results against a fixed boxed in distance
  332. IF ($side1scan > $boxedindistance and $side2scan > $boxedindistance and $centerscan > $boxedindistance)
  333.  
  334. # If any are true set the boxed in flag
  335. $isboxedin = 1
  336.  
  337. ENDIF
  338.  
  339. # Return to the main script
  340. Return()
  341.  
  342.  
  343.  
  344. # The escape code
  345.  
  346. # Set a label for loops and gotos
  347. :escape
  348.  
  349. # Reset the boxed in flag
  350. $isboxedin = 0
  351.  
  352. # Center the sweep servo
  353. Servo(D2,$sweepcenter)
  354.  
  355. # Turn slowly
  356. Left($slowturn)
  357.  
  358. # Set up a loop
  359. :escapeloop
  360.  
  361. # Scan until clear
  362. # Get the current distance
  363. $escapescan = GetADC(ADC0)
  364.  
  365. # If the scan result is above the boxed in distance loop
  366. IF ($escapescan > $BoxedInDistance)
  367.  
  368. # Go back to the start of the escape loop
  369. Goto(escapeloop)
  370.  
  371. ENDIF
  372.  
  373. # Continue forwards
  374. # Add in a stop and sleep before movement
  375. Stop()
  376. Sleep(200)
  377. FORWARD()
  378.  
  379. # Return to the main script
  380. Goto(escapereturn)
  381.  
  382.  
  383.  
  384. # Move Right code
  385.  
  386. # Set a label for loops and gotos
  387. :moveright
  388.  
  389. # Check the last 2 moves to avoid left right left right loops
  390. IF ($lastmove = "left" and $penultimatemove = "right")
  391.  
  392. # If it has been right then left dont move right again but escape from a loop
  393. Goto(escape)
  394.  
  395. # Reset the last move
  396. $lastmove = "none"
  397.  
  398. # Else just move right
  399. ELSE
  400. RIGHT($movementspeed,$turnamount)
  401. Sleep(200)
  402.  
  403. # Save the penultimate move
  404. $penultimatemove = $lastmove
  405.  
  406. # Save the last move
  407. $lastmove = "right"
  408.  
  409. ENDIF
  410.  
  411. # Go back to the main script
  412. Return()
  413.  
  414.  
  415.  
  416. # Move left code
  417.  
  418. # Set a label for loops and gotos
  419. :moveleft
  420.  
  421. # Check the last 2 moves to avoid left right left right loops
  422. IF ($lastmove = "right" and $penultimatemove = "left")
  423.  
  424. # If it has been left then right dont move left afain but escape from a loop
  425. Goto(escape)
  426.  
  427. # Reset the last move
  428. $lastmove = "none"
  429.  
  430. # Else just move left
  431. ELSE
  432. LEFT($movementspeed,$turnamount)
  433. Sleep(200)
  434.  
  435. # Save the penultimate move
  436. $penultimatemove = $lastmove
  437.  
  438. # Save the last move
  439. $lastmove= "left"
  440.  
  441. ENDIF
  442.  
  443. # Go back to the main script
  444. Return()
  445.  
  446. # End of scripts
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement