API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Jun 27th, 2017
50
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 12.24 KB | None | 0 0
raw download clone embed print report
  1. def sprint(motors):
  2. mutex.acquire()
  3. # bodyHeight between -50 -- 175
  4. stepHeight = 60.0
  5. speedFactor = sp.getSpeed()
  6. n_steps = sp.getNumberOfSteps()
  7. forLoopRange = int(n_steps)
  8. moving_time = 1.25
  9. sleep_factor = sp.getSleepFactor()
  10. sleeptime = (moving_time / (speedFactor * sleep_factor)) + 0.1
  11. speedFactor = 1.1
  12. step = 120.0 #normale step
  13. bodyHeight = 130.0
  14. centerShift = 0
  15.  
  16. resetBreadth = sp.getResetBreadth()
  17. walkBreadth = 60.0
  18. sprintRaise = False
  19. ookNeerzetten = False
  20. raise_sleep_time = 0.15
  21. midTurn_Two = 0
  22. midTurn_Five = 0
  23.  
  24. try:
  25.  
  26. while True:
  27.  
  28. # midTurn update
  29. if sp.step_factor_y < 0:
  30. midTurn_Two = sp.step_factor_y * 20.0
  31. midTurn_Five = 0
  32. else:
  33. midTurn_Two = 0
  34. midTurn_Five = sp.step_factor_y * 20.0
  35. # midTurn = sp.step_factor_y * 20.0
  36. # midTurn = 0
  37. ##########################################
  38. # Extra optillen
  39. ##########################################
  40.  
  41. if (math.sqrt(math.pow(sp.step_factor_x, 2) + math.pow(sp.step_factor_y, 2))) < 0.8:
  42.  
  43. sprintRaise = True
  44.  
  45. dest_x1 = 0
  46. dest_x3 = -step
  47. dest_x5 = (-step / 2.0) - midTurn_Five
  48.  
  49. dest_y = 100
  50.  
  51. moving_time = 0.417
  52. sleeptime = raise_sleep_time
  53.  
  54. # 1, 3, 5 optillen
  55. ids1, pos1, speed1 = moveLeg(1, speedFactor, bodyHeight, dest_x1, dest_y, -stepHeight,
  56. moving_time)
  57. ids2, pos2, speed2 = moveLeg(3, speedFactor, bodyHeight, dest_x3, dest_y, -stepHeight, moving_time)
  58. ids3, pos3, speed3 = moveLeg(5, speedFactor, bodyHeight, dest_x5, dest_y + walkBreadth, -stepHeight, moving_time)
  59. getChain().sync_write_pos_speed((ids1 + ids2 + ids3),
  60. (pos1 + pos2 + pos3),
  61. (speed1 + speed2 + speed3))
  62.  
  63. time.sleep(sleeptime)
  64. else:
  65. sprintRaise = False
  66.  
  67.  
  68. ###########################################################################################################
  69. # DRIEHOEKSBEWEGING 1
  70. ###########################################################################################################
  71.  
  72. dest_x2 = -(step / 2.0) - midTurn_Two
  73. dest_x4 = 0
  74. dest_x6 = -step
  75. dest_x1 = step
  76. dest_x3 = 0
  77. dest_x5 = (step / 2.0) - midTurn_Five
  78.  
  79. dest_y = 100
  80.  
  81. if(ookNeerzetten):
  82. dest_z = -stepHeight
  83. else:
  84. dest_z = 0
  85.  
  86. moving_time = 1.0
  87. sleeptime = 0.238
  88. # sleeptime = (moving_time / (speedFactor * sleep_factor)) + (0.05 * n_steps)
  89.  
  90. # 1, 3, 5 door de lucht en 2, 4, 6 over de grond
  91. ids1, pos1, speed1 = moveLegStraight(2, speedFactor, bodyHeight, dest_x2, dest_y + walkBreadth, 0, moving_time)
  92. ids2, pos2, speed2 = moveLegStraight(4, speedFactor, bodyHeight, dest_x4, dest_y, 0, moving_time)
  93. ids3, pos3, speed3 = moveLegStraight(6, speedFactor, bodyHeight, dest_x6, dest_y, 0, moving_time)
  94. ids4, pos4, speed4 = moveLeg(1, speedFactor, bodyHeight, (step / 2.0), dest_y, -stepHeight, moving_time / 2.0)
  95. ids5, pos5, speed5 = moveLeg(3, speedFactor, bodyHeight, -(step / 2.0), dest_y, -stepHeight, moving_time / 2.0)
  96. ids6, pos6, speed6 = moveLeg(5, speedFactor, bodyHeight, 0, dest_y + walkBreadth, -stepHeight, moving_time / 2.0)
  97. getChain().sync_write_pos_speed((ids1 + ids2 + ids3 + ids4 + ids5 + ids6),
  98. (pos1 + pos2 + pos3 + pos4 + pos5 + pos6),
  99. (speed1 + speed2 + speed3 + speed4 + speed5 + speed6))
  100.  
  101. time.sleep(sleeptime / n_steps)
  102.  
  103. ids1, pos1, speed1 = moveLegStraight(2, speedFactor, bodyHeight, dest_x2, dest_y + walkBreadth, 0, moving_time, 2)
  104. ids2, pos2, speed2 = moveLegStraight(4, speedFactor, bodyHeight, dest_x4, dest_y, 0, moving_time, 2)
  105. ids3, pos3, speed3 = moveLegStraight(6, speedFactor, bodyHeight, dest_x6, dest_y, 0, moving_time, 2)
  106. ids4, pos4, speed4 = moveLeg(1, speedFactor, bodyHeight, dest_x1, dest_y, dest_z, moving_time / 2.0)
  107. ids5, pos5, speed5 = moveLeg(3, speedFactor, bodyHeight, dest_x3, dest_y, dest_z, moving_time / 2.0)
  108. ids6, pos6, speed6 = moveLeg(5, speedFactor, bodyHeight, dest_x5, dest_y + walkBreadth, dest_z, moving_time / 2.0)
  109. getChain().sync_write_pos_speed((ids1 + ids2 + ids3 + ids4 + ids5 + ids6),
  110. (pos1 + pos2 + pos3 + pos4 + pos5 + pos6),
  111. (speed1 + speed2 + speed3 + speed4 + speed5 + speed6))
  112. time.sleep(sleeptime / n_steps)
  113.  
  114.  
  115. ##########################################
  116. # Extra neerzetten
  117. ##########################################
  118.  
  119. if (sprintRaise and ookNeerzetten):
  120.  
  121. dest_x1 = step
  122. dest_x3 = 0
  123. dest_x5 = (step / 2.0) - midTurn_Five
  124.  
  125. dest_y = 100
  126.  
  127. moving_time = 0.417
  128. sleeptime = raise_sleep_time
  129.  
  130. # 1, 3, 5 neertillen
  131. ids1, pos1, speed1 = moveLeg(1, speedFactor, bodyHeight, dest_x1, dest_y, 0,
  132. moving_time)
  133. ids2, pos2, speed2 = moveLeg(3, speedFactor, bodyHeight, dest_x3, dest_y, 0, moving_time)
  134. ids3, pos3, speed3 = moveLeg(5, speedFactor, bodyHeight, dest_x5, dest_y + walkBreadth, 0,
  135. moving_time)
  136. getChain().sync_write_pos_speed((ids1 + ids2 + ids3),
  137. (pos1 + pos2 + pos3),
  138. (speed1 + speed2 + speed3))
  139.  
  140. time.sleep(sleeptime)
  141.  
  142.  
  143. if not sp.current_action == 'SPRINT':
  144. break
  145.  
  146. # midTurn update
  147. if sp.step_factor_y < 0:
  148. midTurn_Two = sp.step_factor_y * 20.0
  149. midTurn_Five = 0
  150. else:
  151. midTurn_Two = 0
  152. midTurn_Five = sp.step_factor_y * 20.0
  153. # midTurn = sp.step_factor_y * 20.0
  154. # midTurn = 0
  155.  
  156. ##########################################
  157. # Extra optillen
  158. ##########################################
  159.  
  160. if (math.sqrt(math.pow(sp.step_factor_x, 2) + math.pow(sp.step_factor_y, 2))) < 0.8:
  161.  
  162. sprintRaise = True
  163.  
  164. dest_x2 = -(step / 2.0) - midTurn_Two
  165. dest_x4 = 0
  166. dest_x6 = -step
  167.  
  168. dest_y = 100
  169.  
  170. moving_time = 0.417
  171. sleeptime = raise_sleep_time
  172.  
  173. # 2, 4, 6 optillen
  174. ids1, pos1, speed1 = moveLeg(2, speedFactor, bodyHeight, dest_x2, dest_y + walkBreadth, -stepHeight,
  175. moving_time)
  176. ids2, pos2, speed2 = moveLeg(4, speedFactor, bodyHeight, dest_x4, dest_y, -stepHeight, moving_time)
  177. ids3, pos3, speed3 = moveLeg(6, speedFactor, bodyHeight, dest_x6, dest_y, -stepHeight, moving_time)
  178. getChain().sync_write_pos_speed((ids1 + ids2 + ids3),
  179. (pos1 + pos2 + pos3),
  180. (speed1 + speed2 + speed3))
  181.  
  182. time.sleep(sleeptime)
  183. else:
  184. sprintRaise = False
  185.  
  186.  
  187. ###########################################################################################################
  188. # DRIEHOEKSBEWEGING 2
  189. ###########################################################################################################
  190.  
  191. dest_x1 = 0
  192. dest_x3 = -step
  193. dest_x5 = (-step / 2.0) + midTurn_Five
  194. dest_x2 = (step / 2.0) + midTurn_Two
  195. dest_x4 = (step)
  196. dest_x6 = 0
  197.  
  198. dest_y = 100
  199.  
  200. if (ookNeerzetten):
  201. dest_z = -stepHeight
  202. else:
  203. dest_z = 0
  204.  
  205. moving_time = 1.0
  206. # sleeptime = (moving_time / (speedFactor * sleep_factor)) + (0.05 * n_steps)
  207. sleeptime = 0.238
  208.  
  209. # Poten 1, 3, 5 over de grond en Poten 2, 4, 6 door de lucht
  210. ids1, pos1, speed1 = moveLegStraight(1, speedFactor, bodyHeight, dest_x1, dest_y, 0, moving_time)
  211. ids2, pos2, speed2 = moveLegStraight(3, speedFactor, bodyHeight, dest_x3, dest_y, 0, moving_time)
  212. ids3, pos3, speed3 = moveLegStraight(5, speedFactor, bodyHeight, dest_x5, dest_y + walkBreadth, 0, moving_time)
  213. ids4, pos4, speed4 = moveLeg(2, speedFactor, bodyHeight, 0, dest_y + walkBreadth, -stepHeight, moving_time / 2.0)
  214. ids5, pos5, speed5 = moveLeg(4, speedFactor, bodyHeight, (step / 2.0), dest_y, -stepHeight, moving_time / 2.0)
  215. ids6, pos6, speed6 = moveLeg(6, speedFactor, bodyHeight, -(step / 2.0), dest_y, -stepHeight, moving_time / 2.0)
  216.  
  217. getChain().sync_write_pos_speed((ids1 + ids2 + ids3 + ids4 + ids5 + ids6),
  218. (pos1 + pos2 + pos3 + pos4 + pos5 + pos6),
  219. (speed1 + speed2 + speed3 + speed4 + speed5 + speed6))
  220.  
  221. time.sleep(sleeptime / n_steps)
  222.  
  223. ids1, pos1, speed1 = moveLegStraight(1, speedFactor, bodyHeight, dest_x1, dest_y, 0, moving_time, 2)
  224. ids2, pos2, speed2 = moveLegStraight(3, speedFactor, bodyHeight, dest_x3, dest_y, 0, moving_time, 2)
  225. ids3, pos3, speed3 = moveLegStraight(5, speedFactor, bodyHeight, dest_x5, dest_y + walkBreadth, 0, moving_time, 2)
  226. ids4, pos4, speed4 = moveLeg(2, speedFactor, bodyHeight, dest_x2, dest_y + walkBreadth, dest_z, moving_time / 2.0)
  227. ids5, pos5, speed5 = moveLeg(4, speedFactor, bodyHeight, dest_x4, dest_y, dest_z, moving_time / 2.0)
  228. ids6, pos6, speed6 = moveLeg(6, speedFactor, bodyHeight, dest_x6, dest_y, dest_z, moving_time / 2.0)
  229.  
  230. getChain().sync_write_pos_speed((ids1 + ids2 + ids3 + ids4 + ids5 + ids6),
  231. (pos1 + pos2 + pos3 + pos4 + pos5 + pos6),
  232. (speed1 + speed2 + speed3 + speed4 + speed5 + speed6))
  233. time.sleep(sleeptime / n_steps)
  234.  
  235. ##########################################
  236. # Extra neerzetten
  237. ##########################################
  238.  
  239. if (sprintRaise and ookNeerzetten):
  240.  
  241. dest_x2 = (step / 2.0) + midTurn_Two
  242. dest_x4 = (step)
  243. dest_x6 = 0
  244.  
  245. dest_y = 100
  246.  
  247. moving_time = 0.417
  248. sleeptime = raise_sleep_time
  249.  
  250. # 2, 4, 6 neerzetten
  251. ids1, pos1, speed1 = moveLeg(2, speedFactor, bodyHeight, dest_x2, dest_y + walkBreadth, 0,
  252. moving_time)
  253. ids2, pos2, speed2 = moveLeg(4, speedFactor, bodyHeight, dest_x4, dest_y, 0, moving_time)
  254. ids3, pos3, speed3 = moveLeg(6, speedFactor, bodyHeight, dest_x6, dest_y, 0, moving_time)
  255. getChain().sync_write_pos_speed((ids1 + ids2 + ids3),
  256. (pos1 + pos2 + pos3),
  257. (speed1 + speed2 + speed3))
  258.  
  259. time.sleep(sleeptime)
  260.  
  261. if not sp.current_action == 'SPRINT':
  262. break
  263.  
  264.  
  265.  
  266.  
  267. finally:
  268. mutex.release()
  269. # if not sp.current_action == 'WALK' and not sp.current_action == 'TURN' and not sp.current_action == 'TURNWALK':
  270. resetSpiderAllOnGround(speedFactor)
  271. # else:
  272. # sp.ready_for_action = True
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!