Advertisement
RichPyke

Ping Roam V1.1.2

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