Advertisement
RichPyke

IR Roam V0.0.1 Alpha

Apr 20th, 2013
187
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 7.54 KB | None | 0 0
  1. # IR Object Avoidance
  2. # Using 3xIR sensors
  3. # Version 0.0.1 Alpha
  4. # 2013.04.22
  5. # Author: Rich Pyke
  6. # Language: EZ-Script
  7.  
  8. # Use at your own risk. This code is untested.
  9.  
  10.  
  11. # Configuration Settings
  12.  
  13. # Adjust values below for port configuration
  14. $leftir = ADC5
  15. $rightir = ADC6
  16. $centerir = ADC7
  17.  
  18. # Adjust values below for movement control
  19. $reverseturn = 1 # Reverse before turn? 0 = no, 1 = yes
  20. $maxdistance = 30 # Change for maximum distance from object before avoiding in units
  21. $boxedindistance = 20 # Change for maximum distance for boxed in detection
  22. $turnamount = 500 # Change for how long to turn for in ms
  23. $reverseamount = 500 # Change for how long to reverse for in ms (if applicable)
  24. $movementspeed = 255 # Change for movement speed
  25. $slowturn = 70 # Change for slow turn speed
  26. $boxedinturntime = 250 # Change for how long to turn when checking if boxed in
  27. $thinkdelay = 200 # Change for how long to sleep emulating thinking and reducing load
  28.  
  29. # Adjust values below for scan configuration
  30. $scandelay = 500 # Change for delay between readings
  31.  
  32. # Testing Options
  33. $testmode = 1 # Change to 1 if testing without sensors
  34.  
  35.  
  36.  
  37. # Do not adjust these values
  38. $SweepErrorFlag = 0 # Do not change
  39. $BoxedInErrorFlag = 0 # Do not change
  40. $BoxedInRun = 0 # Do not change
  41. $EscapeRun = 0 # Do not change
  42. $ScriptErrorFlag = 0 # Do not change
  43. $isboxedin = 0 # Do not change
  44. $penultimatemove = "none"
  45. $lastmove = "none"
  46.  
  47.  
  48.  
  49. # ------- The Script --------
  50.  
  51. # Set the start point for any loops
  52. :begin
  53.  
  54. # Start moving forwards
  55. FORWARD($movementspeed)
  56.  
  57. # Start the detection
  58. Goto(detect) # This line is redundant
  59.  
  60.  
  61.  
  62. # Detection code
  63.  
  64. # Set a label for loops and gotos
  65. :detect
  66.  
  67. # Get the sensor readings
  68. Goto(getreadings)
  69.  
  70. # Check the current distance against the max allowed distance
  71. IF ($currentdistanceleft >= $maxdistance or $currentdistanceright >= $maxdistance or $currentdistancecenter >= $maxdistance)
  72. # If the current distance is higher the max distance start avoiding
  73. GOTO(avoid)
  74.  
  75. # Set label for avoid return to avoid return errors
  76. :avoidreturn
  77. ENDIF
  78.  
  79. # Wait
  80. SLEEP ($scandelay)
  81.  
  82. # Loop back to the start of detection
  83. GOTO(detect)
  84.  
  85.  
  86.  
  87. # Avoidance code
  88.  
  89. # Set a label for loops and gotos
  90. :avoid
  91.  
  92. # First check if boxed in
  93. Goto(boxedin)
  94.  
  95. # If the robot is boxed in run the escape code
  96. IF ($isboxedin = 1)
  97. Goto(escape)
  98.  
  99. # Avoid return error after escape loop by setting a label for a goto
  100. :escapereturn
  101. ELSE
  102.  
  103. # Check to see if to reverse before turning
  104. IF ($reverseturn = 1)
  105.  
  106. # If the option of reverse before turning is set reverse
  107. Stop()
  108. Sleep($thinkdelay)
  109. Reverse($movementspeed,$reverseamount)
  110. ENDIF
  111.  
  112. # Check the closest side
  113. # Check if it's to the left
  114. IF ($currentdistanceleft > $currentdistanceright and $currentdistanceleft > $currentdistancecenter)
  115. # If left move right
  116. Goto(moveright)
  117. # Continue moving forwards
  118. Stop()
  119. Sleep($thinkdelay)
  120. FORWARD($movementspeed)
  121.  
  122. # Else check if it's to the right
  123. ELSEIF ($currentdistanceright > $currentdistanceleft and $currentdistanceright > $currentdistancecenter)
  124. # If right move left
  125. Goto(moveleft)
  126. # Continue moving forwards
  127. Stop()
  128. Sleep($thinkdelay)
  129. FORWARD($movementspeed)
  130.  
  131. # Else assume it's in the middle
  132. ELSE
  133. # If it is in the center check which side is closest to the object and move the other way
  134. # Check which is lowest and move the other way
  135. IF ($currentdistanceleft > $currentdistanceright)
  136. Goto(moveright)
  137. Stop()
  138. Sleep($thinkdelay)
  139. FORWARD()
  140. ELSE
  141. Goto(moveleft)
  142. Stop()
  143. Sleep($thinkdelay)
  144. FORWARD()
  145. ENDIF
  146. ENDIF
  147. ENDIF
  148. # Return to the main code
  149. Goto(avoidreturn)
  150.  
  151.  
  152.  
  153. # The boxed in code
  154.  
  155. # Set a label for loops and gotos
  156. :boxedin
  157.  
  158. # Set a flag so we know it has run for debugging
  159. $BoxedInRun = 1
  160.  
  161. # Get distance to the side
  162. # Move to the left
  163. Left($movementspeed,$boxedinturntime)
  164.  
  165. # Use random numbers if in test mode
  166. IF ($testmode=0)
  167. $leftscan = GetADC($centerir)
  168. ELSE
  169. $leftscan = GetRandom(0,255)
  170. ENDIF
  171.  
  172. # Get distance to the other side
  173. # Move to the right
  174. Right($movementspeed,$boxedinturntime)
  175. # Do it twice to get past the center
  176. Right($movementspeed,$boxedinturntime)
  177.  
  178. # Use random numbers if in test mode
  179. IF ($testmode=0)
  180. $rightscan = GetADC($centerir)
  181. ELSE
  182. $rightscan = GetRandom(0,255)
  183. ENDIF
  184.  
  185. # Get distance to the front
  186. # Move to the center
  187. Left($movementspeed,$boxedinturntime)
  188.  
  189. # Use random numbers if in test mode
  190. IF ($testmode=0)
  191. $centerscan = GetADC($centerir)
  192. ELSE
  193. $centerscan = GetRandom(0,255)
  194. ENDIF
  195.  
  196. # Check if boxed in by comparing the results against a fixed boxed in distance
  197. IF ($leftscan > $boxedindistance and $rightscan > $boxedindistance and $centerscan > $boxedindistance)
  198.  
  199. # If any are true set the boxed in flag
  200. $isboxedin = 1
  201.  
  202. ENDIF
  203.  
  204. # Return to the main script
  205. Return()
  206.  
  207.  
  208.  
  209. # The escape code
  210.  
  211. # Set a label for loops and gotos
  212. :escape
  213.  
  214. # Set a flag so we know it has run for debugging
  215. $EscapeRun = 1
  216.  
  217. # Reset the boxed in flag
  218. $isboxedin = 0
  219.  
  220. # Turn slowly
  221. Left($slowturn)
  222.  
  223. # Set up a loop
  224. :escapeloop
  225.  
  226. # Scan until clear
  227. # Use random numbers if in test mode
  228. IF ($testmode=0)
  229. $escapescan = GetADC($centerir)
  230. ELSE
  231. $escapescan = GetRandom(0,255)
  232. ENDIF
  233.  
  234. # If the scan result is below the boxed in distance loop
  235. IF ($escapescan > $BoxedInDistance)
  236.  
  237. # Pause to reduce resource usage and comms bottleneck
  238. Sleep(100)
  239.  
  240. # Go back to the start of the escape loop
  241. Goto(escapeloop)
  242.  
  243. ENDIF
  244.  
  245. # Continue forwards
  246. Stop()
  247. Sleep($thinkdelay)
  248. FORWARD($movementspeed)
  249.  
  250. # Return to the main script
  251. Goto(escapereturn)
  252.  
  253.  
  254.  
  255. # Move Right code
  256.  
  257. # Set a label for loops and gotos
  258. :moveright
  259.  
  260. # Check the last 2 moves to avoid left right left right loops
  261. IF ($lastmove = "left" and $penultimatemove = "right")
  262.  
  263. # If it has been right then left dont move right again but escape from a loop
  264. Goto(escape)
  265.  
  266. # Reset the last move
  267. $lastmove = "none"
  268.  
  269. # Else just move right
  270. ELSE
  271. RIGHT($movementspeed,$turnamount)
  272.  
  273. # Save the penultimate move
  274. $penultimatemove = $lastmove
  275.  
  276. # Save the last move
  277. $lastmove = "right"
  278.  
  279. ENDIF
  280.  
  281. # Go back to the main script
  282. Return()
  283.  
  284.  
  285.  
  286. # Move left code
  287.  
  288. # Set a label for loops and gotos
  289. :moveleft
  290.  
  291. # Check the last 2 moves to avoid left right left right loops
  292. IF ($lastmove = "right" and $penultimatemove = "left")
  293.  
  294. # If it has been left then right dont move left again but escape from a loop
  295. Goto(escape)
  296.  
  297. # Reset the last move
  298. $lastmove = "none"
  299.  
  300. # Else just move left
  301. ELSE
  302. LEFT($movementspeed,$turnamount)
  303.  
  304. # Save the penultimate move
  305. $penultimatemove = $lastmove
  306.  
  307. # Save the last move
  308. $lastmove= "left"
  309.  
  310. ENDIF
  311.  
  312. # Go back to the main script
  313. Return()
  314.  
  315.  
  316.  
  317. # Get readings code
  318.  
  319. :getreadings
  320. # Use random numbers if in test mode
  321. IF ($testmode=0)
  322. $currentdistanceleft = GetADC($leftir)
  323. $currentdistanceright = GetADC($rightir)
  324. $currentdistancecenter = GetADC($centerir)
  325. ELSE
  326. $currentdistanceleft = GetRandom(0,255)
  327. $currentdistanceright = GetRandom(0,255)
  328. $currentdistancecenter = GetRandom(0,255)
  329. ENDIF
  330.  
  331. # Go back to the main script
  332. Return()
  333.  
  334.  
  335.  
  336. # Camera Tracking Code
  337.  
  338. :glyphdetection
  339. # Check if glyph has been tracked
  340. IF ($cameraistracking = 1)
  341. # Do something - not sure what yet
  342. ENDIF
  343.  
  344. # Go back to the main script
  345. Return()
  346.  
  347. # End of scripts
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement