;Project Lynxmotion Phoenix ;Description: Phoenix, controlled by a PS2 remote ;Software version: V1.2 ;Date: 20-10-2008 ;Programmer: Jeroen Janssen (aka Xan) ; ;Hardware setup: ABB2 with ATOM 28 Pro, SSC32 V2, PS2 remote (See further for connections) ; ;NEW IN V1.2 ; - Improved gait engine ; - improved gait speed ; - 5 different gaits Thanks to Kåre Halvorsen (aka Zenta) ; - Improved body rotations Thanks to Kåre Halvorsen (aka Zenta) ; - Internal timer to improve SSC communication ; - Turn on/off the bot with the remote ; - Servo offsets moved to SSC V2 registers ; - Optional compiling for the SSC setup ; ;PS2 CONTROLS: ; - Start Turn on/off the bot ; - select Switch gaits ; - Left Stick Walk/Strafe ; - Right Stick Rotate ; - D-Pad up Body up 10 mm ; - D-Pad down Body down 10 mm ; - Triangle Move body to 35 mm from the ground (walk pos) ; - Circle Move body to the ground ; - L1 & L. Stick Shift body X/Z ; - L2 & Sticks Rotate body X/Y/Z ; - R2 & L. Stick Double gait travel length ; ;KNOWN BUGS: ; - None at the moment ;) ; ;==================================================================== ;[CONSTANDS] TRUE con 1 FALSE con 0 BUTTON_DOWN con 0 BUTTON_UP con 1 ;-------------------------------------------------------------------- ;[SERIAL CONNECTIONS] SSC_LM_SETUP con 1 ;Changes the SSC pins corresponding to the setup ;1 = Setup with connector to the front ;0 = Setup with connector to the back 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] #IF SSC_LM_SETUP ;Connector to the front RRCoxaPin con P0 ;Rear Right leg Hip Horizontal RRFemurPin con P1 ;Rear Right leg Hip Vertical RRTibiaPin con P2 ;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 P8 ;Front Right leg Hip Horizontal RFFemurPin con P9 ;Front Right leg Hip Vertical RFTibiaPin con P10 ;Front Right leg Knee LRCoxaPin con P16 ;Rear Left leg Hip Horizontal LRFemurPin con P17 ;Rear Left leg Hip Vertical LRTibiaPin con P18 ;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 P24 ;Front Left leg Hip Horizontal LFFemurPin con P25 ;Front Left leg Hip Vertical LFTibiaPin con P26 ;Front Left leg Knee #ELSE ;Connector to the back RFCoxaPin con P2 ;Front Right leg Hip Horizontal RFFemurPin con P1 ;Front Right leg Hip Vertical RFTibiaPin con P0 ;Front Right leg Knee RMCoxaPin con P6 ;Middle Right leg Hip Horizontal RMFemurPin con P5 ;Middle Right leg Hip Vertical RMTibiaPin con P4 ;Middle Right leg Knee RRCoxaPin con P10 ;Rear Right leg Hip Horizontal RRFemurPin con P9 ;Rear Right leg Hip Vertical RRTibiaPin con P8 ;Rear Right leg Knee LFCoxaPin con P18 ;Front Left leg Hip Horizontal LFFemurPin con P17 ;Front Left leg Hip Vertical LFTibiaPin con P16 ;Front Left leg Knee LMCoxaPin con P22 ;Middle Left leg Hip Horizontal LMFemurPin con P21 ;Middle Left leg Hip Vertical LMTibiaPin con P20 ;Middle Left leg Knee LRCoxaPin con P26 ;Rear Left leg Hip Horizontal LRFemurPin con P25 ;Rear Left leg Hip Vertical LRTibiaPin con P24 ;Rear Left leg Knee #ENDIF ;-------------------------------------------------------------------- ;[MIN/MAX ANGLES] RRCoxa_MIN con -26 ;Mechanical limits of the Right Rear Leg RRCoxa_MAX con 74 RRFemur_MIN con -101 RRFemur_MAX con 95 RRTibia_MIN con -106 RRTibia_MAX con 77 RMCoxa_MIN con -53 ;Mechanical limits of the Right Middle Leg RMCoxa_MAX con 53 RMFemur_MIN con -101 RMFemur_MAX con 95 RMTibia_MIN con -106 RMTibia_MAX con 77 RFCoxa_MIN con -58 ;Mechanical limits of the Right Front Leg RFCoxa_MAX con 74 RFFemur_MIN con -101 RFFemur_MAX con 95 RFTibia_MIN con -106 RFTibia_MAX con 77 LRCoxa_MIN con -74 ;Mechanical limits of the Left Rear Leg LRCoxa_MAX con 26 LRFemur_MIN con -95 LRFemur_MAX con 101 LRTibia_MIN con -77 LRTibia_MAX con 106 LMCoxa_MIN con -53 ;Mechanical limits of the Left Middle Leg LMCoxa_MAX con 53 LMFemur_MIN con -95 LMFemur_MAX con 101 LMTibia_MIN con -77 LMTibia_MAX con 106 LFCoxa_MIN con -74 ;Mechanical limits of the Left Front Leg LFCoxa_MAX con 58 LFFemur_MIN con -95 LFFemur_MAX con 101 LFTibia_MIN con -77 LFTibia_MAX con 106 ;-------------------------------------------------------------------- ;[BODY DIMENSIONS] CoxaLength con 29 ;Length of the Coxa [mm] FemurLength con 76 ;Length of the Femur [mm] TibiaLength con 106 ;Lenght of the Tibia [mm] CoxaAngle con 60 ;Default Coxa setup angle RFOffsetX con -43 ;Distance X from center of the body to the Right Front coxa RFOffsetZ con -82 ;Distance Z from center of the body to the Right Front coxa RMOffsetX con -63 ;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 -43 ;Distance X from center of the body to the Right Rear coxa RROffsetZ con 82 ;Distance Z from center of the body to the Right Rear coxa LFOffsetX con 43 ;Distance X from center of the body to the Left Front coxa LFOffsetZ con -82 ;Distance Z from center of the body to the Left Front coxa LMOffsetX con 63 ;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 43 ;Distance X from center of the body to the Left Rear coxa LROffsetZ con 82 ;Distance Z from center of the body to the Left Rear coxa ;==================================================================== ;[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 ;-------------------------------------------------------------------- ;[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 ;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 I 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 ;-------------------------------------------------------------------- ;[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 ;-------------------------------------------------------------------- ;[gait] GaitType var byte ;Gait type NomGaitSpeed 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 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 nib ;Number of steps in gait LastLeg var bit ;TRUE when the current leg is the last leg of the sequence GaitStep var nib ;Actual Gait step RFGaitLegNr var nib ;Init position of the leg RMGaitLegNr var nib ;Init position of the leg RRGaitLegNr var nib ;Init position of the leg LFGaitLegNr var nib ;Init position of the leg LMGaitLegNr var nib ;Init position of the leg LRGaitLegNr var nib ;Init position of the leg GaitLegNr var nib ;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 sbyte ;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 RFPosX = 53 ;Start positions of the Right Front leg RFPosY = 25 RFPosZ = -91 RMPosX = 105 ;Start positions of the Right Middle leg RMPosY = 25 RMPosZ = 0 RRPosX = 53 ;Start positions of the Right Rear leg RRPosY = 25 RRPosZ = 91 LFPosX = 53 ;Start positions of the Left Front leg LFPosY = 25 LFPosZ = -91 LMPosX = 105 ;Start positions of the Left Middle leg LMPosY = 25 LMPosZ = 0 LRPosX = 53 ;Start positions of the Left Rear leg LRPosY = 25 LRPosZ = 91 ;Body Positions BodyPosX = 0 BodyPosY = 0 BodyPosZ = 0 ;Body Rotations BodyRotX = 0 BodyRotY = 0 BodyRotZ = 0 ;Gait GaitType = 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 ;SSC SSCTime = 150 HexOn = False ;==================================================================== ;[MAIN] main: 'Start time GOSUB GetCurrentTime[], lTimerStart ;Gait GOSUB GaitSeq 'Reset IKsolution indicators IKSolution = False IKSolutionWarning = False IKSolutionError = False ;Right Front leg GOSUB BodyIK [-RFPosX+BodyPosX+RFGaitPosX, RFPosZ+BodyPosZ+RFGaitPosZ,RFPosY+BodyPosY+RFGaitPosY, RFOffsetX, RFOffsetZ, RFGaitRotY] GOSUB LegIK [RFPosX-BodyPosX+BodyIKPosX-RFGaitPosX, RFPosY+BodyPosY-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+RMGaitPosY, RMOffsetX, RMOffsetZ, RMGaitRotY] GOSUB LegIK [RMPosX-BodyPosX+BodyIKPosX-RMGaitPosX, RMPosY+BodyPosY-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+RRGaitPosY, RROffsetX, RROffsetZ, RRGaitRotY] GOSUB LegIK [RRPosX-BodyPosX+BodyIKPosX-RRGaitPosX, RRPosY+BodyPosY-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+LFGaitPosY, LFOffsetX, LFOffsetZ, LFGaitRotY] GOSUB LegIK [LFPosX+BodyPosX-BodyIKPosX+LFGaitPosX, LFPosY+BodyPosY-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+LMGaitPosY, LMOffsetX, LMOffsetZ, LMGaitRotY] GOSUB LegIK [LMPosX+BodyPosX-BodyIKPosX+LMGaitPosX, LMPosY+BodyPosY-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+LRGaitPosY, LROffsetX, LROffsetZ, LRGaitRotY] GOSUB LegIK [LRPosX+BodyPosX-BodyIKPosX+LRGaitPosX, LRPosY+BodyPosY-BodyIKPosY+LRGaitPosY, LRPosZ+BodyPosZ-BodyIKPosZ+LRGaitPosZ] LRCoxaAngle = IKCoxaAngle - CoxaAngle ;Angle for the basic setup for the front leg LRFemurAngle = IKFemurAngle LRTibiaAngle = IKTibiaAngle 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)>2 | ABS(TravelLengthZ)>2 | ABS(TravelRotationY*2)>2) THEN pause (PrevSSCTime - CycleTime -50) MIN 1 ; Min 1 ensures that there alway is a value in the pause command SSCTime = NomGaitSpeed + (InputTimeDelay*2) ELSE SSCTime = 200 ;NomGaitSpeed ENDIF GOSUB ServoDriver ELSE ;Turn the bot off GOSUB FreeServos ENDIF goto main ;==================================================================== ;[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,80\4500,60\4000] 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,80\4500,100\5000] 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<4 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 ENDIF IF (DualShock(1).bit6 = 0) and LastButton(0).bit6 THEN ;Down Button test BodyPosY = BodyPosY-10 ENDIF IF (DualShock(2).bit4 = 0) and LastButton(1).bit4 THEN ;Triangle Button test BodyPosY = 35 ENDIF IF (DualShock(2).bit5 = 0) and LastButton(1).bit5 THEN ;Circle Button test BodyPosY = 0 ENDIF ' IF (DualShock(2).bit6 = 0) and LastButton(1).bit6 THEN ;Cross Button test ' ENDIF ' IF (DualShock(2).bit7 = 0) and LastButton(1).bit7 THEN ;Square Button test ' ENDIF ' IF (DualShock(2).bit3 = 0) THEN ;R1 Button test ' ENDIF IF (DualShock(2).bit2 = 0) THEN ;L1 Button test BodyPosX = (Dualshock(5) - 128)/2 BodyPosZ = -(Dualshock(6) - 128)/3 ELSEIF (DualShock(2).bit0 = 0) ;L2 Button test BodyRotX = (Dualshock(6) - 128)/10 BodyRotY = (Dualshock(3) - 128)/8 BodyRotZ = (Dualshock(5) - 128)/10 ELSE ;Walk 'BodyPosX = 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 NomGaitSpeed = 150 ENDIF IF (GaitType = 1) THEN ;Ripple Gait 12 steps LRGaitLegNr = 1 RFGaitLegNr = 3 LMGaitLegNr = 5 RRGaitLegNr = 7 LFGaitLegNr = 9 RMGaitLegNr = 11 NrLiftedPos = 3 HalfLiftHeigth = TRUE TLDivFactor = 8 StepsInGait = 12 NomGaitSpeed = 100 ENDIF IF (GaitType = 2) THEN ;Quadripple 9 steps LRGaitLegNr = 1 RFGaitLegNr = 2 LMGaitLegNr = 4 RRGaitLegNr = 5 LFGaitLegNr = 7 RMGaitLegNr = 8 NrLiftedPos = 2 HalfLiftHeigth = FALSE TLDivFactor = 6 StepsInGait = 9 NomGaitSpeed = 150 ENDIF IF (GaitType = 3) THEN ;Tripod 4 steps LRGaitLegNr = 3 RFGaitLegNr = 1 LMGaitLegNr = 1 RRGaitLegNr = 1 LFGaitLegNr = 3 RMGaitLegNr = 3 NrLiftedPos = 1 TLDivFactor = 2 StepsInGait = 4 NomGaitSpeed = 150 ENDIF IF (GaitType = 4) THEN ;Tripod 6 steps LRGaitLegNr = 4 RFGaitLegNr = 1 LMGaitLegNr = 1 RRGaitLegNr = 1 LFGaitLegNr = 4 RMGaitLegNr = 4 NrLiftedPos = 2 HalfLiftHeigth = FALSE TLDivFactor = 4 StepsInGait = 6 NomGaitSpeed = 150 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)>2) | (ABS(TravelLengthZ)>2) | (ABS(TravelRotationY)>2) ) ;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 ;-------------------------------------------------------------------- ;[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)] SinG = SinA CosG = CosA GOSUB GetSinCos [TOFLOAT(BodyRotZ)] SinB = SinA CosB = CosA GOSUB GetSinCos [TOFLOAT(BodyRotY+RotationY)] ;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 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] ;Send serout SSC_OUT,SSC_BAUTE,["T",dec SSCTime,13] PrevSSCTime = SSCTime 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 ;--------------------------------------------------------------------