API tools faq
paste
Guest User

Phoenix 1.3 Test7

a guest
Mar 16th, 2010
758
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 49.63 KB | None | 0 0
raw download clone embed print report
  1. ;Project Lynxmotion Phoenix
  2. ;Description: Phoenix, controlled by a Futaba T7C remote
  3. ;Software version: V1.3
  4. ;Date: 20-10-2008
  5. ;Programmer: Jeroen Janssen (aka Xan)
  6. ;
  7. ;Hardware setup: ABB2 with ATOM 28 Pro, SSC32 V2, PS2 remote (See further for connections)
  8. ;
  9. ;NEW IN V1.3
  10. ; - Changed controls L1+ right stick
  11. ; - Balance calculations Thanks to Kåre Halvorsen (aka Zenta)
  12. ;
  13. ;RC control:
  14. ;Select between walking, movement and Individual leg control mode with the 3-state switch
  15. ;Use the two-state switch to toogle between the different gaits, movement or which leg to control
  16. ;
  17. ;KNOWN BUGS:
  18. ; - None at the moment ;)
  19. ;
  20. ;====================================================================
  21. ;[CONSTANDS]
  22. TRUE con 1
  23. FALSE con 0
  24.  
  25. BUTTON_DOWN con 0
  26. BUTTON_UP con 1
  27. ;--------------------------------------------------------------------
  28. ;[SERIAL CONNECTIONS]
  29. SSC_LM_SETUP con 1 ;Changes the SSC pins corresponding to the setup
  30. ;1 = Setup with connector to the front
  31. ;0 = Setup with connector to the back
  32.  
  33. SSC_OUT con P14 ;black wire ;Output pin for (SSC32 RX) on BotBoard (Yellow)
  34. SSC_IN con P15 ;white wire ;Input pin for (SSC32 TX) on BotBoard (Blue)
  35. SSC_BAUTE con i38400 ;SSC32 Baute rate
  36. ;--------------------------------------------------------------------
  37. ;[RC Controller] Futaba T7C Heli
  38. RCCh0 con P0 ;RC Remote Right Stick Left/Right (ch1 on T7C)
  39. RCCh1 con P1 ;RC Remote Right Stick Up/Down (ch2 on T7C)
  40. RCCh2 con P2 ;RC Remote Left Stick Up/Down (ch3 on T7C)
  41. RCCh3 con P3 ;RC Remote Left Stick Left/Right (ch4 on T7C)
  42. RCCh4 con P4 ;RC Remote E-switch (G switch on A model) 3-state (ch5 on T7C)
  43. RCCh5 con P5 ;RC Remote Vr-pot (ch6 on T7C)
  44. RCCh6 con P6 ;RC Remote H-switch (B switch on A model) 2-state (ch7 on T7C)
  45. awPulsesIn var word(7)
  46. bPulseTimeout var byte
  47.  
  48.  
  49. ;--------------------------------------------------------------------
  50. ;[PIN NUMBERS]
  51. #IF SSC_LM_SETUP ;Connector to the front
  52. RRCoxaPin con P0 ;Rear Right leg Hip Horizontal
  53. RRFemurPin con P1 ;Rear Right leg Hip Vertical
  54. RRTibiaPin con P2 ;Rear Right leg Knee
  55.  
  56. RMCoxaPin con P4 ;Middle Right leg Hip Horizontal
  57. RMFemurPin con P5 ;Middle Right leg Hip Vertical
  58. RMTibiaPin con P6 ;Middle Right leg Knee
  59.  
  60. RFCoxaPin con P8 ;Front Right leg Hip Horizontal
  61. RFFemurPin con P9 ;Front Right leg Hip Vertical
  62. RFTibiaPin con P10 ;Front Right leg Knee
  63.  
  64. LRCoxaPin con P16 ;Rear Left leg Hip Horizontal
  65. LRFemurPin con P17 ;Rear Left leg Hip Vertical
  66. LRTibiaPin con P18 ;Rear Left leg Knee
  67.  
  68. LMCoxaPin con P20 ;Middle Left leg Hip Horizontal
  69. LMFemurPin con P21 ;Middle Left leg Hip Vertical
  70. LMTibiaPin con P22 ;Middle Left leg Knee
  71.  
  72. LFCoxaPin con P24 ;Front Left leg Hip Horizontal
  73. LFFemurPin con P25 ;Front Left leg Hip Vertical
  74. LFTibiaPin con P26 ;Front Left leg Knee
  75.  
  76. ;--------------------------------------------------------------------
  77. ;[MIN/MAX ANGLES]
  78. RRCoxa_MIN con -26 ;Mechanical limits of the Right Rear Leg
  79. RRCoxa_MAX con 74
  80. RRFemur_MIN con -101
  81. RRFemur_MAX con 95
  82. RRTibia_MIN con -106
  83. RRTibia_MAX con 77
  84.  
  85. RMCoxa_MIN con -53 ;Mechanical limits of the Right Middle Leg
  86. RMCoxa_MAX con 53
  87. RMFemur_MIN con -101
  88. RMFemur_MAX con 95
  89. RMTibia_MIN con -106
  90. RMTibia_MAX con 77
  91.  
  92. RFCoxa_MIN con -58 ;Mechanical limits of the Right Front Leg
  93. RFCoxa_MAX con 74
  94. RFFemur_MIN con -101
  95. RFFemur_MAX con 95
  96. RFTibia_MIN con -106
  97. RFTibia_MAX con 77
  98.  
  99. LRCoxa_MIN con -74 ;Mechanical limits of the Left Rear Leg
  100. LRCoxa_MAX con 26
  101. LRFemur_MIN con -95
  102. LRFemur_MAX con 101
  103. LRTibia_MIN con -77
  104. LRTibia_MAX con 106
  105.  
  106. LMCoxa_MIN con -53 ;Mechanical limits of the Left Middle Leg
  107. LMCoxa_MAX con 53
  108. LMFemur_MIN con -95
  109. LMFemur_MAX con 101
  110. LMTibia_MIN con -77
  111. LMTibia_MAX con 106
  112.  
  113. LFCoxa_MIN con -74 ;Mechanical limits of the Left Front Leg
  114. LFCoxa_MAX con 58
  115. LFFemur_MIN con -95
  116. LFFemur_MAX con 101
  117. LFTibia_MIN con -77
  118. LFTibia_MAX con 106
  119. ;--------------------------------------------------------------------
  120. ;[BODY DIMENSIONS]
  121. CoxaLength con 29 ;Length of the Coxa [mm]
  122. FemurLength con 76 ;Length of the Femur [mm]
  123. TibiaLength con 106 ;Lenght of the Tibia [mm]
  124.  
  125. CoxaAngle con 60 ;Default Coxa setup angle
  126.  
  127. RFOffsetX con -43 ;Distance X from center of the body to the Right Front coxa
  128. RFOffsetZ con -82 ;Distance Z from center of the body to the Right Front coxa
  129. RMOffsetX con -63 ;Distance X from center of the body to the Right Middle coxa
  130. RMOffsetZ con 0 ;Distance Z from center of the body to the Right Middle coxa
  131. RROffsetX con -43 ;Distance X from center of the body to the Right Rear coxa
  132. RROffsetZ con 82 ;Distance Z from center of the body to the Right Rear coxa
  133.  
  134. LFOffsetX con 43 ;Distance X from center of the body to the Left Front coxa
  135. LFOffsetZ con -82 ;Distance Z from center of the body to the Left Front coxa
  136. LMOffsetX con 63 ;Distance X from center of the body to the Left Middle coxa
  137. LMOffsetZ con 0 ;Distance Z from center of the body to the Left Middle coxa
  138. LROffsetX con 43 ;Distance X from center of the body to the Left Rear coxa
  139. LROffsetZ con 82 ;Distance Z from center of the body to the Left Rear coxa
  140. ;--------------------------------------------------------------------
  141. ;[REMOTE]
  142. TravelDeadZone con 4 ;The deadzone for the analog input from the remote
  143. ;====================================================================
  144. ;[ANGLES]
  145. RFCoxaAngle var sword ;Actual Angle of the Right Front Leg
  146. RFFemurAngle var sword
  147. RFTibiaAngle var sword
  148.  
  149. RMCoxaAngle var sword ;Actual Angle of the Right Middle Leg
  150. RMFemurAngle var sword
  151. RMTibiaAngle var sword
  152.  
  153. RRCoxaAngle var sword ;Actual Angle of the Right Rear Leg
  154. RRFemurAngle var sword
  155. RRTibiaAngle var sword
  156.  
  157. LFCoxaAngle var sword ;Actual Angle of the Left Front Leg
  158. LFFemurAngle var sword
  159. LFTibiaAngle var sword
  160.  
  161. LMCoxaAngle var sword ;Actual Angle of the Left Middle Leg
  162. LMFemurAngle var sword
  163. LMTibiaAngle var sword
  164.  
  165. LRCoxaAngle var sword ;Actual Angle of the Left Rear Leg
  166. LRFemurAngle var sword
  167. LRTibiaAngle var sword
  168. ;--------------------------------------------------------------------
  169. ;[POSITIONS]
  170. RFPosX var sword ;Actual Position of the Right Front Leg
  171. RFPosY var sword
  172. RFPosZ var sword
  173.  
  174. RMPosX var sword ;Actual Position of the Right Middle Leg
  175. RMPosY var sword
  176. RMPosZ var sword
  177.  
  178. RRPosX var sword ;Actual Position of the Right Rear Leg
  179. RRPosY var sword
  180. RRPosZ var sword
  181.  
  182. LFPosX var sword ;Actual Position of the Left Front Leg
  183. LFPosY var sword
  184. LFPosZ var sword
  185.  
  186. LMPosX var sword ;Actual Position of the Left Middle Leg
  187. LMPosY var sword
  188. LMPosZ var sword
  189.  
  190. LRPosX var sword ;Actual Position of the Left Rear Leg
  191. LRPosY var sword
  192. LRPosZ var sword
  193. ;--------------------------------------------------------------------
  194. ;[INPUTS]
  195. butA var bit
  196. butB var bit
  197. butC var bit
  198.  
  199. prev_butA var bit
  200. prev_butB var bit
  201. prev_butC var bit
  202. ;--------------------------------------------------------------------
  203. ;[OUTPUTS]
  204. LedA var bit ;Red
  205. LedB var bit ;Green
  206. LedC var bit ;Orange
  207. ;--------------------------------------------------------------------
  208. ;[VARIABLES]
  209. Index var byte ;Index used for freeing the servos
  210. SSCDone var byte ;Char to check if SSC is done
  211.  
  212. ;GetSinCos
  213. AngleDeg var float ;Input Angle in degrees
  214. ABSAngleDeg var float ;Absolute value of the Angle in Degrees
  215. AngleRad var float ;Angle in Radian
  216. sinA var float ;Output Sinus of the given Angle
  217. cosA var float ;Output Cosinus of the given Angle
  218.  
  219. ;GetBoogTan
  220. BoogTanX var sword ;Input X
  221. BoogTanY var sword ;Input Y
  222. BoogTan var float ;Output BOOGTAN2(X/Y)
  223.  
  224. ;Body position
  225. BodyPosX var sbyte ;Global Input for the position of the body
  226. BodyPosY var sword
  227. BodyPosZ var sbyte
  228.  
  229. ;Body Inverse Kinematics
  230. BodyRotX var sbyte ;Global Input pitch of the body
  231. BodyRotY var sbyte ;Global Input rotation of the body
  232. BodyRotZ var sbyte ;Global Input roll of the body
  233. PosX var sword ;Input position of the feet X
  234. PosZ var sword ;Input position of the feet Z
  235. PosY var sword ;Input position of the feet Y
  236. RotationY var sbyte ;Input for rotation of a single feet for the gait
  237. BodyOffsetX var sbyte ;Input Offset betweeen the body and Coxa X
  238. BodyOffsetZ var sbyte ;Input Offset betweeen the body and Coxa Z
  239. sinB var float ;Sin buffer for BodyRotX calculations
  240. cosB var float ;Cos buffer for BodyRotX calculations
  241. sinG var float ;Sin buffer for BodyRotZ calculations
  242. cosG var float ;Cos buffer for BodyRotZ calculations
  243. TotalX var sword ;Total X distance between the center of the body and the feet
  244. TotalZ var sword ;Total Z distance between the center of the body and the feet
  245. DistCenterBodyFeet var float ;Total distance between the center of the body and the feet
  246. AngleCenterBodyFeetX var float ;Angle between the center of the body and the feet
  247. BodyIKPosX var sword ;Output Position X of feet with Rotation
  248. BodyIKPosY var sword ;Output Position Y of feet with Rotation
  249. BodyIKPosZ var sword ;Output Position Z of feet with Rotation
  250.  
  251.  
  252. ;Leg Inverse Kinematics
  253. IKFeetPosX var sword ;Input position of the Feet X
  254. IKFeetPosY var sword ;Input position of the Feet Y
  255. IKFeetPosZ var sword ;Input Position of the Feet Z
  256. IKFeetPosXZ var sword ;Length between the coxa and feet
  257. IKSW var float ;Length between shoulder and wrist
  258. IKA1 var float ;Angle between SW line and the ground in rad
  259. IKA2 var float ;?
  260. IKSolution var bit ;Output true if the solution is possible
  261. IKSolutionWarning var bit ;Output true if the solution is NEARLY possible
  262. IKSolutionError var bit ;Output true if the solution is NOT possible
  263. IKFemurAngle var sword ;Output Angle of Femur in degrees
  264. IKTibiaAngle var sword ;Output Angle of Tibia in degrees
  265. IKCoxaAngle var sword ;Output Angle of Coxa in degrees
  266. ;--------------------------------------------------------------------
  267. ;[RC Remote]
  268. Buttons var byte
  269. LastButtons var byte
  270. Alive var byte
  271. Mode var Nib ;ch5 Mode switch + twostate switch H = 6 modes
  272. TwoStateSW var bit ;ch 7 ,2-state switch
  273. ToogleMovement var bit
  274. Testleg var nib
  275. Whatleg var nib
  276. ;--------------------------------------------------------------------
  277. ;[TIMING]
  278. lTimerWOverflowCnt var long ;used in WTimer overflow. Will keep a 16 bit overflow so we have a 32 bit timer
  279. lCurrentTime var long
  280. lTimerStart var long ;Start time of the calculation cycles
  281. lTimerEnd var long ;End time of the calculation cycles
  282. CycleTime var byte ;Total Cycle time
  283.  
  284. SSCTime var word ;Time for servo updates
  285. PrevSSCTime var word ;Previous time for the servo updates
  286.  
  287. InputTimeDelay var Byte ;Delay that depends on the input to get the "sneaking" effect
  288. ;--------------------------------------------------------------------
  289. ;[GLOABAL]
  290. HexOn var bit ;Switch to turn on Phoenix
  291. TurnOff var bit ;Mark to turn off Phoenix
  292. ;--------------------------------------------------------------------
  293. ;[Balance]
  294. BalanceMode var bit
  295. TravelHeightY var sword
  296. TotalTransX var sword
  297. TotalTransZ var sword
  298. TotalTransY var sword
  299. TotalYbal var sword
  300. TotalXBal var sword
  301. TotalZBal var sword
  302. TotalY var sword ;Total Y distance between the center of the body and the feet
  303.  
  304. ;[gait]
  305. GaitType var byte ;Gait type
  306. NomGaitSpeed var byte ;Nominal speed of the gait
  307.  
  308. LegLiftHeight var byte ;Current Travel height
  309. TravelLengthX var sword ;Current Travel length X
  310. TravelLengthZ var sword ;Current Travel length Z
  311. TravelRotationY var sword ;Current Travel Rotation Y
  312.  
  313. TLDivFactor var byte ;Number of steps that a leg is on the floor while walking
  314. NrLiftedPos var nib ;Number of positions that a single leg is lifted (1-3)
  315. HalfLiftHeigth var bit ;If TRUE the outer positions of the ligted legs will be half height
  316.  
  317. GaitInMotion var bit ;Temp to check if the gait is in motion
  318. StepsInGait var byte ;Number of steps in gait
  319. LastLeg var bit ;TRUE when the current leg is the last leg of the sequence
  320. GaitStep var byte ;Actual Gait step
  321.  
  322. RFGaitLegNr var byte ;Init position of the leg
  323. RMGaitLegNr var byte ;Init position of the leg
  324. RRGaitLegNr var byte ;Init position of the leg
  325. LFGaitLegNr var byte ;Init position of the leg
  326. LMGaitLegNr var byte ;Init position of the leg
  327. LRGaitLegNr var byte ;Init position of the leg
  328.  
  329. GaitLegNr var byte ;Input Number of the leg
  330. TravelMulti var sbyte ;Multiplier for the length of the step
  331.  
  332. RFGaitPosX var sbyte ;Relative position corresponding to the Gait
  333. RFGaitPosY var sbyte
  334. RFGaitPosZ var sbyte
  335. RFGaitRotY var sbyte ;Relative rotation corresponding to the Gait
  336.  
  337. RMGaitPosX var sbyte
  338. RMGaitPosY var sbyte
  339. RMGaitPosZ var sbyte
  340. RMGaitRotY var sbyte
  341.  
  342. RRGaitPosX var sbyte
  343. RRGaitPosY var sbyte
  344. RRGaitPosZ var sbyte
  345. RRGaitRotY var sbyte
  346.  
  347. LFGaitPosX var sbyte
  348. LFGaitPosY var sbyte
  349. LFGaitPosZ var sbyte
  350. LFGaitRotY var sbyte
  351.  
  352. LMGaitPosX var sbyte
  353. LMGaitPosY var sbyte
  354. LMGaitPosZ var sbyte
  355. LMGaitRotY var sbyte
  356.  
  357. LRGaitPosX var sbyte
  358. LRGaitPosY var sbyte
  359. LRGaitPosZ var sbyte
  360. LRGaitRotY var sbyte
  361.  
  362. GaitPosX var sbyte ;In-/Output Pos X of feet
  363. GaitPosY var sword ;In-/Output Pos Y of feet
  364. GaitPosZ var sbyte ;In-/Output Pos Z of feet
  365. GaitRotY var sbyte ;In-/Output Rotation Y of feet
  366. ;====================================================================
  367. ;[TIMER INTERRUPT INIT]
  368. ONASMINTERRUPT TIMERWINT, Handle_TIMERW
  369. ;====================================================================
  370. ;[INIT]
  371. ;Turning off all the leds
  372. LedA = 0
  373. LedB = 0
  374. LedC = 0
  375.  
  376. 'Feet Positions
  377. RFPosX = 53 ;Start positions of the Right Front leg
  378. RFPosY = 25
  379. RFPosZ = -91
  380.  
  381. RMPosX = 105 ;Start positions of the Right Middle leg
  382. RMPosY = 25
  383. RMPosZ = 0 ;was 0
  384.  
  385. RRPosX = 53 ;Start positions of the Right Rear leg
  386. RRPosY = 25
  387. RRPosZ = 91
  388.  
  389. LFPosX = 53 ;Start positions of the Left Front leg
  390. LFPosY = 25
  391. LFPosZ = -91
  392.  
  393. LMPosX = 105 ;Start positions of the Left Middle leg
  394. LMPosY = 25
  395. LMPosZ = 0 ;was 0
  396.  
  397. LRPosX = 53 ;Start positions of the Left Rear leg
  398. LRPosY = 25
  399. LRPosZ = 91
  400.  
  401. ;Body Positions
  402. BodyPosX = 0
  403. BodyPosY = 0
  404. BodyPosZ = 0
  405.  
  406. ;Body Rotations
  407. BodyRotX = 0
  408. BodyRotY = 0
  409. BodyRotZ = 0
  410.  
  411. ;Gait
  412. GaitType = 0
  413. BalanceMode = 0
  414. LegLiftHeight = 50
  415. GaitStep = 1
  416.  
  417. ;What leg is active variable 1-6
  418. Whatleg = 0
  419.  
  420. GOSUB GaitSelect
  421.  
  422. ;Timer
  423. WTIMERTICSPERMS con 2000; we have 16 clocks per ms and we are incrementing every 8 so divide again by 2
  424. TCRW = 0x30 ;clears TCNT and sets the timer to inc clock cycle / 8
  425. TMRW = 0x80 ;starts the timer counting
  426. lTimerWOverflowCnt = 0
  427. enable TIMERWINT_OVF
  428.  
  429. ;SSC
  430. SSCTime = 150
  431. InputTimeDelay = 0
  432. Pause 1000
  433. ;====================================================================
  434. ;[MAIN]
  435. main:
  436. 'Start time
  437. GOSUB GetCurrentTime[], lTimerStart
  438. ;Read input
  439. GOSUB RCInput1
  440.  
  441. 'Reset IKsolution indicators
  442. IKSolution = False
  443. IKSolutionWarning = False
  444. IKSolutionError = False
  445.  
  446. ;Gait
  447. GOSUB GaitSeq
  448.  
  449. ;Balance calculations
  450. TotalTransX = 0 'reset values used for calculation of balance
  451. TotalTransZ = 0
  452. TotalTransY = 0
  453. TotalXBal = 0
  454. TotalYBal = 0
  455. TotalZBal = 0
  456. IF (BalanceMode>0) THEN
  457. gosub BalCalcOneLeg [-RFPosX+BodyPosX+RFGaitPosX, RFPosZ+BodyPosZ+RFGaitPosZ,RFGaitPosY, RFOffsetX, RFOffsetZ]
  458. gosub BalCalcOneLeg [-RMPosX+BodyPosX+RMGaitPosX, RMPosZ+BodyPosZ+RMGaitPosZ,RMGaitPosY, RMOffsetX, RMOffsetZ]
  459. gosub BalCalcOneLeg [-RRPosX+BodyPosX+RRGaitPosX, RRPosZ+BodyPosZ+RRGaitPosZ,RRGaitPosY, RROffsetX, RROffsetZ]
  460. gosub BalCalcOneLeg [LFPosX-BodyPosX+LFGaitPosX, LFPosZ+BodyPosZ+LFGaitPosZ,LFGaitPosY, LFOffsetX, LFOffsetZ]
  461. gosub BalCalcOneLeg [LMPosX-BodyPosX+LMGaitPosX, LMPosZ+BodyPosZ+LMGaitPosZ,LMGaitPosY, LMOffsetX, LMOffsetZ]
  462. gosub BalCalcOneLeg [LRPosX-BodyPosX+LRGaitPosX, LRPosZ+BodyPosZ+LRGaitPosZ,LRGaitPosY, LROffsetX, LROffsetZ]
  463. gosub BalanceBody
  464. ENDIF
  465.  
  466. 'Reset IKsolution indicators
  467. IKSolution = False
  468. IKSolutionWarning = False
  469. IKSolutionError = False
  470.  
  471. ;Right Front leg
  472. GOSUB BodyIK [-RFPosX+BodyPosX+RFGaitPosX, RFPosZ+BodyPosZ+RFGaitPosZ,RFPosY+BodyPosY+RFGaitPosY, RFOffsetX, RFOffsetZ, RFGaitRotY]
  473. GOSUB LegIK [RFPosX-BodyPosX+BodyIKPosX-RFGaitPosX, RFPosY+BodyPosY-BodyIKPosY+RFGaitPosY, RFPosZ+BodyPosZ-BodyIKPosZ+RFGaitPosZ]
  474. RFCoxaAngle = IKCoxaAngle + CoxaAngle ;Angle for the basic setup for the front leg
  475. RFFemurAngle = IKFemurAngle
  476. RFTibiaAngle = IKTibiaAngle
  477.  
  478. ;Right Middle leg
  479. GOSUB BodyIK [-RMPosX+BodyPosX+RMGaitPosX, RMPosZ+BodyPosZ+RMGaitPosZ,RMPosY+BodyPosY+RMGaitPosY, RMOffsetX, RMOffsetZ, RMGaitRotY]
  480. GOSUB LegIK [RMPosX-BodyPosX+BodyIKPosX-RMGaitPosX, RMPosY+BodyPosY-BodyIKPosY+RMGaitPosY, RMPosZ+BodyPosZ-BodyIKPosZ+RMGaitPosZ]
  481. RMCoxaAngle = IKCoxaAngle
  482. RMFemurAngle = IKFemurAngle
  483. RMTibiaAngle = IKTibiaAngle
  484.  
  485. ;Right Rear leg
  486. GOSUB BodyIK [-RRPosX+BodyPosX+RRGaitPosX, RRPosZ+BodyPosZ+RRGaitPosZ,RRPosY+BodyPosY+RRGaitPosY, RROffsetX, RROffsetZ, RRGaitRotY]
  487. GOSUB LegIK [RRPosX-BodyPosX+BodyIKPosX-RRGaitPosX, RRPosY+BodyPosY-BodyIKPosY+RRGaitPosY, RRPosZ+BodyPosZ-BodyIKPosZ+RRGaitPosZ]
  488. RRCoxaAngle = IKCoxaAngle - CoxaAngle ;Angle for the basic setup for the front leg
  489. RRFemurAngle = IKFemurAngle
  490. RRTibiaAngle = IKTibiaAngle
  491.  
  492. ;Left Front leg
  493. GOSUB BodyIK [LFPosX-BodyPosX+LFGaitPosX, LFPosZ+BodyPosZ+LFGaitPosZ,LFPosY+BodyPosY+LFGaitPosY, LFOffsetX, LFOffsetZ, LFGaitRotY]
  494. GOSUB LegIK [LFPosX+BodyPosX-BodyIKPosX+LFGaitPosX, LFPosY+BodyPosY-BodyIKPosY+LFGaitPosY, LFPosZ+BodyPosZ-BodyIKPosZ+LFGaitPosZ]
  495. LFCoxaAngle = IKCoxaAngle + CoxaAngle ;Angle for the basic setup for the front leg
  496. LFFemurAngle = IKFemurAngle
  497. LFTibiaAngle = IKTibiaAngle
  498.  
  499. ;Left Middle leg
  500. GOSUB BodyIK [LMPosX-BodyPosX+LMGaitPosX, LMPosZ+BodyPosZ+LMGaitPosZ,LMPosY+BodyPosY+LMGaitPosY, LMOffsetX, LMOffsetZ, LMGaitRotY]
  501. GOSUB LegIK [LMPosX+BodyPosX-BodyIKPosX+LMGaitPosX, LMPosY+BodyPosY-BodyIKPosY+LMGaitPosY, LMPosZ+BodyPosZ-BodyIKPosZ+LMGaitPosZ]
  502. LMCoxaAngle = IKCoxaAngle
  503. LMFemurAngle = IKFemurAngle
  504. LMTibiaAngle = IKTibiaAngle
  505.  
  506. ;Left Rear leg
  507. GOSUB BodyIK [LRPosX-BodyPosX+LRGaitPosX, LRPosZ+BodyPosZ+LRGaitPosZ,LRPosY+BodyPosY+LRGaitPosY, LROffsetX, LROffsetZ, LRGaitRotY]
  508. GOSUB LegIK [LRPosX+BodyPosX-BodyIKPosX+LRGaitPosX, LRPosY+BodyPosY-BodyIKPosY+LRGaitPosY, LRPosZ+BodyPosZ-BodyIKPosZ+LRGaitPosZ]
  509. LRCoxaAngle = IKCoxaAngle - CoxaAngle ;Angle for the basic setup for the front leg
  510. LRFemurAngle = IKFemurAngle
  511. LRTibiaAngle = IKTibiaAngle
  512.  
  513. GOSUB CheckAngles
  514.  
  515. LedC = IKSolutionWarning
  516. LedA = IKSolutionError
  517.  
  518.  
  519.  
  520. ;Get endtime and calculate wait time
  521. GOSUB GetCurrentTime[], lTimerEnd
  522. CycleTime = (lTimerEnd-lTimerStart)/WTIMERTICSPERMS
  523.  
  524. ;Wait for previous commands to be completed while walking
  525. IF(ABS(TravelLengthX)>TravelDeadZone | ABS(TravelLengthZ)>TravelDeadZone | ABS(TravelRotationY*2)>TravelDeadZone) THEN
  526.  
  527. pause (PrevSSCTime - CycleTime -50) MIN 1 ; Min 1 ensures that there alway is a value in the pause command
  528.  
  529.  
  530. Else
  531. SSCTime = NomGaitSpeed + InputTimeDelay
  532. ENDIF
  533.  
  534. GOSUB ServoDriver
  535.  
  536.  
  537. goto main
  538. ;====================================================================
  539.  
  540. ;[RCInput] reads the input data from the RC-Remote and processes the
  541. ;data to the parameters.
  542. RCInput1:
  543.  
  544. ;Read input pulse lengths in the correct order that take shortest time
  545. ;-------------------------------------------------------------------
  546. ; Make sure all 7 IOs are set to input.
  547. PMR5 = 0 ; all IO lines are general IO
  548. PCR5 = 0 ; All are input (may want to leave bit 7 alone...
  549.  
  550. ; Ok now lets transisiton to assembly language.
  551. ;
  552. ; bMask = 0x7f ; set a mask of which bits we are needing...
  553. ; ; Mask could be 0xFE for pins 1-7, need to make array 8 not 7
  554. mov.b #0x7f, r1l ; Ok R1l will be our mask for outstanding IO port bytes.
  555.  
  556. ; wait until none of the IO lines are high...
  557. ; while PDR5 & bMask
  558. ; ;
  559. ; wend
  560. mov.l #250000,er2 ;(4) - setup timeout counter
  561. _PI7_WAIT_FOR_ALL_LOW:
  562. mov.b @PDR5:8, r0l
  563. and.b r1l, r0l ; see if any of the IO bits is still on...
  564. beq _PI7_WAIT_FOR_NEXT_IO_TO_GO_HIGH:8 ; all zero go to wait for one to go high...
  565. dec.l #1,er2 ;(2)
  566. bne _PI7_WAIT_FOR_ALL_LOW:8 ; an IO pin is high and we have not timed out, keep looping until none are high
  567. ; We timed out waiting for all inputs to be low, so error out...
  568. bra _P17_RETURN_STATUS:16 ; will return status that all timed out...
  569.  
  570. ; while bMask
  571.  
  572. _PI7_WAIT_FOR_NEXT_IO_TO_GO_HIGH:
  573. mov.l #250000,er2 ;(4) - setup timeout counter
  574.  
  575. _PI7_WAIT_FOR_NEXT_IO_TO_GO_HIGH2:
  576. ; we still need some 1 or more of the pulses...
  577. ; while (PDR5 & bMask) = 0 ; waiting for a pulse to go high.
  578. mov.b @PDR5:8, r0l ;(4)
  579. and.b r1l, r0l ;(*2) see if any of the IO bits is still on...
  580. bne _P17_IO_WENT_HIGH:8 ;(*4) One went high so go process
  581. dec.l #1,er2 ;(2)
  582. bne _PI7_WAIT_FOR_NEXT_IO_TO_GO_HIGH2:8 ; (4) Not a timeout go try again.
  583. ; we timed out...
  584. bra _P17_RETURN_STATUS:16 ; will return status of what timed out...
  585.  
  586. ; wend
  587. ; iPin = ???; TBD: convert which bit is high to IO line number 0-6
  588. ; see which bit is on in the mask
  589. _P17_IO_WENT_HIGH:
  590. xor.w r2,r2 ;(*2)
  591. xor.b r0h, r0h ;(*2)
  592. mov.l #AWPULSESIN,er3 ;(*6)
  593. _P17_WHICH_BIT_LOOP:
  594. shlr.b r0l ;(@2)
  595. bcs _P17_WHICH_BIT_LOOP_DONE:8 ;(@4)
  596. inc.b r0h ;(@2)
  597. inc.l #2, er3 ;(@2) - point to the next place in the array.
  598. add.w #18,r2 ;(@4) - we do 18 clocks for each pass through this loop
  599. bra _P17_WHICH_BIT_LOOP:8 ;(@4)
  600. _P17_WHICH_BIT_LOOP_DONE:
  601. ; bMaskBit = 1 << iPin ; get the mask for which pin...
  602. xor.b r1h,r1h ;(*2)
  603. bset.b r0h,r1h ;(*2) ok we have the mask
  604. bclr.b r0h,r1l ;(*2) and clear it from our global mask of ones we are waiting for
  605.  
  606. ; = (22) - count so far of clocks after went high
  607.  
  608. ; iPinLoopCnt = 0 ; This may be replaced with time calculations...
  609. ; while (PDR5 & bMaskBit)
  610. ; iPinLoopCnt = iPinLoopCnt + 1 ; how long until it goes low again
  611. ; wend
  612. _P17_WAIT_FOR_IO_GO_BACK_LOW:
  613. mov.b @PDR5:8, r0l ;(#4)
  614. and.b r1h, r0l ;(#2)
  615. beq _P17_IO_WENT_BACK_LOW:8 ;(#4)
  616. add.w #18,r2 ;(#4) - number of cyles per loop
  617. bcc _P17_WAIT_FOR_IO_GO_BACK_LOW:8 ;(#4)
  618.  
  619. ; we had a timeout return the status.
  620. bset.b r0h, r1l ; turn back on the bit we were waiting for...
  621. bra _P17_RETURN_STATUS:8 ;
  622.  
  623. _P17_IO_WENT_BACK_LOW:
  624. ; need to calculate the pulse width in ms... need to divide calculated clocks by 16
  625. add.w #22,r2 ; (4) ; need to add the rest of the overhead(*-1 loop above) in...
  626. shlr.w r2 ; (2)
  627. shlr.w r2 ; (2)
  628. shlr.w r2 ; (2)
  629. shlr.w r2 ; (2) / 16 (for clock speed)
  630.  
  631. ; aPulses(iPin) = iPinLoopCnt ; convert loop count to pulse width...
  632. mov.w r2,@er3 ; Save away the value
  633.  
  634. ; bMask = bMask & ~bMaskBit ; turn off waiting for this one...
  635. or r1l,r1l ; (2) see if we are done or not
  636. ; wend
  637. bne _PI7_WAIT_FOR_NEXT_IO_TO_GO_HIGH:16 ;(6)our mask has not gone to zero so wait for the next one.
  638.  
  639. _P17_RETURN_STATUS:
  640. mov.b r1l,@BPULSETIMEOUT
  641. ; finally transisition back to basic and return.
  642. ;--------------------------------------------------------------------
  643.  
  644. ;serout S_OUT, i9600, ["SSCTime=", sdec Ssctime, " ","InputTimeDelay=", sdec InputTimeDelay," ",sdec RcInput(0), 13]
  645.  
  646. BalanceMode = False
  647.  
  648. if awPulsesIn(4) < 1100 then
  649. Mode = 1
  650. elseif (awPulsesIn(4) > 1400) & (awPulsesIn(4) < 1600)
  651. Mode = 2
  652. else
  653. Mode = 3
  654. endif
  655.  
  656. if awPulsesIn(6) < 1500 then
  657. TwoStateSW = False
  658. else
  659. TwoStateSW = True
  660. endif
  661.  
  662. ;InputTimeDelay = (ABS((RCInput(0)-1500)/4) MIN ABS(((RCInput(1)-1500)/4)) MIN ABS(((RCInput(2)-1500)/4)) MIN ABS(((RCInput(3)-1500)/4)))
  663.  
  664. ;***********************************************************************************
  665. ;*** Walking mode, toogle between the different walking gait with 2-state button ***
  666. ;***********************************************************************************
  667. if Mode = 1 then
  668. BalanceMode = True
  669.  
  670.  
  671. if TwoStateSW then 'Toggle gait method:
  672. if GaitType < 7 then 'so far we have 8 gait methods
  673. GaitType = GaitType +1
  674. sound P9,[150\(800+(GaitType*100))]
  675. else
  676. GaitType = 0
  677. sound P9,[100\1900,150\2100]
  678. endif
  679. gosub GaitSelect
  680. endif
  681.  
  682. BodyPosY = (awPulsesIn(5)-1000)/8
  683. TravelLengthX = (awPulsesIn(0)-1500)/4
  684. TravelLengthZ = (awPulsesIn(1)-1500)/4
  685. TravelRotationY = (awPulsesIn(3)-1500)/12
  686.  
  687. ;***********************************************************************************
  688. ;*** Move mode, toogle between translating and rotating body with 2-state button ***
  689. ;***********************************************************************************
  690. elseif Mode = 2
  691. if TwoStateSw then 'Toogle movement method
  692. if ToogleMovement then
  693. ToogleMovement = False
  694. sound P9,[100\800,150\1800]
  695. else
  696. ToogleMovement = True
  697. sound P9,[100\1800,150\800]
  698. endif
  699. endif
  700. if ToogleMovement then
  701. 'Body translate:
  702. BodyPosX = -(awPulsesIn(0)-1500)/10
  703. BodyPosZ = -(awPulsesIn(1)-1500)/8
  704. BodyRotY = -(awPulsesIn(3)-1500)/20
  705. BodyPosY = (awPulsesIn(5)-1000)/8
  706. else
  707. 'Body rotate:
  708. BodyRotX = -(awPulsesIn(1)-1500)/20
  709. BodyRotY = -(awPulsesIn(3)-1500)/20
  710. BodyRotZ = (awPulsesIn(0)-1500)/20
  711. BodyPosY = (awPulsesIn(5)-1000)/8
  712. endif
  713.  
  714.  
  715. ;*************************************************************************************
  716. ;*** Individual leg control mode choose between 6 legs with 2-state button ***
  717. ;*************************************************************************************
  718. elseif Mode = 3
  719. BalanceMode = True
  720. ;GaitType = 2
  721.  
  722. If awPulsesIn(6) > 1600 then
  723. Whatleg = Whatleg + 1
  724. elseif Whatleg > 7
  725. Whatleg = 1
  726. endif
  727.  
  728. if whatleg = 1 then
  729. TestLeg = RFGaitLegNr
  730. TravelLengthX = (awPulsesIn(1)-1500)/5
  731. TravelLengthZ = (awPulsesIn(3)-1500)/4
  732. TravelHeightY = (awPulsesIn(2)-1500)/4
  733.  
  734. Elseif whatleg = 2
  735. TestLeg = RMGaitLegNr
  736. TravelLengthX = (awPulsesIn(1)-1500)/4
  737. TravelLengthZ = (awPulsesIn(3)-1500)/4
  738. TravelHeightY = (awPulsesIn(2)-1500)/4
  739.  
  740. Elseif whatleg = 3
  741. TestLeg = RRGaitLegNr
  742. TravelLengthX = (awPulsesIn(1)-1500)/5
  743. TravelLengthZ = (awPulsesIn(3)-1500)/4
  744. TravelHeightY = (awPulsesIn(2)-1500)/4
  745.  
  746. Elseif whatleg = 4
  747. TestLeg = LRGaitLegNr
  748. TravelLengthX = (awPulsesIn(1)-1500)/5
  749. TravelLengthZ = (awPulsesIn(3)-1500)/4
  750. TravelHeightY = (awPulsesIn(2)-1500)/4
  751.  
  752. Elseif whatleg = 5
  753. TestLeg = LMGaitLegNr
  754. TravelLengthX = (awPulsesIn(1)-1500)/4
  755. TravelLengthZ = (awPulsesIn(3)-1500)/4
  756. TravelHeightY = (awPulsesIn(2)-1500)/4
  757.  
  758. Elseif whatleg = 6
  759. TestLeg = LFGaitLegNr
  760. TravelLengthX = (awPulsesIn(1)-1500)/5
  761. TravelLengthZ = (awPulsesIn(3)-1500)/4
  762. TravelHeightY = (awPulsesIn(2)-1500)/4
  763. Endif
  764. ;serout S_OUT, i9600, ["GaitPosX=", sdec GaitPosX," GaitPosY=", sdec GaitPosY," GaitPosZ=", sdec GaitPosZ, 13]
  765. ;serout S_OUT, i9600, ["Whatleg=", sdec Whatleg, 13]
  766.  
  767.  
  768. ENDIF
  769.  
  770.  
  771. return
  772. ;--------------------------------------------------------------------
  773. ;[GAIT Select]
  774. GaitSelect
  775. ;Gait selector
  776. IF (GaitType = 0) THEN ;Ripple Gait 6 steps
  777. LRGaitLegNr = 1
  778. RFGaitLegNr = 2
  779. LMGaitLegNr = 3
  780. RRGaitLegNr = 4
  781. LFGaitLegNr = 5
  782. RMGaitLegNr = 6
  783.  
  784. NrLiftedPos = 1
  785. TLDivFactor = 4
  786. StepsInGait = 6
  787. NomGaitSpeed = 150
  788. ENDIF
  789.  
  790. IF (GaitType = 1) THEN ;Ripple Gait 12 steps
  791. LRGaitLegNr = 1
  792. RFGaitLegNr = 3
  793. LMGaitLegNr = 5
  794. RRGaitLegNr = 7
  795. LFGaitLegNr = 9
  796. RMGaitLegNr = 11
  797.  
  798. NrLiftedPos = 3
  799. HalfLiftHeigth = TRUE
  800. TLDivFactor = 8
  801. StepsInGait = 12
  802. NomGaitSpeed = 100
  803. ENDIF
  804.  
  805. IF (GaitType = 2) THEN ;Quadripple 9 steps
  806. LRGaitLegNr = 1
  807. RFGaitLegNr = 2
  808. LMGaitLegNr = 4
  809. RRGaitLegNr = 5
  810. LFGaitLegNr = 7
  811. RMGaitLegNr = 8
  812.  
  813. NrLiftedPos = 2
  814. HalfLiftHeigth = FALSE
  815. TLDivFactor = 6
  816. StepsInGait = 9
  817. NomGaitSpeed = 150
  818. ENDIF
  819.  
  820. IF (GaitType = 3) THEN ;Tripod 4 steps
  821. LRGaitLegNr = 3
  822. RFGaitLegNr = 1
  823. LMGaitLegNr = 1
  824. RRGaitLegNr = 1
  825. LFGaitLegNr = 3
  826. RMGaitLegNr = 3
  827.  
  828. NrLiftedPos = 1
  829. TLDivFactor = 2
  830. StepsInGait = 4
  831. NomGaitSpeed = 150
  832. ENDIF
  833.  
  834. IF (GaitType = 4) THEN ;Tripod 6 steps
  835. LRGaitLegNr = 4
  836. RFGaitLegNr = 1
  837. LMGaitLegNr = 1
  838. RRGaitLegNr = 1
  839. LFGaitLegNr = 4
  840. RMGaitLegNr = 4
  841.  
  842. NrLiftedPos = 2
  843. HalfLiftHeigth = FALSE
  844. TLDivFactor = 4
  845. StepsInGait = 6
  846. NomGaitSpeed = 150
  847. ENDIF
  848.  
  849. IF (GaitType = 5) THEN ;Tripod 8 steps
  850. LRGaitLegNr = 5
  851. RFGaitLegNr = 1
  852. LMGaitLegNr = 1
  853. RRGaitLegNr = 1
  854. LFGaitLegNr = 5
  855. RMGaitLegNr = 5
  856.  
  857. NrLiftedPos = 3
  858. HalfLiftHeigth = TRUE
  859. TLDivFactor = 4
  860. StepsInGait = 8
  861. NomGaitSpeed = 110
  862. ENDIF
  863.  
  864. IF (GaitType = 6) THEN ;Wave 12 steps
  865. LRGaitLegNr = 7
  866. RFGaitLegNr = 1
  867. LMGaitLegNr = 9
  868. RRGaitLegNr = 5
  869. LFGaitLegNr = 11
  870. RMGaitLegNr = 3
  871.  
  872. NrLiftedPos = 1
  873. HalfLiftHeigth = FALSE
  874. TLDivFactor = 10
  875. StepsInGait = 12
  876. NomGaitSpeed = 120
  877. ENDIF
  878.  
  879. IF (GaitType = 7) THEN ;Wave 18 steps
  880. LRGaitLegNr = 10
  881. RFGaitLegNr = 1
  882. LMGaitLegNr = 13
  883. RRGaitLegNr = 7
  884. LFGaitLegNr = 16
  885. RMGaitLegNr = 4
  886.  
  887. NrLiftedPos = 2
  888. HalfLiftHeigth = FALSE
  889. TLDivFactor = 16
  890. StepsInGait = 18
  891. NomGaitSpeed = 100
  892. ENDIF
  893. return
  894. ;--------------------------------------------------------------------
  895. ;[GAIT Sequence]
  896. GaitSeq
  897. ;Calculate Gait sequence
  898. LastLeg = FALSE
  899. GOSUB Gait [LRGaitLegNr, LRGaitPosX, LRGaitPosY, LRGaitPosZ, LRGaitRotY]
  900. LRGaitPosX = GaitPosX
  901. LRGaitPosY = GaitPosY
  902. LRGaitPosZ = GaitPosZ
  903. LRGaitRotY = GaitRotY
  904.  
  905. GOSUB Gait [RFGaitLegNr, RFGaitPosX, RFGaitPosY, RFGaitPosZ, RFGaitRotY]
  906. RFGaitPosX = GaitPosX
  907. RFGaitPosY = GaitPosY
  908. RFGaitPosZ = GaitPosZ
  909. RFGaitRotY = GaitRotY
  910.  
  911. GOSUB Gait [LMGaitLegNr, LMGaitPosX, LMGaitPosY, LMGaitPosZ, LMGaitRotY]
  912. LMGaitPosX = GaitPosX
  913. LMGaitPosY = GaitPosY
  914. LMGaitPosZ = GaitPosZ
  915. LMGaitRotY = GaitRotY
  916.  
  917. GOSUB Gait [RRGaitLegNr, RRGaitPosX, RRGaitPosY, RRGaitPosZ, RRGaitRotY]
  918. RRGaitPosX = GaitPosX
  919. RRGaitPosY = GaitPosY
  920. RRGaitPosZ = GaitPosZ
  921. RRGaitRotY = GaitRotY
  922.  
  923. GOSUB Gait [LFGaitLegNr, LFGaitPosX, LFGaitPosY, LFGaitPosZ, LFGaitRotY]
  924. LFGaitPosX = GaitPosX
  925. LFGaitPosY = GaitPosY
  926. LFGaitPosZ = GaitPosZ
  927. LFGaitRotY = GaitRotY
  928.  
  929. LastLeg = TRUE
  930. GOSUB Gait [RMGaitLegNr, RMGaitPosX, RMGaitPosY, RMGaitPosZ, RMGaitRotY]
  931. RMGaitPosX = GaitPosX
  932. RMGaitPosY = GaitPosY
  933. RMGaitPosZ = GaitPosZ
  934. RMGaitRotY = GaitRotY
  935. return
  936. ;--------------------------------------------------------------------
  937. ;[GAIT]
  938. Gait [GaitLegNr, GaitPosX, GaitPosY, GaitPosZ, GaitRotY]
  939. IF Mode = 3 then
  940. IF TestLeg = GaitLegNr then
  941. GaitPosX = TravelLengthX
  942. GaitPosY = -TravelHeightY
  943. GaitPosZ = TravelLengthZ
  944. GaitRotY = 0
  945. ENDIF
  946.  
  947. ELSE
  948. ;Check IF the Gait is in motion
  949. GaitInMotion = ((ABS(TravelLengthX)>TravelDeadZone) | (ABS(TravelLengthZ)>TravelDeadZone) | (ABS(TravelRotationY)>TravelDeadZone) )
  950.  
  951. ;Leg middle up position
  952. ;Gait in motion Gait NOT in motion, return to home position
  953. IF (GaitInMotion & (NrLiftedPos=1 | NrLiftedPos=3) & GaitStep=GaitLegNr) | (GaitInMotion=FALSE & GaitStep=GaitLegNr & ((ABS(GaitPosX)>2) | (ABS(GaitPosZ)>2) | (ABS(GaitRotY)>2))) THEN ;Up
  954. GaitPosX = 0
  955. GaitPosY = -LegLiftHeight
  956. GaitPosZ = 0
  957. GaitRotY = 0
  958. ELSE
  959.  
  960. ;Optional Half heigth Rear
  961. IF ((NrLiftedPos=2 & GaitStep=GaitLegNr) | (NrLiftedPos=3 & (GaitStep=GaitLegNr-1 | GaitStep=GaitLegNr+(StepsInGait-1)))) & GaitInMotion THEN
  962. GaitPosX = -TravelLengthX/2
  963. GaitPosY = -LegLiftHeight/(HalfLiftHeigth+1)
  964. GaitPosZ = -TravelLengthZ/2
  965. GaitRotY = -TravelRotationY/2
  966. ELSE
  967.  
  968. ;Optional half heigth front
  969. IF (NrLiftedPos>=2) & (GaitStep=GaitLegNr+1 | GaitStep=GaitLegNr-(StepsInGait-1)) & GaitInMotion THEN
  970. GaitPosX = TravelLengthX/2
  971. GaitPosY = -LegLiftHeight/(HalfLiftHeigth+1)
  972. GaitPosZ = TravelLengthZ/2
  973. GaitRotY = TravelRotationY/2
  974. ELSE
  975.  
  976. ;Leg front down position
  977. IF (GaitStep=GaitLegNr+NrLiftedPos | GaitStep=GaitLegNr-(StepsInGait-NrLiftedPos)) & GaitPosY<0 THEN
  978. GaitPosX = TravelLengthX/2
  979. GaitPosY = 0
  980. GaitPosZ = TravelLengthZ/2
  981. GaitRotY = TravelRotationY/2
  982.  
  983. ;Move body forward
  984. ELSE
  985. GaitPosX = GaitPosX - (TravelLengthX/TLDivFactor)
  986. GaitPosY = 0
  987. GaitPosZ = GaitPosZ - (TravelLengthZ/TLDivFactor)
  988. GaitRotY = GaitRotY - (TravelRotationY/TLDivFactor)
  989. ENDIF
  990. ENDIF
  991. ENDIF
  992. ENDIF
  993. Endif
  994.  
  995.  
  996. ;Advance to the next step
  997. IF LastLeg THEN ;The last leg in this step
  998. GaitStep = GaitStep+1
  999. IF GaitStep>StepsInGait THEN
  1000. GaitStep = 1
  1001. ENDIF
  1002. ENDIF
  1003.  
  1004. return
  1005. ;--------------------------------------------------------------------
  1006. ;[BalCalcOneLeg]
  1007. BalCalcOneLeg [PosX, PosZ, PosY, BodyOffsetX, BodyOffsetZ]
  1008. ;Calculating totals from center of the body to the feet
  1009. TotalZ = BodyOffsetZ+PosZ
  1010. TotalX = BodyOffsetX+PosX
  1011. TotalY = 150 + PosY' using the value 150 to lower the centerpoint of rotation 'BodyPosY +
  1012. TotalTransY = TotalTransY + PosY
  1013. TotalTransZ = TotalTransZ + TotalZ
  1014. TotalTransX = TotalTransX + TotalX
  1015. gosub GetBoogTan [TotalX, TotalZ]
  1016. TotalYbal = TotalYbal + TOINT((BoogTan*180.0) / 3.141592)
  1017. gosub GetBoogTan [TotalX, TotalY]
  1018. TotalZbal = TotalZbal + TOINT((BoogTan*180.0) / 3.141592)
  1019. gosub GetBoogTan [TotalZ, TotalY]
  1020. TotalXbal = TotalXbal + TOINT((BoogTan*180.0) / 3.141592)
  1021. ;serout S_OUT, i9600, ["BalOneLeg PosX=", sdec PosX," PosZ=", sdec PosZ," TotalXTransZ=", sdec TotalTransZ, 13]
  1022. return
  1023. ;--------------------------------------------------------------------
  1024. ;[BalanceBody]
  1025. BalanceBody:
  1026. TotalTransZ = TotalTransZ/6
  1027. TotalTransX = TotalTransX/6
  1028. TotalTransY = TotalTransY/6
  1029. if TotalYbal < -180 then 'Tangens fix caused by +/- 180 deg
  1030. TotalYbal = TotalYbal + 360
  1031. endif
  1032. if TotalZbal < -180 then 'Tangens fix caused by +/- 180 deg
  1033. TotalZbal = TotalZbal + 360
  1034. endif
  1035. if TotalXbal < -180 then 'Tangens fix caused by +/- 180 deg
  1036. TotalXbal = TotalXbal + 360
  1037. endif
  1038.  
  1039. ;Balance rotation
  1040. TotalYBal = TotalYbal/6
  1041. TotalXBal = TotalXbal/6
  1042. TotalZBal = -TotalZbal/6
  1043.  
  1044. ;Balance translation
  1045. LFGaitPosZ = LFGaitPosZ - TotalTransZ
  1046. LMGaitPosZ = LMGaitPosZ - TotalTransZ
  1047. LRGaitPosZ = LRGaitPosZ - TotalTransZ
  1048. RFGaitPosZ = RFGaitPosZ - TotalTransZ
  1049. RMGaitPosZ = RMGaitPosZ - TotalTransZ
  1050. RRGaitPosZ = RRGaitPosZ - TotalTransZ
  1051.  
  1052. LFGaitPosX = LFGaitPosX - TotalTransX
  1053. LMGaitPosX = LMGaitPosX - TotalTransX
  1054. LRGaitPosX = LRGaitPosX - TotalTransX
  1055. RFGaitPosX = RFGaitPosX - TotalTransX
  1056. RMGaitPosX = RMGaitPosX - TotalTransX
  1057. RRGaitPosX = RRGaitPosX - TotalTransX
  1058.  
  1059. LFGaitPosY = LFGaitPosY - TotalTransY
  1060. LMGaitPosY = LMGaitPosY - TotalTransY
  1061. LRGaitPosY = LRGaitPosY - TotalTransY
  1062. RFGaitPosY = RFGaitPosY - TotalTransY
  1063. RMGaitPosY = RMGaitPosY - TotalTransY
  1064. RRGaitPosY = RRGaitPosY - TotalTransY
  1065. return
  1066. ;--------------------------------------------------------------------
  1067. ;[GETSINCOS] Get the sinus and cosinus from the angle +/- multiple circles
  1068. ;AngleDeg - Input Angle in degrees
  1069. ;SinA - Output Sinus of AngleDeg
  1070. ;CosA - Output Cosinus of AngleDeg
  1071. GetSinCos [AngleDeg]
  1072.  
  1073. ;Get the absolute value of AngleDeg
  1074. IF AngleDeg < 0.0 THEN
  1075. ABSAngleDeg = AngleDeg *-1.0
  1076. ELSE
  1077. ABSAngleDeg = AngleDeg
  1078. ENDIF
  1079.  
  1080. ;Shift rotation to a full circle of 360 deg -> AngleDeg // 360
  1081. IF AngleDeg < 0.0 THEN ;Negative values
  1082. AngleDeg = 360.0-(ABSAngleDeg-TOFLOAT(360*(TOINT(ABSAngleDeg/360.0))))
  1083. ELSE ;Positive values
  1084. AngleDeg = ABSAngleDeg-TOFLOAT(360*(TOINT(ABSAngleDeg/360.0)))
  1085. ENDIF
  1086.  
  1087. IF AngleDeg < 180.0 THEN ;Angle between 0 and 180
  1088. ;Subtract 90 to shift range
  1089. AngleDeg = AngleDeg -90.0
  1090. ;Convert degree to radials
  1091. AngleRad = (AngleDeg*3.141592)/180.0
  1092.  
  1093. SinA = FCOS(AngleRad) ;Sin o to 180 deg = cos(Angle Rad - 90deg)
  1094. CosA = -FSIN(AngleRad) ;Cos 0 to 180 deg = -sin(Angle Rad - 90deg)
  1095.  
  1096. ELSE ;Angle between 180 and 360
  1097. ;Subtract 270 to shift range
  1098. AngleDeg = AngleDeg -270.0
  1099. ;Convert degree to radials
  1100. AngleRad = (AngleDeg*3.141592)/180.0
  1101.  
  1102. SinA = -FCOS(AngleRad) ;Sin 180 to 360 deg = -cos(Angle Rad - 270deg)
  1103. CosA = FSIN(AngleRad) ;Cos 180 to 360 deg = sin(Angle Rad - 270deg)
  1104. ENDIF
  1105. return
  1106. ;--------------------------------------------------------------------
  1107. ;[BOOGTAN2] Gets the Inverse Tangus from X/Y with the where Y can be zero or negative
  1108. ;BoogTanX - Input X
  1109. ;BoogTanY - Input Y
  1110. ;BoogTan - Output BOOGTAN2(X/Y)
  1111. GetBoogTan [BoogTanX, BoogTanY]
  1112. IF(BoogTanX = 0) THEN ; X=0 -> 0 or PI
  1113. IF(BoogTanY >= 0) THEN
  1114. BoogTan = 0.0
  1115. ELSE
  1116. BoogTan = 3.141592
  1117. ENDIF
  1118. ELSE
  1119.  
  1120. IF(BoogTanY = 0) THEN ; Y=0 -> +/- Pi/2
  1121. IF(BoogTanX > 0) THEN
  1122. BoogTan = 3.141592 / 2.0
  1123. ELSE
  1124. BoogTan = -3.141592 / 2.0
  1125. ENDIF
  1126. ELSE
  1127.  
  1128. IF(BoogTanY > 0) THEN ;BOOGTAN(X/Y)
  1129. BoogTan = FATAN(TOFLOAT(BoogTanX) / TOFLOAT(BoogTanY))
  1130. ELSE
  1131. IF(BoogTanX > 0) THEN ;BOOGTAN(X/Y) + PI
  1132. BoogTan = FATAN(TOFLOAT(BoogTanX) / TOFLOAT(BoogTanY)) + 3.141592
  1133. ELSE ;BOOGTAN(X/Y) - PI
  1134. BoogTan = FATAN(TOFLOAT(BoogTanX) / TOFLOAT(BoogTanY)) - 3.141592
  1135. ENDIF
  1136. ENDIF
  1137. ENDIF
  1138. ENDIF
  1139. return
  1140. ;--------------------------------------------------------------------
  1141. ;[BODY INVERSE KINEMATICS]
  1142. ;BodyRotX - Global Input pitch of the body
  1143. ;BodyRotY - Global Input rotation of the body
  1144. ;BodyRotZ - Global Input roll of the body
  1145. ;RotationY - Input Rotation for the gait
  1146. ;PosX - Input position of the feet X
  1147. ;PosZ - Input position of the feet Z
  1148. ;BodyOffsetX - Input Offset betweeen the body and Coxa X
  1149. ;BodyOffsetZ - Input Offset betweeen the body and Coxa Z
  1150. ;SinB - Sin buffer for BodyRotX
  1151. ;CosB - Cos buffer for BodyRotX
  1152. ;SinG - Sin buffer for BodyRotZ
  1153. ;CosG - Cos buffer for BodyRotZ
  1154. ;BodyIKPosX - Output Position X of feet with Rotation
  1155. ;BodyIKPosY - Output Position Y of feet with Rotation
  1156. ;BodyIKPosZ - Output Position Z of feet with Rotation
  1157. BodyIK [PosX, PosZ, PosY, BodyOffsetX, BodyOffsetZ, RotationY]
  1158.  
  1159. ;Calculating totals from center of the body to the feet
  1160. TotalZ = BodyOffsetZ+PosZ
  1161. TotalX = BodyOffsetX+PosX
  1162. ;PosY are equal to a "TotalY"
  1163.  
  1164. ;Successive global rotation matrix:
  1165. ;Math shorts for rotation: Alfa (A) = Xrotate, Beta (B) = Zrotate, Gamma (G) = Yrotate
  1166. ;Sinus Alfa = sinA, cosinus Alfa = cosA. and so on...
  1167.  
  1168. ;First calculate sinus and cosinus for each rotation:
  1169. GOSUB GetSinCos [TOFLOAT(BodyRotX+TotalXBal)]
  1170. SinG = SinA
  1171. CosG = CosA
  1172. GOSUB GetSinCos [TOFLOAT(BodyRotZ+TotalZBal)]
  1173. SinB = SinA
  1174. CosB = CosA
  1175. GOSUB GetSinCos [TOFLOAT(BodyRotY+RotationY+TotalYBal)]
  1176.  
  1177. ;Calcualtion of rotation matrix:
  1178. BodyIKPosX = TotalX-TOINT(TOFLOAT(TotalX)*CosA*CosB - TOFLOAT(TotalZ)*CosB*SinA + TOFLOAT(PosY)*SinB)
  1179. BodyIKPosZ = TotalZ-TOINT(TOFLOAT(TotalX)*CosG*SinA + TOFLOAT(TotalX)*CosA*SinB*SinG +TOFLOAT(TotalZ)*CosA*CosG-TOFLOAT(TotalZ)*SinA*SinB*SinG-TOFLOAT(PosY)*CosB*SinG)
  1180. BodyIKPosY = PosY - TOINT(TOFLOAT(TotalX)*SinA*SinG - TOFLOAT(TotalX)*CosA*CosG*SinB + TOFLOAT(TotalZ)*CosA*SinG + TOFLOAT(TotalZ)*CosG*SinA*SinB + TOFLOAT(PosY)*CosB*CosG)
  1181.  
  1182. return
  1183. ;--------------------------------------------------------------------
  1184. ;[LEG INVERSE KINEMATICS] Calculates the angles of the tibia and femur for the given position of the feet
  1185. ;IKFeetPosX - Input position of the Feet X
  1186. ;IKFeetPosY - Input position of the Feet Y
  1187. ;IKFeetPosZ - Input Position of the Feet Z
  1188. ;IKSolution - Output true IF the solution is possible
  1189. ;IKSolutionWarning - Output true IF the solution is NEARLY possible
  1190. ;IKSolutionError - Output true IF the solution is NOT possible
  1191. ;IKFemurAngle - Output Angle of Femur in degrees
  1192. ;IKTibiaAngle - Output Angle of Tibia in degrees
  1193. ;IKCoxaAngle - Output Angle of Coxa in degrees
  1194. LegIK [IKFeetPosX, IKFeetPosY, IKFeetPosZ]
  1195.  
  1196. ;Length between the Coxa and Feet
  1197. IKFeetPosXZ = TOINT(FSQRT(TOFLOAT((IKFeetPosX*IKFeetPosX)+(IKFeetPosZ*IKFeetPosZ))))
  1198.  
  1199. ;IKSW - Length between shoulder and wrist
  1200. IKSW = FSQRT(TOFLOAT(((IKFeetPosXZ-CoxaLength)*(IKFeetPosXZ-CoxaLength))+(IKFeetPosY*IKFeetPosY)))
  1201.  
  1202. ;IKA1 - Angle between SW line and the ground in rad
  1203. GOSUB GetBoogTan [IKFeetPosXZ-CoxaLength, IKFeetPosY]
  1204. IKA1 = BoogTan
  1205.  
  1206. ;IKA2 - ?
  1207. IKA2 = FACOS((TOFLOAT((FemurLength*FemurLength) - (TibiaLength*TibiaLength)) + (IKSW*IKSW)) / (TOFLOAT(2*Femurlength) * IKSW))
  1208.  
  1209. ;IKFemurAngle
  1210. IKFemurAngle = (TOINT(((IKA1 + IKA2) * 180.0) / 3.141592)*-1)+90
  1211.  
  1212. ;IKTibiaAngle
  1213. IKTibiaAngle = (90-TOINT(((FACOS((TOFLOAT((FemurLength*FemurLength) + (TibiaLength*TibiaLength)) - (IKSW*IKSW)) / TOFLOAT(2*Femurlength*TibiaLength)))*180.0) / 3.141592)) * -1
  1214.  
  1215. ;IKCoxaAngle
  1216. GOSUB GetBoogTan [IKFeetPosZ, IKFeetPosX]
  1217. IKCoxaAngle = TOINT((BoogTan*180.0) / 3.141592)
  1218.  
  1219. ;Set the Solution quality
  1220. IF(IKSW < TOFLOAT(FemurLength+TibiaLength-30)) THEN
  1221. IKSolution = TRUE
  1222. ELSE
  1223. IF(IKSW < TOFLOAT(FemurLength+TibiaLength)) THEN
  1224. IKSolutionWarning = TRUE
  1225. ELSE
  1226. IKSolutionError = TRUE
  1227. ENDIF
  1228. ENDIF
  1229.  
  1230. return
  1231. ;--------------------------------------------------------------------
  1232. ;[CHECK ANGLES] Checks the mechanical limits of the servos
  1233. CheckAngles:
  1234. RFCoxaAngle = (RFCoxaAngle min RFCoxa_MIN) max RFCoxa_MAX
  1235. RFFemurAngle = (RFFemurAngle min RFFemur_MIN) max RFFemur_MAX
  1236. RFTibiaAngle = (RFTibiaAngle min RFTibia_MIN) max RFTibia_MAX
  1237.  
  1238. RMCoxaAngle = (RMCoxaAngle min RMCoxa_MIN) max RMCoxa_MAX
  1239. RMFemurAngle = (RMFemurAngle min RMFemur_MIN) max RMFemur_MAX
  1240. RMTibiaAngle = (RMTibiaAngle min RMTibia_MIN) max RMTibia_MAX
  1241.  
  1242. RRCoxaAngle = (RRCoxaAngle min RRCoxa_MIN) max RRCoxa_MAX
  1243. RRFemurAngle = (RRFemurAngle min RRFemur_MIN) max RRFemur_MAX
  1244. RRTibiaAngle = (RRTibiaAngle min RRTibia_MIN) max RRTibia_MAX
  1245.  
  1246. LFCoxaAngle = (LFCoxaAngle min LFCoxa_MIN) max LFCoxa_MAX
  1247. LFFemurAngle = (LFFemurAngle min LFFemur_MIN) max LFFemur_MAX
  1248. LFTibiaAngle = (LFTibiaAngle min LFTibia_MIN) max LFTibia_MAX
  1249.  
  1250. LMCoxaAngle = (LMCoxaAngle min LMCoxa_MIN) max LMCoxa_MAX
  1251. LMFemurAngle = (LMFemurAngle min LMFemur_MIN) max LMFemur_MAX
  1252. LMTibiaAngle = (LMTibiaAngle min LMTibia_MIN) max LMTibia_MAX
  1253.  
  1254. LRCoxaAngle = (LRCoxaAngle min LRCoxa_MIN) max LRCoxa_MAX
  1255. LRFemurAngle = (LRFemurAngle min LRFemur_MIN) max LRFemur_MAX
  1256. LRTibiaAngle = (LRTibiaAngle min LRTibia_MIN) max LRTibia_MAX
  1257. return
  1258. ;--------------------------------------------------------------------
  1259. ;[SERVO DRIVER] Updates the positions of the servos
  1260. ServoDriver:
  1261. ;Front Right leg
  1262. serout SSC_OUT,SSC_BAUTE,["#",dec RFCoxaPin,"P",dec TOINT(TOFLOAT(-RFCoxaAngle +90)/0.10588238)+650]
  1263. serout SSC_OUT,SSC_BAUTE,["#",dec RFFemurPin,"P",dec TOINT(TOFLOAT(-RFFemurAngle+90)/0.10588238)+650]
  1264. serout SSC_OUT,SSC_BAUTE,["#",dec RFTibiaPin,"P",dec TOINT(TOFLOAT(-RFTibiaAngle+90)/0.10588238)+650]
  1265.  
  1266. ;Middle Right leg
  1267. serout SSC_OUT,SSC_BAUTE,["#",dec RMCoxaPin,"P",dec TOINT(TOFLOAT(-RMCoxaAngle +90)/0.10588238)+650]
  1268. serout SSC_OUT,SSC_BAUTE,["#",dec RMFemurPin,"P",dec TOINT(TOFLOAT(-RMFemurAngle+90)/0.10588238)+650]
  1269. serout SSC_OUT,SSC_BAUTE,["#",dec RMTibiaPin,"P",dec TOINT(TOFLOAT(-RMTibiaAngle+90)/0.10588238)+650]
  1270.  
  1271. ;Rear Right leg
  1272. serout SSC_OUT,SSC_BAUTE,["#",dec RRCoxaPin,"P",dec TOINT(TOFLOAT(-RRCoxaAngle +90)/0.10588238)+650]
  1273. serout SSC_OUT,SSC_BAUTE,["#",dec RRFemurPin,"P",dec TOINT(TOFLOAT(-RRFemurAngle+90)/0.10588238)+650]
  1274. serout SSC_OUT,SSC_BAUTE,["#",dec RRTibiaPin,"P",dec TOINT(TOFLOAT(-RRTibiaAngle+90)/0.10588238)+650]
  1275.  
  1276. ;Front Left leg
  1277. serout SSC_OUT,SSC_BAUTE,["#",dec LFCoxaPin,"P",dec TOINT(TOFLOAT(LFCoxaAngle +90)/0.10588238)+650]
  1278. serout SSC_OUT,SSC_BAUTE,["#",dec LFFemurPin,"P",dec TOINT(TOFLOAT(LFFemurAngle+90)/0.10588238)+650]
  1279. serout SSC_OUT,SSC_BAUTE,["#",dec LFTibiaPin ,"P",dec TOINT(TOFLOAT(LFTibiaAngle+90)/0.10588238)+650]
  1280.  
  1281. ;Middle Left leg
  1282. serout SSC_OUT,SSC_BAUTE,["#",dec LMCoxaPin,"P",dec TOINT(TOFLOAT(LMCoxaAngle +90)/0.10588238)+650]
  1283. serout SSC_OUT,SSC_BAUTE,["#",dec LMFemurPin,"P",dec TOINT(TOFLOAT(LMFemurAngle+90)/0.10588238)+650]
  1284. serout SSC_OUT,SSC_BAUTE,["#",dec LMTibiaPin,"P",dec TOINT(TOFLOAT(LMTibiaAngle+90)/0.10588238)+650]
  1285.  
  1286. ;Rear Left leg
  1287. serout SSC_OUT,SSC_BAUTE,["#",dec LRCoxaPin,"P",dec TOINT(TOFLOAT(LRCoxaAngle +90)/0.10588238)+650]
  1288. serout SSC_OUT,SSC_BAUTE,["#",dec LRFemurPin,"P",dec TOINT(TOFLOAT(LRFemurAngle+90)/0.10588238)+650]
  1289. serout SSC_OUT,SSC_BAUTE,["#",dec LRTibiaPin,"P",dec TOINT(TOFLOAT(LRTibiaAngle+90)/0.10588238)+650]
  1290.  
  1291. ;Send <CR>
  1292. serout SSC_OUT,SSC_BAUTE,["T",dec SSCTime,13]
  1293.  
  1294. PrevSSCTime = SSCTime
  1295. return
  1296. ;--------------------------------------------------------------------
  1297. ;[FREE SERVOS] Frees all the servos
  1298. FreeServos
  1299. for Index = 0 to 31
  1300. serout SSC_OUT,SSC_BAUTE,["#",DEC Index,"P0"]
  1301. next
  1302. serout SSC_OUT,SSC_BAUTE,["T200",13]
  1303. return
  1304. ;-----------------------------------------------------------------------------------
  1305. ;[Handle TimerW interrupt]
  1306. BEGINASMSUB
  1307. HANDLE_TIMERW
  1308. push.w r1 ; save away register we will use
  1309. bclr #7,@TSRW:8 ; clear the overflow bit in the Timer status word
  1310. mov.w @LTIMERWOVERFLOWCNT+1:16,r1 ; We will increment the word that is the highword for a clock timer
  1311. inc.w #1,r1
  1312. mov.w r1, @LTIMERWOVERFLOWCNT+1:16
  1313. pop.w r1 ; restore our registers
  1314. rte ; and return
  1315. return
  1316. ;-------------------------------------------------------------------------------------
  1317. ;[Simple function to get the current time and verify that no overflow happened]
  1318. GetCurrentTime
  1319. lCurrentTime = lTimerWoverflowCnt + TCNT ; calculate the timer
  1320. IF lCurrentTime.highword <> lTimerWOverflowcnt.highword THEN
  1321. lCurrentTime = lTimerWoverflowCnt + TCNT ; calculate the timer
  1322. ENDIF
  1323. return lCurrentTIme
  1324. ;----------------------------------------------------
Advertisement
Add Comment
Please, Sign In to add comment
Public Pastes
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!