Advertisement
RichPyke

Ping Roam V1.1.0

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