;Project BlackWido ;Description: "Phoenix" style bot controlled by a wii remote ;Software version: V1.

1 ;Date: 25-06-2008 ;Programmer: Jeroen Janssen (aka Xan) ; ;Hardware setup: ABB2 with ATOM 28 Pro, SSC32, BlueSmirf (See further for connections) ; ;NEW IN V1.1 ; - 6 steps Ripple Gait ; - Lynxmotion PS2 controller support ; ;KNOWN BUGS: ; - Synchronisation PS2 controller (for more information see: http://www.lynxmotion.net/viewtopic.php?t=3979&postdays=0&postorder=asc&start =0) ; - Maximum height of the body. If BodyY > 128 it will overflow and gets a negative value which causes the hex to drop down with the legs in the air. ; ;TODO: ; - Include Deadzone on the controls ; - Replace sin, cos, atan with a byte table to improve calculating speed ; - Replace All Floats with sword * 1000 to improve calculating speed and free some memory ; - Implement 12 steps Gait ; - Run some synchronisation tests ; - Include balance calculations ; ;-------------------------------------------------------------------;[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 on BotBoard (Yellow) SSC_IN con P10 ;Input pin for SSC32 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] 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 RMTibiaPin RRCoxaPin RRFemurPin RRTibiaPin LFCoxaPin LFFemurPin LFTibiaPin LMCoxaPin LMFemurPin LMTibiaPin

con P5 ;Middle Right leg Hip Vertical con P4 ;Middle Right leg Knee con P10 ;Rear Right leg Hip Horizontal con P9 ;Rear Right leg Hip Vertical con P8 ;Rear Right leg Knee con P18 ;Front Left leg Hip Horizontal con P17 ;Front Left leg Hip Vertical con P16 ;Front Left leg Knee con P22 ;Middle Left leg Hip Horizontal con P21 ;Middle Left leg Hip Vertical 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 ;-------------------------------------------------------------------;[SERVO Offsets] RFCoxaOffset con -23 ;Front Right leg Hip Horizontal RFFemurOffset con -25 ;Front Right leg Hip Vertical RFTibiaOffset con -34 ;Front Right leg Knee RMCoxaOffset RMFemurOffset RMTibiaOffset RRCoxaOffset RRFemurOffset RRTibiaOffset LFCoxaOffset LFFemurOffset LFTibiaOffset LMCoxaOffset LMFemurOffset LMTibiaOffset con 19 ;Middle Right leg Hip Horizontal con 16 ;Middle Right leg Hip Vertical con -44 ;Middle Right leg Knee con -75 ;Rear Right leg Hip Horizontal con 12 ;Rear Right leg Hip Vertical con 66 ;Rear Right leg Knee con 42 ;Front Left leg Hip Horizontal con -3 ;Front Left leg Hip Vertical con -66 ;Front Left leg Knee con -33 ;Middle Left leg Hip Horizontal con 21 ;Middle Left leg Hip Vertical con 36 ;Middle Left leg Knee

LRCoxaOffset con 0 ;Rear Left leg Hip Horizontal LRFemurOffset con 28 ;Rear Left leg Hip Vertical LRTibiaOffset con -34 ;Rear Left leg Knee ;-------------------------------------------------------------------;[MIN/MAX ANGLES] RFCoxa_MIN con -90 ;Mechanical limits of the Right Front Leg RFCoxa_MAX con 80 RFFemur_MIN con -92 RFFemur_MAX con 92 RFTibia_MIN con -104 RFTibia_MAX con 74 RMCoxa_MIN RMCoxa_MAX RMFemur_MIN RMFemur_MAX RMTibia_MIN con con con con con -58 96 -92 95 -105 ;Mechanical limits of the Right Middle Leg

RMTibia_MAX RRCoxa_MIN RRCoxa_MAX RRFemur_MIN RRFemur_MAX RRTibia_MIN RRTibia_MAX LFCoxa_MIN LFCoxa_MAX LFFemur_MIN LFFemur_MAX LFTibia_MIN LFTibia_MAX LMCoxa_MIN LMCoxa_MAX LMFemur_MIN LMFemur_MAX LMTibia_MIN LMTibia_MAX

con 72 con con con con con con con con con con con con con con con con con con -100 52 -96 95 -96 82 -80 95 -97 96 -81 104 -95 58 -94 96 -81 102 ;Mechanical limits of the Right Rear Leg

;Mechanical limits of the Left Front Leg

;Mechanical limits of the Left Middle Leg

LRCoxa_MIN con -100 ;Mechanical limits of the Left Rear Leg LRCoxa_MAX con 50 LRFemur_MIN con -96 LRFemur_MAX con 94 LRTibia_MIN con -82 LRTibia_MAX con 100 ;-------------------------------------------------------------------;[BODY DIMENSIONS] CoxaLength con 25 ;Length of the Coxa [mm] FemurLength con 77 ;Length of the Femur [mm] TibiaLength con 105 ;Lenght of the Tibia [mm] RFOffsetX con -42 Right Front coxa RFOffsetZ con -88 Right Front coxa RMOffsetX con -65 Right Middle coxa RMOffsetZ con 0 Right Middle coxa RROffsetX con -42 Right Rear coxa RROffsetZ con 88 Right Rear coxa LFOffsetX con Front coxa LFOffsetZ con Front coxa LMOffsetX con Middle coxa LMOffsetZ con Middle coxa 42 -88 65 0 ;Distance X from center of the body to the ;Distance Z from center of the body to the ;Distance X from center of the body to the ;Distance Z from center of the body to the ;Distance X from center of the body to the ;Distance Z from center of the body to the ;Distance X from center of the body to the Left ;Distance Z from center of the body to the Left ;Distance X from center of the body to the Left ;Distance Z from center of the body to the Left

LROffsetX con 42 ;Distance X from center of the body to the Left Rear coxa LROffsetZ con 88 ;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 RMFemurAngle RMTibiaAngle RRCoxaAngle RRFemurAngle RRTibiaAngle LFCoxaAngle LFFemurAngle LFTibiaAngle LMCoxaAngle LMFemurAngle LMTibiaAngle var sword var sword var sword var sword var sword var sword var sword var sword var sword var sword var sword var sword ;Actual Angle of the Right Middle Leg

;Actual Angle of the Right Rear Leg

;Actual Angle of the Left Front Leg

;Actual Angle of the Left Middle Leg

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 RMPosY var sword RMPosZ var sword RRPosX var sword RRPosY var sword RRPosZ var sword LFPosX var sword LFPosY var sword LFPosZ var sword LMPosX var sword LMPosY var sword LMPosZ var sword ;Actual Position of the Right Middle Leg

;Actual Position of the Right Rear Leg

;Actual Position of the Left Front Leg

;Actual Position of the Left Middle Leg

LRPosX var sword ;Actual Position of the Left Rear Leg LRPosY var sword LRPosZ var sword ;-------------------------------------------------------------------;[INPUTS] butA var bit

butB butC

var bit 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 ABSAngleDeg AngleRad Sinus Cosinus Angle ;GetBoogTan BoogTanX BoogTanY BoogTan ;Body position BodyPosX of the body BodyPosY BodyPosZ var var float var var var float float float float ;Input Angle in degrees ;Absolute value of the Angle in Degrees ;Angle in Radian ;Output Sinus of the given Angle ;Output Cosinus of the given

var sword var sword var float var sbyte var sbyte var sbyte

;Input X ;Input Y ;Output BOOGTAN2(X/Y) ;Global Input for the position

;Body Inverse Kinematics BodyRotX body BodyRotY body BodyRotZ body PosX PosZ RotationY single feet for the gait BodyOffsetX body and Coxa X BodyOffsetZ body and Coxa Z PitchY be added to the BodyPosY RollY added to the BodyPosY TotalX center of the body and the feet

var sbyte ;Global Input pitch of the var sbyte ;Global Input rotation of the var sbyte ;Global Input roll of the var sword ;Input position of the feet X var sword ;Input position of the feet Z var sbyte ;Input for rotation of a var sbyte ;Input Offset betweeen the var sbyte ;Input Offset betweeen the var sword ;PitchY offset, needs to

var sword ;RollY offset, needs to be var sword ;Total X distance between the

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 ;-------------------------------------------------------------------;[TEMP] ;gait LegLiftHeight var byte TravelLengthX var sbyte TravelLengthZ var sbyte TravelRotationY var sbyte GaitStep GaitLegNr TravelMulti RFGaitPosX Gait RFGaitPosY RFGaitPosZ RFGaitRotY Gait ;Current ;Current ;Current ;Current Travel Travel Travel Travel height length X length Z Rotation Y

var nib var nib var sbyte var sbyte var sbyte var sbyte var sbyte

;Global Input Gait step ;Input Number of the leg ;Multiplier for the length of the step ;Relative position corresponding to the

;Relative rotation corresponding to the

RMGaitPosX RMGaitPosY RMGaitPosZ RMGaitRotY RRGaitPosX RRGaitPosY RRGaitPosZ RRGaitRotY LFGaitPosX LFGaitPosY LFGaitPosZ LFGaitRotY LMGaitPosX LMGaitPosY LMGaitPosZ LMGaitRotY LRGaitPosX LRGaitPosY LRGaitPosZ LRGaitRotY GaitPosX GaitPosY GaitPosZ GaitRotY

var var var var var var var var var var var var var var var var var var var var var var var var

sbyte sbyte sbyte sbyte sbyte sbyte sbyte sbyte sbyte sbyte sbyte sbyte sbyte sbyte sbyte sbyte sbyte sbyte sbyte sbyte sbyte sbyte sbyte sbyte ;In-/Output ;In-/Output ;In-/Output ;In-/Output Pos X of Pos Y of Pos Z of Rotation feet feet feet Y of feet

;-------------------------------------------------------------------;[Init SSC-32 with pulse offsets] SEROUT SSC_OUT,SSC_BAUTE,["#", | dec RRCoxaPin,"po",sdec RRCoxaOffset,"#",dec RRFemurPin,"po",sdec RRFemurOffset,"#",dec RRTibiaPin,"po",sdec RRTibiaOffset,"#", | dec RMCoxaPin,"po",sdec RMCoxaOffset,"#",dec RMFemurPin,"po",sdec RMFemurOffset,"#",dec RMTibiaPin,"po",sdec RMTibiaOffset,"#", | dec RFCoxaPin,"po",sdec RFCoxaOffset,"#",dec RFFemurPin,"po",sdec RFFemurOffset,"#",dec RFTibiaPin,"po",sdec RFTibiaOffset,"#", | dec LRCoxaPin,"po",sdec LRCoxaOffset,"#",dec LRFemurPin,"po",sdec LRFemurOffset,"#",dec LRTibiaPin,"po",sdec LRTibiaOffset,"#", | dec LMCoxaPin,"po",sdec LMCoxaOffset,"#",dec LMFemurPin,"po",sdec LMFemurOffset,"#",dec LMTibiaPin,"po",sdec LMTibiaOffset,"#", | dec LFCoxaPin,"po",sdec LFCoxaOffset,"#",dec LFFemurPin,"po",sdec LFFemurOffset,"#",dec LFTibiaPin,"po",sdec LFTibiaOffset,13] ;-------------------------------------------------------------------;[INIT] ;Turning off all the leds LedA = 0 LedB = 0 LedC = 0 'Feet Positions RFPosX = 80 RFPosY = 15 RFPosZ = -75 ;Start positions of the Right Front leg

RMPosX = 110 RMPosY = 15 RMPosZ = 0 RRPosX = 80 RRPosY = 15 RRPosZ = 75 LFPosX = 80 LFPosY = 15 LFPosZ = -75 LMPosX = 110 LMPosY = 15 LMPosZ = 0 LRPosX = 80 LRPosY = 15 LRPosZ = 75 ;Body Positions BodyPosX = 0 BodyPosY = 0 BodyPosZ = 0 ;Body Rotations BodyRotX = 0 BodyRotY = 0 BodyRotZ = 0

;Start positions of the Right Middle leg

;Start positions of the Right Rear leg

;Start positions of the Left Front leg

;Start positions of the Left Middle leg

;Start positions of the Left Rear leg

;Gait LegLiftHeight = 50 GaitStep = 1 ;PS2 controller high PS2CLK LastButton(0) = 255 LastButton(1) = 255 ;-------------------------------------------------------------------;[MAIN] main: ;Calculate gosub Gait LRGaitPosX LRGaitPosY LRGaitPosZ LRGaitRotY gosub Gait RFGaitPosX RFGaitPosY RFGaitPosZ RFGaitRotY Gait positions [1, LRGaitPosX, LRGaitPosY, LRGaitPosZ, LRGaitRotY] = GaitPosX = GaitPosY = GaitPosZ = GaitRotY [2, RFGaitPosX, RFGaitPosY, RFGaitPosZ, RFGaitRotY] = GaitPosX = GaitPosY = GaitPosZ = GaitRotY

gosub Gait LMGaitPosX LMGaitPosY LMGaitPosZ LMGaitRotY gosub Gait RRGaitPosX RRGaitPosY RRGaitPosZ RRGaitRotY gosub Gait LFGaitPosX LFGaitPosY LFGaitPosZ LFGaitRotY gosub Gait RMGaitPosX RMGaitPosY RMGaitPosZ RMGaitRotY

[3, LMGaitPosX, LMGaitPosY, LMGaitPosZ, LMGaitRotY] = GaitPosX = GaitPosY = GaitPosZ = GaitRotY [4, RRGaitPosX, RRGaitPosY, RRGaitPosZ, RRGaitRotY] = GaitPosX = GaitPosY = GaitPosZ = GaitRotY [5, LFGaitPosX, LFGaitPosY, LFGaitPosZ, LFGaitRotY] = GaitPosX = GaitPosY = GaitPosZ = GaitRotY [6, RMGaitPosX, RMGaitPosY, RMGaitPosZ, RMGaitRotY] = GaitPosX = GaitPosY = GaitPosZ = GaitRotY

;Right Front leg gosub BodyIK [-RFPosX+BodyPosX+RFGaitPosX, RFPosZ+BodyPosZ+RFGaitPosZ, RFOffsetX, RFOffsetZ, RFGaitRotY] gosub LegIK [RFPosX-BodyPosX+BodyIKPosX-RFGaitPosX, RFPosY+BodyPosY+BodyIKPosY+RFGaitPosY, RFPosZ+BodyPosZ-BodyIKPosZ+RFGaitPosZ] RFCoxaAngle = IKCoxaAngle + 60 ;60 degree for the basic setup for the front leg RFFemurAngle = IKFemurAngle RFTibiaAngle = IKTibiaAngle ;Right Middle leg gosub BodyIK [-RMPosX+BodyPosX+RMGaitPosX, RMPosZ+BodyPosZ+RMGaitPosZ, 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, RROffsetX, RROffsetZ, RRGaitRotY] gosub LegIK [RRPosX-BodyPosX+BodyIKPosX-RRGaitPosX, RRPosY+BodyPosY+BodyIKPosY+RRGaitPosY, RRPosZ+BodyPosZ-BodyIKPosZ+RRGaitPosZ] RRCoxaAngle = IKCoxaAngle - 60 ;60 degree for the basic setup for the front leg RRFemurAngle = IKFemurAngle RRTibiaAngle = IKTibiaAngle ;Left Front leg gosub BodyIK [LFPosX-BodyPosX+LFGaitPosX, LFPosZ+BodyPosZ+LFGaitPosZ, LFOffsetX, LFOffsetZ, LFGaitRotY]

gosub LegIK [LFPosX+BodyPosX-BodyIKPosX+LFGaitPosX, LFPosY+BodyPosY+BodyIKPosY+LFGaitPosY, LFPosZ+BodyPosZ-BodyIKPosZ+LFGaitPosZ] LFCoxaAngle = IKCoxaAngle + 60 ;60 degree for the basic setup for the front leg LFFemurAngle = IKFemurAngle LFTibiaAngle = IKTibiaAngle ;Left Middle leg gosub BodyIK [LMPosX-BodyPosX+LMGaitPosX, LMPosZ+BodyPosZ+LMGaitPosZ, 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, LROffsetX, LROffsetZ, LRGaitRotY] gosub LegIK [LRPosX+BodyPosX-BodyIKPosX+LRGaitPosX, LRPosY+BodyPosY+BodyIKPosY+LRGaitPosY, LRPosZ+BodyPosZ-BodyIKPosZ+LRGaitPosZ] LRCoxaAngle = IKCoxaAngle - 60 ;60 degree for the basic setup for the front leg LRFemurAngle = IKFemurAngle LRTibiaAngle = IKTibiaAngle gosub CheckAngles gosub ServoDriver LedC = IKSolutionWarning LedA = IKSolutionError gosub ReadButtons gosub WriteLeds gosub Ps2Input 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 low endif if ledC = 1 low endif

then p5 then p6

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 nap 4 sound P9,[100\4000, 100\4000, 100\4000] return endif serout s_out, i9600, [dec DS2Mode, " ", dec dualshock(0), " ", dec dualshock(1), " ", dec dualshock(2), " ", dec dualshock(3), | dec dualshock(4), " ", dec dualshock(5), " ", dec dualshock(6), 13] if (DualShock(2).bit4 = 0) and LastButton(1).bit4 then Button test BodyPosY = 30 endif if (DualShock(2).bit5 = 0) and LastButton(1).bit5 then Button test BodyPosY = 0 endif if (DualShock(1).bit4 = 0) and LastButton(0).bit4 then Button test BodyPosY = BodyPosY+10 endif if (DualShock(1).bit6 = 0) and LastButton(0).bit6 then Button test BodyPosY = BodyPosY-10 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 TravelLengthX = -(Dualshock(5) - 128)/2 TravelLengthZ = (Dualshock(6) - 128)/2 TravelRotationY = -(Dualshock(3) - 128)/5 endif LastButton(0) = DualShock(1) LastButton(1) = DualShock(2) return ;-------------------------------------------------------------------;[GAIT] Gait [GaitLegNr, GaitPosX, GaitPosY, GaitPosZ, GaitRotY] ;Triangle

;Circle

;Up

;Down

IF (GaitStep=GaitLegNr) & ((ABS(TravelLengthX)>2) | (ABS(TravelLengthZ)>2) | (ABS(GaitPosX)>2) | (ABS(GaitPosZ)>2) | (ABS(GaitRotY)>2)) THEN ;Up GaitPosX = 0 GaitPosY = -LegLiftHeight GaitPosZ = 0 GaitRotY = 0 ELSE IF (GaitStep=GaitLegNr+1 | GaitStep=GaitLegNr-5) & GaitPosY<0 THEN ;Down GaitPosX = TravelLengthX/2 GaitPosY = 0 GaitPosZ = TravelLengthZ/2 GaitRotY = TravelRotationY/2 ELSE GaitPosX GaitPosY GaitPosZ GaitRotY ENDIF ENDIF = = = = GaitPosX - (TravelLengthX/4) 0 GaitPosZ - (TravelLengthZ/4) GaitRotY - (TravelRotationY/4)

;serout S_OUT, i9600, ["GaitStep=", sdec GaitStep, " LegNr=", sdec GaitLegNr, " TravelLengthZ=", sdec TravelLengthZ, " LegTravelLengthZ=", sdec LegTravelLengthZ, " Multi=", sdec TravelMulti, " Y=", sdec GaitPosY, " Z=", sdec GaitPosZ,13] ;Advance to the next step IF GaitLegNr=6 THEN GaitStep = GaitStep+1 IF GaitStep>6 THEN GaitStep = 1 ENDIF ENDIF return ;-------------------------------------------------------------------;[GETSINCOS] Get the sinus and cosinus from the angle +/- multiple circles ;AngleDeg - Input Angle in degrees ;Sinus - Output Sinus of AngleDeg ;Cosinus - 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-(ABSAngleDegTOFLOAT(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 Sinus = FCOS(AngleRad) Rad - 90deg) Cosinus = -FSIN(AngleRad) 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 ;Cos 0 to 180 deg = -sin(Angle ;Sin o to 180 deg = cos(Angle

Sinus = -FCOS(AngleRad) ;Sin 180 to 360 deg = cos(Angle Rad - 270deg) Cosinus = 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 ;RollY - RollY offset, needs to be added to the BodyPosY ;PitchY - PitchY offset, needs to be added to the BodyPosY ;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, BodyOffsetX, BodyOffsetZ, RotationY] ;Calculating totals from center of the body to the feet TotalZ = BodyOffsetZ+PosZ TotalX = BodyOffsetX+PosX ;Distance between center body and feet DistCenterBodyFeet = FSQRT(TOFLOAT((TotalX*TotalX) + (TotalZ*TotalZ))) ;Angle X between center body and feet gosub GetBoogTan [TotalZ, TotalX] AngleCenterBodyFeetX = (BoogTan*180.0) / 3.141592 ;Calculate position corrections of feet X and Z for BodyRotation gosub GetSinCos [AngleCenterBodyFeetX+TOFLOAT(BodyRotY+RotationY)] BodyIKPosX = TotalX-TOINT(Cosinus*DistCenterBodyFeet) BodyIKPosZ = TotalZ-TOINT(Sinus*DistCenterBodyFeet) ;Calculate position corrections for Y for Body Roll and Pitch RollY = TOINT(FTAN((TOFLOAT(BodyRotZ) * 3.141592) / 180.0) * TOFLOAT(TotalX)) PitchY = TOINT(-FTAN((TOFLOAT(BodyRotX) * 3.141592) / 180.0) * TOFLOAT(TotalZ)) ;Calculate total position correction in Y for Body Roll and Pitch BodyIKPosY = RollY + PitchY 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] ;Reset all Solution options IKSolution = FALSE IKSolutionWarning = FALSE IKSolutionError = FALSE ;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)*(IKFeetPosXZCoxaLength))+(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: ;Wait for previous commands to be completed SSCWait: serout SSC_OUT,SSC_BAUTE,["Q",13] serin SSC_IN, SSC_BAUTE,[SSCDone] IF SSCDone <> "." THEN goto SSCWait ENDIF ;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 <CR> serout SSC_OUT,SSC_BAUTE,["T200",13] 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,[13] return ;--------------------------------------------------------------------