This is a modified code for the Lynxmotion Phoenix hexapod originally programmed by Jeroen Janssen (aka Xan).
This code is still in development for my current project but if things take a big change i will throw in a few updates.
The analog sticks not only controls the step length but also the speed of the gait.
Improved gait engine/speed/body rotations X-Z-Y/SSC communication/SSC V2 registers.
A universal gait engine which makes it possible to simply add new gait types by setting properties.
The analog sticks not only controls the step length but also the speed of the gait.
Selective Balance gesture control on all gaits
A internal timer measures the calculation time of each step. This makes it possible to get a perfect timing between the BB2 and the SSC.
The offsets of the servos are moved to the SSC V2 register.
8 different gaits: Thanks to Zenta
Gaits are: *due to updates some of these gaits have been removed from code.
- 6 steps ripple
- 12 steps ripple
- 9 steps quadripple
- 4 steps tripod
- 6 steps tripod
- 8 steps tripod
- 12 steps wave
- 18 steps wave
Improved body rotations: Thanks to Zenta
The improved body calculations by Zenta are implemented
Internal timer to improve SSC communication:
A internal timer measures the calculation time of each step. This makes it possible to get a perfect timing between the BB2 and the SSC. This makes the gaits very stable.
Turn on/off the bot with the remote:
Press the start button on the PS2 remote to turn on and off the bot.
Servo offsets moved to SSC V2 registers:
The offsets of the servos are moved to the SSC V2 register. This way there is no need to change the offsets in case of a new version.
CODE VERSION 1.0 Still under development
- Code: Select all
;Project: MSR-H01 Hexapod using Lynxmotion Phoenix code 1.3
;Description: MSR-H01 Hexapod, controlled by ABB2 with ATOM 28 Pro, SSC32 V2, PS2 Wireless control
;Programmer: Jeroen Janssen (aka Xan)
;Developer: Jonny Poole (aka Innerbreed)
;PS2 CONTROLS:
; - Start Turn on/off the bot
; - select Switch gaits
; - Left Stick Walk/Strafe
; - Right Stick Rotate
; - D-Pad up Body up manual control
; - D-Pad down Body down manual control
; - Triangle Move body to (walk pos)
; - Circle Move body to (crouch pos)
; - Square Switch Balance mode on or off
; - Cross Attack Mode Sequence
; - L1 & L. Stick Shift body X/Z
; - L1 & R. Stick Shift body Y and rotate body Y
; - L2 & Sticks Rotate body X/Y/Z
; - L3 *non-defined*
; - R1 & L. Stick Moves Head X/Z
; - R2 & L. Stick Double gait travel length
; - R3 *non-defined*
;
;====================================================================
;[CONSTANDS]
TRUE con 1
FALSE con 0
BUTTON_DOWN con 0
BUTTON_UP con 1
;--------------------------------------------------------------------
;[SERIAL CONNECTIONS]
SSC_OUT con P11 ;Output pin for (SSC32 RX) on BotBoard (Yellow)
SSC_IN con P10 ;Input pin for (SSC32 TX) on BotBoard (Blue)
SSC_BAUTE con i38400 ;SSC32 Baute rate
;--------------------------------------------------------------------
;[PS2 Controller]
PS2DAT con P12 ;PS2 Controller DAT (Brown)
PS2CMD con P13 ;PS2 controller CMD (Orange)
PS2SEL con P14 ;PS2 Controller SEL (Blue)
PS2CLK con P15 ;PS2 Controller CLK (White)
PadMode con $79
;--------------------------------------------------------------------
;[PIN NUMBERS]
RRCoxaPin con P12 ;Rear Right leg Hip Horizontal
RRFemurPin con P13 ;Rear Right leg Hip Vertical
RRTibiaPin con P14 ;Rear Right leg Knee
RMCoxaPin con P4 ;Middle Right leg Hip Horizontal
RMFemurPin con P5 ;Middle Right leg Hip Vertical
RMTibiaPin con P6 ;Middle Right leg Knee
RFCoxaPin con P0 ;Front Right leg Hip Horizontal
RFFemurPin con P1 ;Front Right leg Hip Vertical
RFTibiaPin con P2 ;Front Right leg Knee
LRCoxaPin con P28 ;Rear Left leg Hip Horizontal
LRFemurPin con P29 ;Rear Left leg Hip Vertical
LRTibiaPin con P30 ;Rear Left leg Knee
LMCoxaPin con P20 ;Middle Left leg Hip Horizontal
LMFemurPin con P21 ;Middle Left leg Hip Vertical
LMTibiaPin con P22 ;Middle Left leg Knee
LFCoxaPin con P16 ;Front Left leg Hip Horizontal
LFFemurPin con P17 ;Front Left leg Hip Vertical
LFTibiaPin con P18 ;Front Left leg Knee
HeadPan con P19
HeadTilt con P3
;--------------------------------------------------------------------
;[MIN/MAX ANGLES]
RRCoxa_MIN con -45 ;Mechanical limits of the Right Rear Leg
RRCoxa_MAX con 45
RRFemur_MIN con -80
RRFemur_MAX con 80
RRTibia_MIN con -70
RRTibia_MAX con 70
RMCoxa_MIN con -45 ;Mechanical limits of the Right Middle Leg
RMCoxa_MAX con 45
RMFemur_MIN con -80
RMFemur_MAX con 80
RMTibia_MIN con -70
RMTibia_MAX con 70
RFCoxa_MIN con -10 ;Mechanical limits of the Right Front Leg
RFCoxa_MAX con 40
RFFemur_MIN con -80
RFFemur_MAX con 80
RFTibia_MIN con -70
RFTibia_MAX con 70
LRCoxa_MIN con -45 ;Mechanical limits of the Left Rear Leg
LRCoxa_MAX con 45
LRFemur_MIN con -80
LRFemur_MAX con 80
LRTibia_MIN con -70
LRTibia_MAX con 70
LMCoxa_MIN con -45 ;Mechanical limits of the Left Middle Leg
LMCoxa_MAX con 45
LMFemur_MIN con -80
LMFemur_MAX con 80
LMTibia_MIN con -70
LMTibia_MAX con 70
LFCoxa_MIN con -10 ;Mechanical limits of the Left Front Leg
LFCoxa_MAX con 40
LFFemur_MIN con -80
LFFemur_MAX con 80
LFTibia_MIN con -70
LFTibia_MAX con 70
HeadPan_MAX con 20
HeadPan_MIN con -20
HeadTilt_MAX con 20
HeadTilt_MIN con -30
;--------------------------------------------------------------------
;[BODY DIMENSIONS] [mm]
CoxaLength con 20 ;Length of the Coxa [mm]
FemurLength con 80 ;Length of the Femur [mm]
TibiaLength con 130 ;Lenght of the Tibia [mm]
Panlength con 10
Tiltlength con 40
CoxaAngle con 30 ;Default Coxa setup angle / Was 60
RFOffsetX con -40 ;Distance X from center of the body to the Right Front coxa
RFOffsetZ con -80 ;Distance Z from center of the body to the Right Front coxa
RMOffsetX con -60 ;Distance X from center of the body to the Right Middle coxa
RMOffsetZ con 0 ;Distance Z from center of the body to the Right Middle coxa
RROffsetX con -40 ;Distance X from center of the body to the Right Rear coxa
RROffsetZ con 80 ;Distance Z from center of the body to the Right Rear coxa
LFOffsetX con 40 ;Distance X from center of the body to the Left Front coxa
LFOffsetZ con -80 ;Distance Z from center of the body to the Left Front coxa
LMOffsetX con 60 ;Distance X from center of the body to the Left Middle coxa
LMOffsetZ con 0 ;Distance Z from center of the body to the Left Middle coxa
LROffsetX con 40 ;Distance X from center of the body to the Left Rear coxa
LROffsetZ con 80 ;Distance Z from center of the body to the Left Rear coxa
PanOffsetX con 0 ;Distance X from center of the body to the head
PanOffsetZ con 110 ;Distance Z from center of the body to the head
TiltOffsetX con 130 ;Distance X from center of the body to the head
TiltffsetZ con 0 ;Distance Z from center of the body to the head
;--------------------------------------------------------------------
;[REMOTE]
TravelDeadZone con 9 ;The deadzone for the analog input from the remote
;====================================================================
;[ANGLES]
RFCoxaAngle var sword ;Actual Angle of the Right Front Leg
RFFemurAngle var sword
RFTibiaAngle var sword
RMCoxaAngle var sword ;Actual Angle of the Right Middle Leg
RMFemurAngle var sword
RMTibiaAngle var sword
RRCoxaAngle var sword ;Actual Angle of the Right Rear Leg
RRFemurAngle var sword
RRTibiaAngle var sword
LFCoxaAngle var sword ;Actual Angle of the Left Front Leg
LFFemurAngle var sword
LFTibiaAngle var sword
LMCoxaAngle var sword ;Actual Angle of the Left Middle Leg
LMFemurAngle var sword
LMTibiaAngle var sword
LRCoxaAngle var sword ;Actual Angle of the Left Rear Leg
LRFemurAngle var sword
LRTibiaAngle var sword
HeadPanAngle var sword ;Actual Angle of the Head pan
HeadTiltAngle var sword ;Actual Angle of the Head tilt
;--------------------------------------------------------------------
;[POSITIONS]
RFPosX var sword ;Actual Position of the Right Front Leg
RFPosY var sword
RFPosZ var sword
RMPosX var sword ;Actual Position of the Right Middle Leg
RMPosY var sword
RMPosZ var sword
RRPosX var sword ;Actual Position of the Right Rear Leg
RRPosY var sword
RRPosZ var sword
LFPosX var sword ;Actual Position of the Left Front Leg
LFPosY var sword
LFPosZ var sword
LMPosX var sword ;Actual Position of the Left Middle Leg
LMPosY var sword
LMPosZ var sword
LRPosX var sword ;Actual Position of the Left Rear Leg
LRPosY var sword
LRPosZ var sword
;--------------------------------------------------------------------
;[INPUTS]
butA var bit
butB var bit
butC var bit
prev_butA var bit
prev_butB var bit
prev_butC var bit
;--------------------------------------------------------------------
;[OUTPUTS]
LedA var bit ;Red
LedB var bit ;Green
LedC var bit ;Orange
;--------------------------------------------------------------------
;[VARIABLES]
Index var byte ;Index used for freeing the servos
SSCDone var byte ;Char to check if SSC is done
;GetSinCos
AngleDeg var float ;Input Angle in degrees
ABSAngleDeg var float ;Absolute value of the Angle in Degrees
AngleRad var float ;Angle in Radian
sinA var float ;Output Sinus of the given Angle
cosA var float ;Output Cosinus of the given Angle
;GetBoogTan
BoogTanX var sword ;Input X
BoogTanY var sword ;Input Y
BoogTan var float ;Output BOOGTAN2(X/Y)
;Body position
BodyPosX var sbyte ;Global Input for the position of the body
BodyPosY var sword
BodyPosZ var sbyte
BodyPosXZ var sword
;Body Inverse Kinematics
BodyRotX var sbyte ;Global Input pitch of the body
BodyRotY var sbyte ;Global Input rotation of the body
BodyRotZ var sbyte ;Global Input roll of the body
PosX var sword ;Input position of the feet X
PosZ var sword ;Input position of the feet Z
PosY var sword ;Input position of the feet Y
RotationY var sbyte ;Input for rotation of a single feet for the gait
BodyOffsetX var sbyte ;Input Offset betweeen the body and Coxa X
BodyOffsetZ var sbyte ;Input Offset betweeen the body and Coxa Z
sinB var float ;Sin buffer for BodyRotX calculations
cosB var float ;Cos buffer for BodyRotX calculations
sinG var float ;Sin buffer for BodyRotZ calculations
cosG var float ;Cos buffer for BodyRotZ calculations
TotalX var sword ;Total X distance between the center of the body and the feet
TotalZ var sword ;Total Z distance between the center of the body and the feet
DistCenterBodyFeet var float ;Total distance between the center of the body and the feet
AngleCenterBodyFeetX var float ;Angle between the center of the body and the feet
BodyIKPosX var sword ;Output Position X of feet with Rotation
BodyIKPosY var sword ;Output Position Y of feet with Rotation
BodyIKPosZ var sword ;Output Position Z of feet with Rotation
;Leg Inverse Kinematics
IKFeetPosX var sword ;Input position of the Feet X
IKFeetPosY var sword ;Input position of the Feet Y
IKFeetPosZ var sword ;Input Position of the Feet Z
IKFeetPosXZ var sword ;Length between the coxa and feet
IKSW var float ;Length between shoulder and wrist
IKA1 var float ;Angle between SW line and the ground in rad
IKA2 var float ;?
IKSolution var bit ;Output true if the solution is possible
IKSolutionWarning var bit ;Output true if the solution is NEARLY possible
IKSolutionError var bit ;Output true if the solution is NOT possible
IKFemurAngle var sword ;Output Angle of Femur in degrees
IKTibiaAngle var sword ;Output Angle of Tibia in degrees
IKCoxaAngle var sword ;Output Angle of Coxa in degrees
;--------------------------------------------------------------------
;[Ps2 Controller]
DualShock var Byte(7)
LastButton var Byte(2)
DS2Mode var Byte
PS2Index var byte
BodyYShift var sbyte
;--------------------------------------------------------------------
;[TIMING]
lTimerWOverflowCnt var long ;used in WTimer overflow. Will keep a 16 bit overflow so we have a 32 bit timer
lCurrentTime var long
lTimerStart var long ;Start time of the calculation cycles
lTimerEnd var long ;End time of the calculation cycles
CycleTime var byte ;Total Cycle time
SSCTime var word ;Time for servo updates
PrevSSCTime var word ;Previous time for the servo updates
InputTimeDelay var byte ;Delay that depends on the input to get the "sneaking" effect
;--------------------------------------------------------------------
;[GLOABAL]
HexOn var bit ;Switch to turn on Phoenix
;--------------------------------------------------------------------
;[Balance]
BalanceMode var bit
TravelHeightY var sword
TotalTransX var sword
TotalTransZ var sword
TotalTransY var sword
TotalYbal var sword
TotalXBal var sword
TotalZBal var sword
TotalY var sword ;Total Y distance between the center of the body and the feet
;[gait]
GaitType var byte ;Gait type
GaitSpeed var byte ;Nominal speed of the gait
LegLiftHeight var byte ;Current Travel height
TravelLengthX var sword ;Current Travel length X
TravelLengthZ var sword ;Current Travel length Z
TravelLengthXZ Var sword ;Current Travel length X/Z
TravelRotationY var sword ;Current Travel Rotation Y
TLDivFactor var byte ;Number of steps that a leg is on the floor while walking
NrLiftedPos var nib ;Number of positions that a single leg is lifted (1-3)
HalfLiftHeigth var bit ;If TRUE the outer positions of the ligted legs will be half height
GaitInMotion var bit ;Temp to check if the gait is in motion
StepsInGait var byte ;Number of steps in gait
LastLeg var bit ;TRUE when the current leg is the last leg of the sequence
GaitStep var byte ;Actual Gait step
RFGaitLegNr var byte ;Init position of the leg
RMGaitLegNr var byte ;Init position of the leg
RRGaitLegNr var byte ;Init position of the leg
LFGaitLegNr var byte ;Init position of the leg
LMGaitLegNr var byte ;Init position of the leg
LRGaitLegNr var byte ;Init position of the leg
GaitLegNr var byte ;Input Number of the leg
TravelMulti var sbyte ;Multiplier for the length of the step
RFGaitPosX var sbyte ;Relative position corresponding to the Gait
RFGaitPosY var sbyte
RFGaitPosZ var sbyte
RFGaitRotY var sbyte ;Relative rotation corresponding to the Gait
RMGaitPosX var sbyte
RMGaitPosY var sbyte
RMGaitPosZ var sbyte
RMGaitRotY var sbyte
RRGaitPosX var sbyte
RRGaitPosY var sbyte
RRGaitPosZ var sbyte
RRGaitRotY var sbyte
LFGaitPosX var sbyte
LFGaitPosY var sbyte
LFGaitPosZ var sbyte
LFGaitRotY var sbyte
LMGaitPosX var sbyte
LMGaitPosY var sbyte
LMGaitPosZ var sbyte
LMGaitRotY var sbyte
LRGaitPosX var sbyte
LRGaitPosY var sbyte
LRGaitPosZ var sbyte
LRGaitRotY var sbyte
GaitPosX var sbyte ;In-/Output Pos X of feet
GaitPosY var sword ;In-/Output Pos Y of feet
GaitPosZ var sbyte ;In-/Output Pos Z of feet
GaitRotY var sbyte ;In-/Output Rotation Y of feet
;====================================================================
;[TIMER INTERRUPT INIT]
ONASMINTERRUPT TIMERWINT, Handle_TIMERW
;====================================================================
;[INIT]
;Turning off all the leds
LedA = 0
LedB = 0
LedC = 0
'Feet Positions [mm]
RFPosX = 78 ;Start positions of the Right Front leg
RFPosY = 0
RFPosZ = -45
RMPosX = 90 ;Start positions of the Right Middle leg
RMPosY = 0
RMPosZ = 0
RRPosX = 78 ;Start positions of the Right Rear leg
RRPosY = 0
RRPosZ = 45
LFPosX = 78 ;Start positions of the Left Front leg
LFPosY = 0
LFPosZ = -45
LMPosX = 90 ;Start positions of the Left Middle leg
LMPosY = 0
LMPosZ = 0
LRPosX = 78 ;Start positions of the Left Rear leg
LRPosY = 0
LRPosZ = 45
;Body Positions
BodyPosX = 0
BodyPosY = 0
BodyPosZ = 0
;Body Rotations
BodyRotX = 0
BodyRotY = 0
BodyRotZ = 0
;Gait
GaitType = 0
BalanceMode = 0
LegLiftHeight = 50
GaitStep = 1
GOSUB GaitSelect
;Timer
WTIMERTICSPERMS con 2000; we have 16 clocks per ms and we are incrementing every 8 so divide again by 2
TCRW = 0x30 ;clears TCNT and sets the timer to inc clock cycle / 8
TMRW = 0x80 ;starts the timer counting
lTimerWOverflowCnt = 0
enable TIMERWINT_OVF
;PS2 controller
high PS2CLK
LastButton(0) = 255
LastButton(1) = 255
BodyYShift = 0
;SSC
SSCTime = 150
HexOn = False
;====================================================================
;[MAIN]
main:
'Start time
GOSUB GetCurrentTime[], lTimerStart
'Reset IKsolution indicators
IKSolution = False
IKSolutionWarning = False
IKSolutionError = False
;Gait
GOSUB GaitSeq
;Balance calculations
TotalTransX = 0 'reset values used for calculation of balance
TotalTransZ = 0
TotalTransY = 0
TotalXBal = 0
TotalYBal = 0
TotalZBal = 0
IF (BalanceMode>0) THEN
gosub BalCalcOneLeg [-RFPosX+BodyPosX+RFGaitPosX, RFPosZ+BodyPosZ+RFGaitPosZ,RFGaitPosY, RFOffsetX, RFOffsetZ]
gosub BalCalcOneLeg [-RMPosX+BodyPosX+RMGaitPosX, RMPosZ+BodyPosZ+RMGaitPosZ,RMGaitPosY, RMOffsetX, RMOffsetZ]
gosub BalCalcOneLeg [-RRPosX+BodyPosX+RRGaitPosX, RRPosZ+BodyPosZ+RRGaitPosZ,RRGaitPosY, RROffsetX, RROffsetZ]
gosub BalCalcOneLeg [LFPosX-BodyPosX+LFGaitPosX, LFPosZ+BodyPosZ+LFGaitPosZ,LFGaitPosY, LFOffsetX, LFOffsetZ]
gosub BalCalcOneLeg [LMPosX-BodyPosX+LMGaitPosX, LMPosZ+BodyPosZ+LMGaitPosZ,LMGaitPosY, LMOffsetX, LMOffsetZ]
gosub BalCalcOneLeg [LRPosX-BodyPosX+LRGaitPosX, LRPosZ+BodyPosZ+LRGaitPosZ,LRGaitPosY, LROffsetX, LROffsetZ]
gosub BalanceBody
ENDIF
'Reset IKsolution indicators
IKSolution = False
IKSolutionWarning = False
IKSolutionError = False
;Right Front leg
GOSUB BodyIK [-RFPosX+BodyPosX+RFGaitPosX, RFPosZ+BodyPosZ+RFGaitPosZ,RFPosY+BodyPosY+BodyYShift+RFGaitPosY, RFOffsetX, RFOffsetZ, RFGaitRotY]
GOSUB LegIK [RFPosX-BodyPosX+BodyIKPosX-RFGaitPosX, RFPosY+BodyPosY+BodyYShift-BodyIKPosY+RFGaitPosY, RFPosZ+BodyPosZ-BodyIKPosZ+RFGaitPosZ]
RFCoxaAngle = IKCoxaAngle + CoxaAngle ;Angle for the basic setup for the front leg
RFFemurAngle = IKFemurAngle
RFTibiaAngle = IKTibiaAngle
;Right Middle leg
GOSUB BodyIK [-RMPosX+BodyPosX+RMGaitPosX, RMPosZ+BodyPosZ+RMGaitPosZ,RMPosY+BodyPosY+BodyYShift+RMGaitPosY, RMOffsetX, RMOffsetZ, RMGaitRotY]
GOSUB LegIK [RMPosX-BodyPosX+BodyIKPosX-RMGaitPosX, RMPosY+BodyPosY+BodyYShift-BodyIKPosY+RMGaitPosY, RMPosZ+BodyPosZ-BodyIKPosZ+RMGaitPosZ]
RMCoxaAngle = IKCoxaAngle
RMFemurAngle = IKFemurAngle
RMTibiaAngle = IKTibiaAngle
;Right Rear leg
GOSUB BodyIK [-RRPosX+BodyPosX+RRGaitPosX, RRPosZ+BodyPosZ+RRGaitPosZ,RRPosY+BodyPosY+BodyYShift+RRGaitPosY, RROffsetX, RROffsetZ, RRGaitRotY]
GOSUB LegIK [RRPosX-BodyPosX+BodyIKPosX-RRGaitPosX, RRPosY+BodyPosY+BodyYShift-BodyIKPosY+RRGaitPosY, RRPosZ+BodyPosZ-BodyIKPosZ+RRGaitPosZ]
RRCoxaAngle = IKCoxaAngle - CoxaAngle ;Angle for the basic setup for the front leg
RRFemurAngle = IKFemurAngle
RRTibiaAngle = IKTibiaAngle
;Left Front leg
GOSUB BodyIK [LFPosX-BodyPosX+LFGaitPosX, LFPosZ+BodyPosZ+LFGaitPosZ,LFPosY+BodyPosY+BodyYShift+LFGaitPosY, LFOffsetX, LFOffsetZ, LFGaitRotY]
GOSUB LegIK [LFPosX+BodyPosX-BodyIKPosX+LFGaitPosX, LFPosY+BodyPosY+BodyYShift-BodyIKPosY+LFGaitPosY, LFPosZ+BodyPosZ-BodyIKPosZ+LFGaitPosZ]
LFCoxaAngle = IKCoxaAngle + CoxaAngle ;Angle for the basic setup for the front leg
LFFemurAngle = IKFemurAngle
LFTibiaAngle = IKTibiaAngle
;Left Middle leg
GOSUB BodyIK [LMPosX-BodyPosX+LMGaitPosX, LMPosZ+BodyPosZ+LMGaitPosZ,LMPosY+BodyPosY+BodyYShift+LMGaitPosY, LMOffsetX, LMOffsetZ, LMGaitRotY]
GOSUB LegIK [LMPosX+BodyPosX-BodyIKPosX+LMGaitPosX, LMPosY+BodyPosY+BodyYShift-BodyIKPosY+LMGaitPosY, LMPosZ+BodyPosZ-BodyIKPosZ+LMGaitPosZ]
LMCoxaAngle = IKCoxaAngle
LMFemurAngle = IKFemurAngle
LMTibiaAngle = IKTibiaAngle
;Left Rear leg
GOSUB BodyIK [LRPosX-BodyPosX+LRGaitPosX, LRPosZ+BodyPosZ+LRGaitPosZ,LRPosY+BodyPosY+BodyYShift+LRGaitPosY, LROffsetX, LROffsetZ, LRGaitRotY]
GOSUB LegIK [LRPosX+BodyPosX-BodyIKPosX+LRGaitPosX, LRPosY+BodyPosY+BodyYShift-BodyIKPosY+LRGaitPosY, LRPosZ+BodyPosZ-BodyIKPosZ+LRGaitPosZ]
LRCoxaAngle = IKCoxaAngle - CoxaAngle ;Angle for the basic setup for the front leg
LRFemurAngle = IKFemurAngle
LRTibiaAngle = IKTibiaAngle
GOSUB HeadTracking
GOSUB CheckAngles
LedC = IKSolutionWarning
LedA = IKSolutionError
;Read input
GOSUB Ps2Input
;GOSUB ReadButtons ;I/O used by the PS2 remote
;GOSUB WriteLeds ;I/O used by the PS2 remote
;Get endtime and calculate wait time
GOSUB GetCurrentTime[], lTimerEnd
CycleTime = (lTimerEnd-lTimerStart)/WTIMERTICSPERMS
IF(HexOn)THEN
;Wait for previous commands to be completed while walking
IF(ABS(TravelLengthX)>TravelDeadZone | ABS(TravelLengthZ)>TravelDeadZone | ABS(TravelRotationY*2)>TravelDeadZone) THEN
pause (PrevSSCTime - CycleTime -50) MIN 1 ; Min 1 ensures that there alway is a value in the pause command
IF(BalanceMode=0)THEN
SSCTime = GaitSpeed + (InputTimeDelay*2)
ELSE
SSCTime = GaitSpeed + (InputTimeDelay*2) + 100
ENDIF
ELSE
SSCTime = 200 ;GaitSpeed
ENDIF
GOSUB ServoDriver
ELSE
;Turn off
GOSUB FreeServos
ENDIF
goto main
;-------------------------------------------------------------------
;[Head Tracking]
HeadTracking:
;Return to the middle position
; HeadPanAngle=0
;IF (ABS(TravelLengthX)>TravelDeadZone | ABS(TravelLengthZ)>TravelDeadZone | ABS(TravelRotationY*2)>TravelDeadZone) THEN
;Calculate walking direction X and Z
; TravelLengthXZ = SQR((TravelLengthX * TravelLengthX) + TravelLengthZ * TravelLengthZ)
; HeadPanAngle = TOINT(FACOS(TOFLOAT(TravelLengthZ) / TOFLOAT(TravelLengthXZ)) * 180.0 / 3.141592)-180
;Add sign depending on the direction of X
; HeadPanAngle = HeadPanAngle * (TravelLengthX/ABS(TravelLengthX))
; ENDIF
;Calculate body angle depending on rotation
; IF ABS(TravelRotationY*2)>TravelDeadZone & ABS(TravelRotationY*3) > ABS(HeadPanAngle) THEN
; HeadPanAngle = -TravelRotationY*3 ; Rotation max = 16*6 to get max range of 90 deg.
; ENDIF
;Return to the middle position
HeadPanAngle=0
IF (ABS(BodyRotY)>TravelDeadZone | ABS(BodyRotY)>TravelDeadZone | ABS(TravelRotationY*2)>TravelDeadZone) THEN
;Calculate direction Z and X
HeadPanAngle = SQR((BodyRotY * BodyRotY) + BodyYShift * BodyYShift)
;Add sign depending on the direction of X
HeadPanAngle = HeadPanAngle * (BodyRotY/ABS(BodyRotY))
ENDIF
;Calculate body angle depending on rotation
IF ABS(TravelRotationY*2)>TravelDeadZone & ABS(TravelRotationY*3) > ABS(HeadPanAngle) THEN
HeadPanAngle = -TravelRotationY*3 ; Rotation max = 16*6 to get max range of 90 deg.
ENDIF
;----------------
;Return to the middle position
; HeadTiltAngle=0
;IF (ABS(TravelLengthZ)>TravelDeadZone | ABS(TravelLengthX)>TravelDeadZone | ABS(TravelRotationY*2)>TravelDeadZone) THEN
;Calculate walking direction Z and X
; TravelLengthXZ = SQR((TravelLengthZ * TravelLengthZ) + TravelLengthX * TravelLengthX)
; HeadTiltAngle = TOINT(FACOS(TOFLOAT(TravelLengthX) / TOFLOAT(TravelLengthXZ)) * 180.0 / 3.141592)-180
;Add sign depending on the direction of Z
; HeadTiltAngle = HeadTiltAngle * (TravelLengthZ/ABS(TravelLengthZ))
; ENDIF
;Calculate body angle depending on rotation
; IF ABS(TravelRotationY*2)>TravelDeadZone & ABS(TravelRotationY*3) > ABS(HeadTiltAngle) THEN
; HeadTiltAngle = -TravelRotationY*3 ; Rotation max = 16*6 to get max range of 90 deg.
; ENDIF
;Return to the middle position
HeadTiltAngle=0
IF (ABS(BodyYShift)>TravelDeadZone | ABS(BodyRotY)>TravelDeadZone | ABS(TravelRotationY*2)>TravelDeadZone) THEN
;Calculate direction Z and X
HeadTiltAngle = SQR((BodyRotY * BodyRotY) + BodyYShift * BodyYShift)
;Add sign depending on the direction of Z
HeadTiltAngle = HeadTiltAngle * (BodyYShift/ABS(BodyYShift))
ENDIF
;Calculate body angle depending on rotation
IF ABS(TravelRotationY*2)>TravelDeadZone & ABS(TravelRotationY*3) > ABS(HeadTiltAngle) THEN
HeadTiltAngle = -TravelRotationY*3 ; Rotation max = 16*6 to get max range of 90 deg.
ENDIF
return
;====================================================================
;[ReadButtons] Reading input buttons from the ABB
ReadButtons:
input P4
input P5
input P6
prev_butA = butA
prev_butB = butB
prev_butC = butC
butA = IN4
butB = IN5
butC = IN6
return
;--------------------------------------------------------------------
;[WriteLEDs] Updates the state of the leds
WriteLEDs:
if ledA = 1 THEN
low p4
ENDIF
if ledB = 1 THEN
low p5
ENDIF
if ledC = 1 THEN
low p6
ENDIF
return
;--------------------------------------------------------------------
;[PS2Input] reads the input data from the Wiiremote and processes the
;data to the parameters.
Ps2Input:
low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,[$1\8]
shiftin PS2DAT,PS2CLK,FASTLSBPOST,[DS2Mode\8]
high PS2SEL
pause 1
low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,[$1\8,$42\8]
shiftin PS2DAT,PS2CLK,FASTLSBPOST,[DualShock(0)\8, DualShock(1)\8, DualShock(2)\8, DualShock(3)\8, |
DualShock(4)\8, DualShock(5)\8, DualShock(6)\8]
high PS2SEL
pause 1
if DS2Mode <> PadMode THEN
low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,[$1\8,$43\8,$0\8,$1\8,$0\8] ;CONFIG_MODE_ENTER
high PS2SEL
pause 1
low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,[$01\8,$44\8,$00\8,$01\8,$03\8,$00\8,$00\8,$00\8,$00\8] ;SET_MODE_AND_LOCK
high PS2SEL
pause 1
low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,[$01\8,$4F\8,$00\8,$FF\8,$FF\8,$03\8,$00\8,$00\8,$00\8] ;SET_DS2_NATIVE_MODE
high PS2SEL
pause 1
low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,[$01\8,$43\8,$00\8,$00\8,$5A\8,$5A\8,$5A\8,$5A\8,$5A\8] ;CONFIG_MODE_EXIT_DS2_NATIVE
high PS2SEL
pause 1
low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,[$01\8,$43\8,$00\8,$00\8,$00\8,$00\8,$00\8,$00\8,$00\8] ;CONFIG_MODE_EXIT
high PS2SEL
pause 100
sound P9,[100\4000, 100\4500, 100\5000]
return
ENDIF
;------------------------------------------------------------
IF (DualShock(1).bit3 = 0) and LastButton(0).bit3 THEN ;Start Button test
IF(HexOn) THEN
'Turn off
Sound P9,[100\5000]
BodyPosX = 0
BodyPosY = 0
BodyPosZ = 0
BodyRotX = 0
BodyRotY = 0
BodyRotZ = 0
TravelLengthX = 0
TravelLengthZ = 0
TravelRotationY = 0
SSCTime = 600
GOSUB ServoDriver
HexOn = False
ELSE
'Turn on
Sound P9,[60\4000]
SSCTime = 200
HexOn = True
ENDIF
ENDIF
;-------------------
IF HexOn THEN
IF (DualShock(1).bit0 = 0) and LastButton(0).bit0 THEN ;Select Button test
IF TravelLengthX=0 & TravelLengthZ=0 & TravelRotationY=0 THEN
;Switch to next Gait type
IF GaitType<5 THEN
Sound P9,[50\4000]
GaitType = GaitType+1
ELSE
Sound P9,[50\4000, 50\4500]
GaitType = 0
ENDIF
GOSUB GaitSelect
ENDIF
ENDIF
;-------------------
IF (DualShock(1).bit4 = 0) and LastButton(0).bit4 THEN ;Up Button test
BodyPosY = BodyPosY+10 ;adjust pos hight
ENDIF
;-------------------
IF (DualShock(1).bit6 = 0) and LastButton(0).bit6 THEN ;Down Button test
BodyPosY = BodyPosY-10 ;adjust pos hight
ENDIF
;-------------------
IF (DualShock(2).bit4 = 0) and LastButton(1).bit4 THEN ;Triangle Button test
BodyPosY = 120 ;walk pos
ENDIF
;-------------------
IF (DualShock(2).bit5 = 0) and LastButton(1).bit5 THEN ;Circle Button test
BodyPosY = 0 ;crouch pos
ENDIF
;-------------------
IF (DualShock(2).bit6 = 0) THEN ;Cross Button test
GOSUB ATTACK
ENDIF
;-------------------
IF (DualShock(2).bit7 = 0) and LastButton(1).bit7 THEN ;Square Button test
IF BalanceMode = 0 THEN
BalanceMode = 1
sound P9,[100\4000, 50\8000]
ELSE
BalanceMode = 0
sound P9,[250\3000]
ENDIF
ENDIF
;-------------------
; if (DualShock(1).bit1 = 0) then ;L3 Button test
; ENDIF
;-------------------
; if (DualShock(1).bit2 = 0) then ;R3 Button test
; ENDIF
;-------------------
BodyYShift = 0
IF (DualShock(2).bit3 = 0) THEN ;R1 Button test
HeadPanAngle = (Dualshock(3) - 128)/2 ;X-axis Right stick horz
HeadTiltAngle = (Dualshock(4) - 128)/2 ;Z-axis Right stick vert
ENDIF
;-------------------
BodyYShift = 0
IF (DualShock(2).bit2 = 0) THEN ;L1 Button test
BodyPosX = (Dualshock(5) - 128)/2 ;X-axis Left stick horz
BodyPosZ = -(Dualshock(6) - 128)/3 ;Z-axis Left stick vert
BodyRotY = (Dualshock(3) - 128)/6
BodyYShift = (-(Dualshock(4) - 128)/2)MIN-(BodyPosY-10)
;--------
ELSEIF (DualShock(2).bit0 = 0) ;L2 Button test
BodyRotX = (Dualshock(6) - 128)/8
BodyRotY = (Dualshock(3) - 128)/6
BodyRotZ = (Dualshock(5) - 128)/8
BodyYShift = (-(Dualshock(4) - 128)/2)MIN-(BodyPosY-10)
ELSE ;Walk
'BodyPosX = 0
'BodyPosY = 0
'BodyPosZ = 0
'BodyRotX = 0
'BodyRotY = 0
'BodyRotZ = 0
;-------------------
IF (DualShock(2).bit1 = 0) THEN ;R2 Button test
TravelLengthX = -(Dualshock(5) - 128)
TravelLengthZ = (Dualshock(6) - 128)
ELSE
TravelLengthX = -(Dualshock(5) - 128)/2
TravelLengthZ = (Dualshock(6) - 128)/2
ENDIF
TravelRotationY = -(Dualshock(3) - 128)/4
ENDIF
;Calculate walking time delay
InputTimeDelay = 128 - (ABS((Dualshock(5) - 128)) MIN ABS((Dualshock(6) - 128))) MIN ABS((Dualshock(3) - 128))
ENDIF
LastButton(0) = DualShock(1)
LastButton(1) = DualShock(2)
return
;--------------------------------------------------------------------
;[GAIT Select]
GaitSelect
;Gait selector
IF (GaitType = 0) THEN ;Ripple Gait 6 steps
LRGaitLegNr = 1
RFGaitLegNr = 2
LMGaitLegNr = 3
RRGaitLegNr = 4
LFGaitLegNr = 5
RMGaitLegNr = 6
NrLiftedPos = 1
TLDivFactor = 4
StepsInGait = 6
GaitSpeed = 200
ENDIF
IF (GaitType = 1) THEN ;Quadripple 6 steps
LRGaitLegNr = 3
RFGaitLegNr = 3
LMGaitLegNr = 5
RRGaitLegNr = 5
LFGaitLegNr = 1
RMGaitLegNr = 1
sound P9,[50\1000]
NrLiftedPos = 2
HalfLiftHeigth = FALSE
TLDivFactor = 6
StepsInGait = 6
GaitSpeed = 200
ENDIF
IF (GaitType = 2) THEN ;Tripod 4 steps
LRGaitLegNr = 3
RFGaitLegNr = 1
LMGaitLegNr = 1
RRGaitLegNr = 1
LFGaitLegNr = 3
RMGaitLegNr = 3
sound P9,[50\2000]
NrLiftedPos = 1
TLDivFactor = 2
StepsInGait = 4
GaitSpeed = 200
ENDIF
IF (GaitType = 3) THEN ;Wave 12 steps
LRGaitLegNr = 7
RFGaitLegNr = 1
LMGaitLegNr = 9
RRGaitLegNr = 5
LFGaitLegNr = 11
RMGaitLegNr = 3
sound P9,[50\3000]
NrLiftedPos = 1
HalfLiftHeigth = FALSE
TLDivFactor = 10
StepsInGait = 12
GaitSpeed = 200
ENDIF
IF (GaitType = 4) THEN ;Ripple 22 steps
LRGaitLegNr = 3
RFGaitLegNr = 3
LMGaitLegNr = 5
RRGaitLegNr = 5
LFGaitLegNr = 1
RMGaitLegNr = 1
sound P9,[50\4000]
NrLiftedPos = 2
HalfLiftHeigth = FALSE
TLDivFactor = 6
StepsInGait = 6
GaitSpeed = 200
ENDIF
IF (GaitType = 5) THEN ;TriRipple Gait 6 steps
LRGaitLegNr = 1
RFGaitLegNr = 5
LMGaitLegNr = 3
RRGaitLegNr = 1
LFGaitLegNr = 5
RMGaitLegNr = 3
sound P9,[50\5000]
NrLiftedPos = 1
TLDivFactor = 3
StepsInGait = 6
GaitSpeed = 200
ENDIF
return
;--------------------------------------------------------------------
;[GAIT Sequence]
GaitSeq
;Calculate Gait sequence
LastLeg = FALSE
GOSUB Gait [LRGaitLegNr, LRGaitPosX, LRGaitPosY, LRGaitPosZ, LRGaitRotY]
LRGaitPosX = GaitPosX
LRGaitPosY = GaitPosY
LRGaitPosZ = GaitPosZ
LRGaitRotY = GaitRotY
GOSUB Gait [RFGaitLegNr, RFGaitPosX, RFGaitPosY, RFGaitPosZ, RFGaitRotY]
RFGaitPosX = GaitPosX
RFGaitPosY = GaitPosY
RFGaitPosZ = GaitPosZ
RFGaitRotY = GaitRotY
GOSUB Gait [LMGaitLegNr, LMGaitPosX, LMGaitPosY, LMGaitPosZ, LMGaitRotY]
LMGaitPosX = GaitPosX
LMGaitPosY = GaitPosY
LMGaitPosZ = GaitPosZ
LMGaitRotY = GaitRotY
GOSUB Gait [RRGaitLegNr, RRGaitPosX, RRGaitPosY, RRGaitPosZ, RRGaitRotY]
RRGaitPosX = GaitPosX
RRGaitPosY = GaitPosY
RRGaitPosZ = GaitPosZ
RRGaitRotY = GaitRotY
GOSUB Gait [LFGaitLegNr, LFGaitPosX, LFGaitPosY, LFGaitPosZ, LFGaitRotY]
LFGaitPosX = GaitPosX
LFGaitPosY = GaitPosY
LFGaitPosZ = GaitPosZ
LFGaitRotY = GaitRotY
LastLeg = TRUE
GOSUB Gait [RMGaitLegNr, RMGaitPosX, RMGaitPosY, RMGaitPosZ, RMGaitRotY]
RMGaitPosX = GaitPosX
RMGaitPosY = GaitPosY
RMGaitPosZ = GaitPosZ
RMGaitRotY = GaitRotY
return
;--------------------------------------------------------------------
;[GAIT]
Gait [GaitLegNr, GaitPosX, GaitPosY, GaitPosZ, GaitRotY]
;Check IF the Gait is in motion
GaitInMotion = ((ABS(TravelLengthX)>TravelDeadZone) | (ABS(TravelLengthZ)>TravelDeadZone) | (ABS(TravelRotationY)>TravelDeadZone) )
;Leg middle up position
;Gait in motion Gait NOT in motion, return to home position
IF (GaitInMotion & (NrLiftedPos=1 | NrLiftedPos=3) & GaitStep=GaitLegNr) | (GaitInMotion=FALSE & GaitStep=GaitLegNr & ((ABS(GaitPosX)>2) | (ABS(GaitPosZ)>2) | (ABS(GaitRotY)>2))) THEN ;Up
GaitPosX = 0
GaitPosY = -LegLiftHeight
GaitPosZ = 0
GaitRotY = 0
ELSE
;Optional Half heigth Rear
IF ((NrLiftedPos=2 & GaitStep=GaitLegNr) | (NrLiftedPos=3 & (GaitStep=GaitLegNr-1 | GaitStep=GaitLegNr+(StepsInGait-1)))) & GaitInMotion THEN
GaitPosX = -TravelLengthX/2
GaitPosY = -LegLiftHeight/(HalfLiftHeigth+1)
GaitPosZ = -TravelLengthZ/2
GaitRotY = -TravelRotationY/2
ELSE
;Optional half heigth front
IF (NrLiftedPos>=2) & (GaitStep=GaitLegNr+1 | GaitStep=GaitLegNr-(StepsInGait-1)) & GaitInMotion THEN
GaitPosX = TravelLengthX/2
GaitPosY = -LegLiftHeight/(HalfLiftHeigth+1)
GaitPosZ = TravelLengthZ/2
GaitRotY = TravelRotationY/2
ELSE
;Leg front down position
IF (GaitStep=GaitLegNr+NrLiftedPos | GaitStep=GaitLegNr-(StepsInGait-NrLiftedPos)) & GaitPosY<0 THEN
GaitPosX = TravelLengthX/2
GaitPosY = 0
GaitPosZ = TravelLengthZ/2
GaitRotY = TravelRotationY/2
;Move body forward
ELSE
GaitPosX = GaitPosX - (TravelLengthX/TLDivFactor)
GaitPosY = 0
GaitPosZ = GaitPosZ - (TravelLengthZ/TLDivFactor)
GaitRotY = GaitRotY - (TravelRotationY/TLDivFactor)
ENDIF
ENDIF
ENDIF
ENDIF
;Advance to the next step
IF LastLeg THEN ;The last leg in this step
GaitStep = GaitStep+1
IF GaitStep>StepsInGait THEN
GaitStep = 1
ENDIF
ENDIF
return
;--------------------------------------------------------------------
;[BalCalcOneLeg]
BalCalcOneLeg [PosX, PosZ, PosY, BodyOffsetX, BodyOffsetZ]
;Calculating totals from center of the body to the feet
TotalZ = BodyOffsetZ+PosZ
TotalX = BodyOffsetX+PosX
TotalY = 150 + PosY' using the value 150 to lower the centerpoint of rotation 'BodyPosY +
TotalTransY = TotalTransY + PosY
TotalTransZ = TotalTransZ + TotalZ
TotalTransX = TotalTransX + TotalX
gosub GetBoogTan [TotalX, TotalZ]
TotalYbal = TotalYbal + TOINT((BoogTan*180.0) / 3.141592)
gosub GetBoogTan [TotalX, TotalY]
TotalZbal = TotalZbal + TOINT((BoogTan*180.0) / 3.141592)
gosub GetBoogTan [TotalZ, TotalY]
TotalXbal = TotalXbal + TOINT((BoogTan*180.0) / 3.141592)
'serout S_OUT, i9600, ["BalOneLeg PosX=", sdec PosX," PosZ=", sdec PosZ," TotalXTransZ=", sdec TotalTransZ, 13]
return
;--------------------------------------------------------------------
;[BalanceBody]
BalanceBody:
TotalTransZ = TotalTransZ/6
TotalTransX = TotalTransX/6
TotalTransY = TotalTransY/6
if TotalYbal < -180 then 'Tangens fix caused by +/- 180 deg
TotalYbal = TotalYbal + 360
endif
if TotalZbal < -180 then 'Tangens fix caused by +/- 180 deg
TotalZbal = TotalZbal + 360
endif
if TotalXbal < -180 then 'Tangens fix caused by +/- 180 deg
TotalXbal = TotalXbal + 360
endif
;Balance rotation
TotalYBal = TotalYbal/6
TotalXBal = TotalXbal/6
TotalZBal = -TotalZbal/6
;Balance translation
LFGaitPosZ = LFGaitPosZ - TotalTransZ
LMGaitPosZ = LMGaitPosZ - TotalTransZ
LRGaitPosZ = LRGaitPosZ - TotalTransZ
RFGaitPosZ = RFGaitPosZ - TotalTransZ
RMGaitPosZ = RMGaitPosZ - TotalTransZ
RRGaitPosZ = RRGaitPosZ - TotalTransZ
LFGaitPosX = LFGaitPosX - TotalTransX
LMGaitPosX = LMGaitPosX - TotalTransX
LRGaitPosX = LRGaitPosX - TotalTransX
RFGaitPosX = RFGaitPosX - TotalTransX
RMGaitPosX = RMGaitPosX - TotalTransX
RRGaitPosX = RRGaitPosX - TotalTransX
LFGaitPosY = LFGaitPosY - TotalTransY
LMGaitPosY = LMGaitPosY - TotalTransY
LRGaitPosY = LRGaitPosY - TotalTransY
RFGaitPosY = RFGaitPosY - TotalTransY
RMGaitPosY = RMGaitPosY - TotalTransY
RRGaitPosY = RRGaitPosY - TotalTransY
return
;--------------------------------------------------------------------
;[GETSINCOS] Get the sinus and cosinus from the angle +/- multiple circles
;AngleDeg - Input Angle in degrees
;SinA - Output Sinus of AngleDeg
;CosA - Output Cosinus of AngleDeg
GetSinCos [AngleDeg]
;Get the absolute value of AngleDeg
IF AngleDeg < 0.0 THEN
ABSAngleDeg = AngleDeg *-1.0
ELSE
ABSAngleDeg = AngleDeg
ENDIF
;Shift rotation to a full circle of 360 deg -> AngleDeg // 360
IF AngleDeg < 0.0 THEN ;Negative values
AngleDeg = 360.0-(ABSAngleDeg-TOFLOAT(360*(TOINT(ABSAngleDeg/360.0))))
ELSE ;Positive values
AngleDeg = ABSAngleDeg-TOFLOAT(360*(TOINT(ABSAngleDeg/360.0)))
ENDIF
IF AngleDeg < 180.0 THEN ;Angle between 0 and 180
;Subtract 90 to shift range
AngleDeg = AngleDeg -90.0
;Convert degree to radials
AngleRad = (AngleDeg*3.141592)/180.0
SinA = FCOS(AngleRad) ;Sin o to 180 deg = cos(Angle Rad - 90deg)
CosA = -FSIN(AngleRad) ;Cos 0 to 180 deg = -sin(Angle Rad - 90deg)
ELSE ;Angle between 180 and 360
;Subtract 270 to shift range
AngleDeg = AngleDeg -270.0
;Convert degree to radials
AngleRad = (AngleDeg*3.141592)/180.0
SinA = -FCOS(AngleRad) ;Sin 180 to 360 deg = -cos(Angle Rad - 270deg)
CosA = FSIN(AngleRad) ;Cos 180 to 360 deg = sin(Angle Rad - 270deg)
ENDIF
return
;--------------------------------------------------------------------
;[BOOGTAN2] Gets the Inverse Tangus from X/Y with the where Y can be zero or negative
;BoogTanX - Input X
;BoogTanY - Input Y
;BoogTan - Output BOOGTAN2(X/Y)
GetBoogTan [BoogTanX, BoogTanY]
IF(BoogTanX = 0) THEN ; X=0 -> 0 or PI
IF(BoogTanY >= 0) THEN
BoogTan = 0.0
ELSE
BoogTan = 3.141592
ENDIF
ELSE
IF(BoogTanY = 0) THEN ; Y=0 -> +/- Pi/2
IF(BoogTanX > 0) THEN
BoogTan = 3.141592 / 2.0
ELSE
BoogTan = -3.141592 / 2.0
ENDIF
ELSE
IF(BoogTanY > 0) THEN ;BOOGTAN(X/Y)
BoogTan = FATAN(TOFLOAT(BoogTanX) / TOFLOAT(BoogTanY))
ELSE
IF(BoogTanX > 0) THEN ;BOOGTAN(X/Y) + PI
BoogTan = FATAN(TOFLOAT(BoogTanX) / TOFLOAT(BoogTanY)) + 3.141592
ELSE ;BOOGTAN(X/Y) - PI
BoogTan = FATAN(TOFLOAT(BoogTanX) / TOFLOAT(BoogTanY)) - 3.141592
ENDIF
ENDIF
ENDIF
ENDIF
return
;--------------------------------------------------------------------
;[BODY INVERSE KINEMATICS]
;BodyRotX - Global Input pitch of the body
;BodyRotY - Global Input rotation of the body
;BodyRotZ - Global Input roll of the body
;RotationY - Input Rotation for the gait
;PosX - Input position of the feet X
;PosZ - Input position of the feet Z
;BodyOffsetX - Input Offset betweeen the body and Coxa X
;BodyOffsetZ - Input Offset betweeen the body and Coxa Z
;SinB - Sin buffer for BodyRotX
;CosB - Cos buffer for BodyRotX
;SinG - Sin buffer for BodyRotZ
;CosG - Cos buffer for BodyRotZ
;BodyIKPosX - Output Position X of feet with Rotation
;BodyIKPosY - Output Position Y of feet with Rotation
;BodyIKPosZ - Output Position Z of feet with Rotation
BodyIK [PosX, PosZ, PosY, BodyOffsetX, BodyOffsetZ, RotationY]
;Calculating totals from center of the body to the feet
TotalZ = BodyOffsetZ+PosZ
TotalX = BodyOffsetX+PosX
;PosY are equal to a "TotalY"
;Successive global rotation matrix:
;Math shorts for rotation: Alfa (A) = Xrotate, Beta (B) = Zrotate, Gamma (G) = Yrotate
;Sinus Alfa = sinA, cosinus Alfa = cosA. and so on...
;First calculate sinus and cosinus for each rotation:
GOSUB GetSinCos [TOFLOAT(BodyRotX+TotalXBal)]
SinG = SinA
CosG = CosA
GOSUB GetSinCos [TOFLOAT(BodyRotZ+TotalZBal)]
SinB = SinA
CosB = CosA
GOSUB GetSinCos [TOFLOAT(BodyRotY+RotationY+TotalYBal)]
;Calcualtion of rotation matrix:
BodyIKPosX = TotalX-TOINT(TOFLOAT(TotalX)*CosA*CosB - TOFLOAT(TotalZ)*CosB*SinA + TOFLOAT(PosY)*SinB)
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)
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)
return
;--------------------------------------------------------------------
;[LEG INVERSE KINEMATICS] Calculates the angles of the tibia and femur for the given position of the feet
;IKFeetPosX - Input position of the Feet X
;IKFeetPosY - Input position of the Feet Y
;IKFeetPosZ - Input Position of the Feet Z
;IKSolution - Output true IF the solution is possible
;IKSolutionWarning - Output true IF the solution is NEARLY possible
;IKSolutionError - Output true IF the solution is NOT possible
;IKFemurAngle - Output Angle of Femur in degrees
;IKTibiaAngle - Output Angle of Tibia in degrees
;IKCoxaAngle - Output Angle of Coxa in degrees
LegIK [IKFeetPosX, IKFeetPosY, IKFeetPosZ]
;Length between the Coxa and Feet
IKFeetPosXZ = TOINT(FSQRT(TOFLOAT((IKFeetPosX*IKFeetPosX)+(IKFeetPosZ*IKFeetPosZ))))
;IKSW - Length between shoulder and wrist
IKSW = FSQRT(TOFLOAT(((IKFeetPosXZ-CoxaLength)*(IKFeetPosXZ-CoxaLength))+(IKFeetPosY*IKFeetPosY)))
;IKA1 - Angle between SW line and the ground in rad
GOSUB GetBoogTan [IKFeetPosXZ-CoxaLength, IKFeetPosY]
IKA1 = BoogTan
;IKA2 - ?
IKA2 = FACOS((TOFLOAT((FemurLength*FemurLength) - (TibiaLength*TibiaLength)) + (IKSW*IKSW)) / (TOFLOAT(2*Femurlength) * IKSW))
;IKFemurAngle
IKFemurAngle = (TOINT(((IKA1 + IKA2) * 180.0) / 3.141592)*-1)+90
;IKTibiaAngle
IKTibiaAngle = (90-TOINT(((FACOS((TOFLOAT((FemurLength*FemurLength) + (TibiaLength*TibiaLength)) - (IKSW*IKSW)) / TOFLOAT(2*Femurlength*TibiaLength)))*180.0) / 3.141592)) * -1
;IKCoxaAngle
GOSUB GetBoogTan [IKFeetPosZ, IKFeetPosX]
IKCoxaAngle = TOINT((BoogTan*180.0) / 3.141592)
;Set the Solution quality
IF(IKSW < TOFLOAT(FemurLength+TibiaLength-30)) THEN
IKSolution = TRUE
ELSE
IF(IKSW < TOFLOAT(FemurLength+TibiaLength)) THEN
IKSolutionWarning = TRUE
ELSE
IKSolutionError = TRUE
ENDIF
ENDIF
return
;--------------------------------------------------------------------
;[CHECK ANGLES] Checks the mechanical limits of the servos
CheckAngles:
RFCoxaAngle = (RFCoxaAngle min RFCoxa_MIN) max RFCoxa_MAX
RFFemurAngle = (RFFemurAngle min RFFemur_MIN) max RFFemur_MAX
RFTibiaAngle = (RFTibiaAngle min RFTibia_MIN) max RFTibia_MAX
RMCoxaAngle = (RMCoxaAngle min RMCoxa_MIN) max RMCoxa_MAX
RMFemurAngle = (RMFemurAngle min RMFemur_MIN) max RMFemur_MAX
RMTibiaAngle = (RMTibiaAngle min RMTibia_MIN) max RMTibia_MAX
RRCoxaAngle = (RRCoxaAngle min RRCoxa_MIN) max RRCoxa_MAX
RRFemurAngle = (RRFemurAngle min RRFemur_MIN) max RRFemur_MAX
RRTibiaAngle = (RRTibiaAngle min RRTibia_MIN) max RRTibia_MAX
LFCoxaAngle = (LFCoxaAngle min LFCoxa_MIN) max LFCoxa_MAX
LFFemurAngle = (LFFemurAngle min LFFemur_MIN) max LFFemur_MAX
LFTibiaAngle = (LFTibiaAngle min LFTibia_MIN) max LFTibia_MAX
LMCoxaAngle = (LMCoxaAngle min LMCoxa_MIN) max LMCoxa_MAX
LMFemurAngle = (LMFemurAngle min LMFemur_MIN) max LMFemur_MAX
LMTibiaAngle = (LMTibiaAngle min LMTibia_MIN) max LMTibia_MAX
LRCoxaAngle = (LRCoxaAngle min LRCoxa_MIN) max LRCoxa_MAX
LRFemurAngle = (LRFemurAngle min LRFemur_MIN) max LRFemur_MAX
LRTibiaAngle = (LRTibiaAngle min LRTibia_MIN) max LRTibia_MAX
HeadPanAngle = (HeadPanAngle min HeadPan_MIN) max HeadPan_MAX
HeadTiltAngle = (HeadTiltAngle min HeadTilt_MIN) max HeadTilt_MAX
return
;--------------------------------------------------------------------
;[SERVO DRIVER] Updates the positions of the servos
ServoDriver:
;Front Right leg
serout SSC_OUT,SSC_BAUTE,["#",dec RFCoxaPin,"P",dec TOINT(TOFLOAT(-RFCoxaAngle +90)/0.10588238)+650]
serout SSC_OUT,SSC_BAUTE,["#",dec RFFemurPin,"P",dec TOINT(TOFLOAT(-RFFemurAngle+90)/0.10588238)+650]
serout SSC_OUT,SSC_BAUTE,["#",dec RFTibiaPin,"P",dec TOINT(TOFLOAT(-RFTibiaAngle+90)/0.10588238)+650]
;Middle Right leg
serout SSC_OUT,SSC_BAUTE,["#",dec RMCoxaPin,"P",dec TOINT(TOFLOAT(-RMCoxaAngle +90)/0.10588238)+650]
serout SSC_OUT,SSC_BAUTE,["#",dec RMFemurPin,"P",dec TOINT(TOFLOAT(-RMFemurAngle+90)/0.10588238)+650]
serout SSC_OUT,SSC_BAUTE,["#",dec RMTibiaPin,"P",dec TOINT(TOFLOAT(-RMTibiaAngle+90)/0.10588238)+650]
;Rear Right leg
serout SSC_OUT,SSC_BAUTE,["#",dec RRCoxaPin,"P",dec TOINT(TOFLOAT(-RRCoxaAngle +90)/0.10588238)+650]
serout SSC_OUT,SSC_BAUTE,["#",dec RRFemurPin,"P",dec TOINT(TOFLOAT(-RRFemurAngle+90)/0.10588238)+650]
serout SSC_OUT,SSC_BAUTE,["#",dec RRTibiaPin,"P",dec TOINT(TOFLOAT(-RRTibiaAngle+90)/0.10588238)+650]
;Front Left leg
serout SSC_OUT,SSC_BAUTE,["#",dec LFCoxaPin,"P",dec TOINT(TOFLOAT(LFCoxaAngle +90)/0.10588238)+650]
serout SSC_OUT,SSC_BAUTE,["#",dec LFFemurPin,"P",dec TOINT(TOFLOAT(LFFemurAngle+90)/0.10588238)+650]
serout SSC_OUT,SSC_BAUTE,["#",dec LFTibiaPin ,"P",dec TOINT(TOFLOAT(LFTibiaAngle+90)/0.10588238)+650]
;Middle Left leg
serout SSC_OUT,SSC_BAUTE,["#",dec LMCoxaPin,"P",dec TOINT(TOFLOAT(LMCoxaAngle +90)/0.10588238)+650]
serout SSC_OUT,SSC_BAUTE,["#",dec LMFemurPin,"P",dec TOINT(TOFLOAT(LMFemurAngle+90)/0.10588238)+650]
serout SSC_OUT,SSC_BAUTE,["#",dec LMTibiaPin,"P",dec TOINT(TOFLOAT(LMTibiaAngle+90)/0.10588238)+650]
;Rear Left leg
serout SSC_OUT,SSC_BAUTE,["#",dec LRCoxaPin,"P",dec TOINT(TOFLOAT(LRCoxaAngle +90)/0.10588238)+650]
serout SSC_OUT,SSC_BAUTE,["#",dec LRFemurPin,"P",dec TOINT(TOFLOAT(LRFemurAngle+90)/0.10588238)+650]
serout SSC_OUT,SSC_BAUTE,["#",dec LRTibiaPin,"P",dec TOINT(TOFLOAT(LRTibiaAngle+90)/0.10588238)+650]
Serout SSC_OUT,SSC_BAUTE,["#",dec HeadPan,"P",dec (-HeadPanAngle+90)*10000/1059+650]
Serout SSC_OUT,SSC_BAUTE,["#",dec HeadTilt,"P",dec (-HeadTiltAngle+90)*10000/1059+650]
;Send <CR>
serout SSC_OUT,SSC_BAUTE,["T",dec SSCTime,13]
PrevSSCTime = SSCTime
return
;--------------------------------------------------------------------
ATTACK: ;Attack mode
IF TravelLengthX=0 & TravelLengthZ=0 & TravelRotationY=0 THEN
;Attack posture
serout SSC_OUT,SSC_BAUTE,["#",DEC RMCoxaPin,"P1800#",DEC RMFemurPin,"P1700#",DEC RMTibiaPin,"P1700#",DEC LMCoxaPin,"P1200#", |
DEC LMFemurPin,"P1300#",DEC LMTibiaPin,"P1300T288",13]
pause 500
serout SSC_OUT,SSC_BAUTE,["#",DEC RMFemurPin,"P1500#",DEC RMTibiaPin,"P1500#",DEC LMFemurPin,"P1500#",DEC LMTibiaPin,"P1500T288",13]
pause 500
serout SSC_OUT,SSC_BAUTE,["#",DEC RRFemurPin,"P1800#",DEC RRTibiaPin,"P1800#",DEC RMFemurPin,"P1400#",DEC RMTibiaPin,"P1400#", |
DEC RFCoxaPin,"P1800#",DEC RFFemurPin,"P2100#",DEC LRFemurPin,"P1200#",DEC LRTibiaPin,"P1200#", |
DEC LMFemurPin,"P1600#",DEC LMTibiaPin,"P1600#",DEC LFCoxaPin,"P1300#",DEC LFFemurPin,"P900T288",13]
pause 500
for Index = 1 to 10
serout SSC_OUT,SSC_BAUTE,["#",DEC RFCoxaPin,"P1800#",DEC RFFemurPin,"P2000#",DEC RFTibiaPin,"P1600#", |
DEC LFCoxaPin,"P1300#",DEC LFFemurPin,"P900#",DEC LFTibiaPin,"P1600T144",13]
pause 150
serout SSC_OUT,SSC_BAUTE,["#",DEC RFCoxaPin,"P1800#",DEC RFFemurPin,"P2100#",DEC RFTibiaPin,"P1300#", |
DEC LFCoxaPin,"P1300#",DEC LFFemurPin,"P1000#",DEC LFTibiaPin,"P1300T144",13]
pause 150
next
serout SSC_OUT,SSC_BAUTE,["#",DEC RFCoxaPin,"P1500#",DEC RFFemurPin,"P2100#",DEC RFTibiaPin,"P1500#", |
DEC LFCoxaPin,"P1500#",DEC LFFemurPin,"P900#",DEC LFTibiaPin,"P1500T288",13]
pause 400
serout SSC_OUT,SSC_BAUTE,["#",DEC RRFemurPin,"P1500#",DEC RRTibiaPin,"P1500#",DEC RMFemurPin,"P1500#",DEC RMTibiaPin,"P1500#",DEC RFFemurPin,"P1500#", |
DEC LMFemurPin,"P1500#",DEC LMTibiaPin,"P1500#",DEC LFFemurPin,"P1500#",DEC LRFemurPin,"P1500#",DEC LRTibiaPin,"P1500T576",13]
pause 400
serout SSC_OUT,SSC_BAUTE,["#",DEC RMCoxaPin,"P1500#",DEC RMFemurPin,"P1700#",DEC RMTibiaPin,"P1700#", |
DEC LMCoxaPin,"P1500#",DEC LMFemurPin,"P1300#",DEC LMTibiaPin,"P1300T288",13]
pause 400
serout SSC_OUT,SSC_BAUTE,["#",DEC RMFemurPin,"P1500#",DEC RMTibiaPin,"P1500#",DEC LMFemurPin,"P1500#",DEC LMTibiaPin,"P1500T288",13]
pause 400
ENDIF
return
;--------------------------------------------------------------------
;[FREE SERVOS] Frees all the servos
FreeServos
for Index = 0 to 31
serout SSC_OUT,SSC_BAUTE,["#",DEC Index,"P0"]
next
serout SSC_OUT,SSC_BAUTE,["T200",13]
return
;-----------------------------------------------------------------------------------
;[Handle TimerW interrupt]
BEGINASMSUB
HANDLE_TIMERW
push.w r1 ; save away register we will use
bclr #7,@TSRW:8 ; clear the overflow bit in the Timer status word
mov.w @LTIMERWOVERFLOWCNT+1:16,r1 ; We will increment the word that is the highword for a clock timer
inc.w #1,r1
mov.w r1, @LTIMERWOVERFLOWCNT+1:16
pop.w r1 ; restore our registers
rte ; and return
return
;-------------------------------------------------------------------------------------
;[Simple function to get the current time and verify that no overflow happened]
GetCurrentTime
lCurrentTime = lTimerWoverflowCnt + TCNT ; calculate the timer
IF lCurrentTime.highword <> lTimerWOverflowcnt.highword THEN
lCurrentTime = lTimerWoverflowCnt + TCNT ; calculate the timer
ENDIF
return lCurrentTIme
;--------------------------------------------------------------------
