;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 1FALSE con 0

BUTTON_DOWN con 0BUTTON_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 HorizontalRFFemurPin con P1 ;Front Right leg Hip VerticalRFTibiaPin con P0 ;Front Right leg Knee

RMCoxaPin con P6 ;Middle Right leg Hip Horizontal

RMFemurPin con P5 ;Middle Right leg Hip VerticalRMTibiaPin con P4 ;Middle Right leg Knee

RRCoxaPin con P10 ;Rear Right leg Hip HorizontalRRFemurPin con P9 ;Rear Right leg Hip VerticalRRTibiaPin con P8 ;Rear Right leg Knee

LFCoxaPin con P18 ;Front Left leg Hip HorizontalLFFemurPin con P17 ;Front Left leg Hip VerticalLFTibiaPin con P16 ;Front Left leg Knee

LMCoxaPin con P22 ;Middle Left leg Hip HorizontalLMFemurPin con P21 ;Middle Left leg Hip VerticalLMTibiaPin con P20 ;Middle Left leg Knee

LRCoxaPin con P26 ;Rear Left leg Hip HorizontalLRFemurPin con P25 ;Rear Left leg Hip VerticalLRTibiaPin con P24 ;Rear Left leg Knee;--------------------------------------------------------------------;[SERVO Offsets]RFCoxaOffset con -23 ;Front Right leg Hip HorizontalRFFemurOffset con -25 ;Front Right leg Hip VerticalRFTibiaOffset con -34 ;Front Right leg Knee

RMCoxaOffset con 19 ;Middle Right leg Hip HorizontalRMFemurOffset con 16 ;Middle Right leg Hip VerticalRMTibiaOffset con -44 ;Middle Right leg Knee

RRCoxaOffset con -75 ;Rear Right leg Hip HorizontalRRFemurOffset con 12 ;Rear Right leg Hip VerticalRRTibiaOffset con 66 ;Rear Right leg Knee

LFCoxaOffset con 42 ;Front Left leg Hip HorizontalLFFemurOffset con -3 ;Front Left leg Hip VerticalLFTibiaOffset con -66 ;Front Left leg Knee

LMCoxaOffset con -33 ;Middle Left leg Hip HorizontalLMFemurOffset con 21 ;Middle Left leg Hip VerticalLMTibiaOffset con 36 ;Middle Left leg Knee

LRCoxaOffset con 0 ;Rear Left leg Hip HorizontalLRFemurOffset con 28 ;Rear Left leg Hip VerticalLRTibiaOffset con -34 ;Rear Left leg Knee;--------------------------------------------------------------------;[MIN/MAX ANGLES]RFCoxa_MIN con -90 ;Mechanical limits of the Right Front LegRFCoxa_MAX con 80RFFemur_MIN con -92RFFemur_MAX con 92RFTibia_MIN con -104RFTibia_MAX con 74

RMCoxa_MIN con -58 ;Mechanical limits of the Right Middle LegRMCoxa_MAX con 96RMFemur_MIN con -92RMFemur_MAX con 95RMTibia_MIN con -105

RMTibia_MAX con 72

RRCoxa_MIN con -100 ;Mechanical limits of the Right Rear LegRRCoxa_MAX con 52RRFemur_MIN con -96RRFemur_MAX con 95RRTibia_MIN con -96RRTibia_MAX con 82

LFCoxa_MIN con -80 ;Mechanical limits of the Left Front LegLFCoxa_MAX con 95LFFemur_MIN con -97LFFemur_MAX con 96LFTibia_MIN con -81LFTibia_MAX con 104

LMCoxa_MIN con -95 ;Mechanical limits of the Left Middle LegLMCoxa_MAX con 58LMFemur_MIN con -94LMFemur_MAX con 96LMTibia_MIN con -81LMTibia_MAX con 102

LRCoxa_MIN con -100 ;Mechanical limits of the Left Rear LegLRCoxa_MAX con 50LRFemur_MIN con -96LRFemur_MAX con 94LRTibia_MIN con -82LRTibia_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 ;Distance X from center of the body to the Right Front coxaRFOffsetZ con -88 ;Distance Z from center of the body to the Right Front coxaRMOffsetX con -65 ;Distance X from center of the body to the Right Middle coxaRMOffsetZ con 0 ;Distance Z from center of the body to the Right Middle coxaRROffsetX con -42 ;Distance X from center of the body to the Right Rear coxaRROffsetZ con 88 ;Distance Z from center of the body to the Right Rear coxa

LFOffsetX con 42 ;Distance X from center of the body to the Left Front coxaLFOffsetZ con -88 ;Distance Z from center of the body to the Left Front coxaLMOffsetX con 65 ;Distance X from center of the body to the Left Middle coxaLMOffsetZ con 0 ;Distance Z from center of the body to the Left Middle coxa

LROffsetX con 42 ;Distance X from center of the body to the Left Rear coxaLROffsetZ con 88 ;Distance Z from center of the body to the Left Rear coxa;--------------------------------------------------------------------;[ANGLES]RFCoxaAngle var sword ;Actual Angle of the Right Front LegRFFemurAngle var swordRFTibiaAngle var sword

RMCoxaAngle var sword ;Actual Angle of the Right Middle LegRMFemurAngle var swordRMTibiaAngle var sword

RRCoxaAngle var sword ;Actual Angle of the Right Rear LegRRFemurAngle var swordRRTibiaAngle var sword

LFCoxaAngle var sword ;Actual Angle of the Left Front LegLFFemurAngle var swordLFTibiaAngle var sword

LMCoxaAngle var sword ;Actual Angle of the Left Middle LegLMFemurAngle var swordLMTibiaAngle var sword

LRCoxaAngle var sword ;Actual Angle of the Left Rear LegLRFemurAngle var swordLRTibiaAngle var sword;--------------------------------------------------------------------;[POSITIONS]RFPosX var sword ;Actual Position of the Right Front LegRFPosY var swordRFPosZ var sword

RMPosX var sword ;Actual Position of the Right Middle LegRMPosY var swordRMPosZ var sword

RRPosX var sword ;Actual Position of the Right Rear LegRRPosY var swordRRPosZ var sword

LFPosX var sword ;Actual Position of the Left Front LegLFPosY var swordLFPosZ var sword

LMPosX var sword ;Actual Position of the Left Middle LegLMPosY var swordLMPosZ var sword

LRPosX var sword ;Actual Position of the Left Rear LegLRPosY var swordLRPosZ var sword;--------------------------------------------------------------------;[INPUTS]butA var bit

butB var bitbutC var bit

prev_butA var bitprev_butB var bitprev_butC var bit;--------------------------------------------------------------------;[OUTPUTS]LedA var bit ;RedLedB var bit ;GreenLedC var bit ;Orange;--------------------------------------------------------------------;[VARIABLES]Index var byte ;Index used for freeing the servosSSCDone var byte ;Char to check if SSC is done

;GetSinCosAngleDeg var float ;Input Angle in degreesABSAngleDeg var float ;Absolute value of the Angle in DegreesAngleRad var float ;Angle in RadianSinus var float ;Output Sinus of the given AngleCosinus var float ;Output Cosinus of the given Angle

;GetBoogTanBoogTanX var sword ;Input XBoogTanY var sword ;Input YBoogTan var float ;Output BOOGTAN2(X/Y)

;Body positionBodyPosX var sbyte ;Global Input for the position of the bodyBodyPosY var sbyteBodyPosZ var sbyte

;Body Inverse KinematicsBodyRotX var sbyte ;Global Input pitch of the bodyBodyRotY var sbyte ;Global Input rotation of the bodyBodyRotZ var sbyte ;Global Input roll of the bodyPosX var sword ;Input position of the feet XPosZ var sword ;Input position of the feet ZRotationY var sbyte ;Input for rotation of a single feet for the gaitBodyOffsetX var sbyte ;Input Offset betweeen the body and Coxa XBodyOffsetZ var sbyte ;Input Offset betweeen the body and Coxa ZPitchY var sword ;PitchY offset, needs to be added to the BodyPosYRollY var sword ;RollY offset, needs to be added to the BodyPosYTotalX 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 feetDistCenterBodyFeet var float ;Total distance between the center of the body and the feetAngleCenterBodyFeetX var float ;Angle between the center of the body and the feetBodyIKPosX var sword ;Output Position X of feet with RotationBodyIKPosY var sword ;Output Position Y of feet with RotationBodyIKPosZ var sword ;Output Position Z of feet with Rotation

;Leg Inverse KinematicsIKFeetPosX var sword ;Input position of the Feet XIKFeetPosY var sword ;Input position of the Feet YIKFeetPosZ var sword ;Input Position of the Feet ZIKFeetPosXZ var sword ;Length between the coxa and feetIKSW var float ;Length between shoulder and wristIKA1 var float ;Angle between SW line and the ground in radIKA2 var float ;?IKSolution var bit ;Output true if the solution is possibleIKSolutionWarning var bit ;Output true if the solution is NEARLY possibleIKSolutionError var bit ;Output true if the solution is NOT possibleIKFemurAngle var sword ;Output Angle of Femur in degreesIKTibiaAngle var sword ;Output Angle of Tibia in degreesIKCoxaAngle var sword ;Output Angle of Coxa in degrees;--------------------------------------------------------------------;[Ps2 Controller]DualShock var Byte(7)LastButton var Byte(2)DS2Mode var BytePS2Index var byte;--------------------------------------------------------------------;[TEMP]

;gaitLegLiftHeight var byte ;Current Travel heightTravelLengthX var sbyte ;Current Travel length XTravelLengthZ var sbyte ;Current Travel length ZTravelRotationY var sbyte ;Current Travel Rotation Y

GaitStep var nib ;Global Input Gait stepGaitLegNr var nib ;Input Number of the legTravelMulti var sbyte ;Multiplier for the length of the step

RFGaitPosX var sbyte ;Relative position corresponding to the GaitRFGaitPosY var sbyteRFGaitPosZ var sbyteRFGaitRotY var sbyte ;Relative rotation corresponding to the Gait

RMGaitPosX var sbyteRMGaitPosY var sbyteRMGaitPosZ var sbyteRMGaitRotY var sbyte

RRGaitPosX var sbyteRRGaitPosY var sbyteRRGaitPosZ var sbyteRRGaitRotY var sbyte

LFGaitPosX var sbyteLFGaitPosY var sbyteLFGaitPosZ var sbyteLFGaitRotY var sbyte

LMGaitPosX var sbyteLMGaitPosY var sbyteLMGaitPosZ var sbyteLMGaitRotY var sbyte

LRGaitPosX var sbyteLRGaitPosY var sbyteLRGaitPosZ var sbyteLRGaitRotY var sbyte

GaitPosX var sbyte ;In-/Output Pos X of feetGaitPosY var sbyte ;In-/Output Pos Y of feetGaitPosZ var sbyte ;In-/Output Pos Z of feetGaitRotY var sbyte ;In-/Output Rotation 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 ledsLedA = 0LedB = 0LedC = 0

'Feet PositionsRFPosX = 80 ;Start positions of the Right Front legRFPosY = 15RFPosZ = -75

RMPosX = 110 ;Start positions of the Right Middle legRMPosY = 15RMPosZ = 0

RRPosX = 80 ;Start positions of the Right Rear legRRPosY = 15RRPosZ = 75

LFPosX = 80 ;Start positions of the Left Front legLFPosY = 15LFPosZ = -75

LMPosX = 110 ;Start positions of the Left Middle legLMPosY = 15LMPosZ = 0

LRPosX = 80 ;Start positions of the Left Rear legLRPosY = 15LRPosZ = 75

;Body PositionsBodyPosX = 0BodyPosY = 0BodyPosZ = 0

;Body RotationsBodyRotX = 0BodyRotY = 0BodyRotZ = 0

;GaitLegLiftHeight = 50GaitStep = 1

;PS2 controllerhigh PS2CLKLastButton(0) = 255LastButton(1) = 255 ;--------------------------------------------------------------------;[MAIN]main:

;Calculate Gait positionsgosub Gait [1, LRGaitPosX, LRGaitPosY, LRGaitPosZ, LRGaitRotY] LRGaitPosX = GaitPosXLRGaitPosY = GaitPosYLRGaitPosZ = GaitPosZLRGaitRotY = GaitRotY

gosub Gait [2, RFGaitPosX, RFGaitPosY, RFGaitPosZ, RFGaitRotY]RFGaitPosX = GaitPosXRFGaitPosY = GaitPosYRFGaitPosZ = GaitPosZRFGaitRotY = GaitRotY

gosub Gait [3, LMGaitPosX, LMGaitPosY, LMGaitPosZ, LMGaitRotY]LMGaitPosX = GaitPosXLMGaitPosY = GaitPosYLMGaitPosZ = GaitPosZLMGaitRotY = GaitRotY

gosub Gait [4, RRGaitPosX, RRGaitPosY, RRGaitPosZ, RRGaitRotY]RRGaitPosX = GaitPosXRRGaitPosY = GaitPosYRRGaitPosZ = GaitPosZRRGaitRotY = GaitRotY

gosub Gait [5, LFGaitPosX, LFGaitPosY, LFGaitPosZ, LFGaitRotY]LFGaitPosX = GaitPosXLFGaitPosY = GaitPosYLFGaitPosZ = GaitPosZLFGaitRotY = GaitRotY

gosub Gait [6, RMGaitPosX, RMGaitPosY, RMGaitPosZ, RMGaitRotY]RMGaitPosX = GaitPosXRMGaitPosY = GaitPosYRMGaitPosZ = GaitPosZRMGaitRotY = GaitRotY

;Right Front leggosub 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 legRFFemurAngle = IKFemurAngleRFTibiaAngle = IKTibiaAngle

;Right Middle leggosub 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 = IKCoxaAngleRMFemurAngle = IKFemurAngleRMTibiaAngle = IKTibiaAngle

;Right Rear leggosub 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 legRRFemurAngle = IKFemurAngleRRTibiaAngle = 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 = IKFemurAngleLFTibiaAngle = IKTibiaAngle

;Left Middle leggosub 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 = IKCoxaAngleLMFemurAngle = IKFemurAngleLMTibiaAngle = IKTibiaAngle

;Left Rear leggosub 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 legLRFemurAngle = IKFemurAngleLRTibiaAngle = IKTibiaAngle

gosub CheckAnglesgosub ServoDriver

LedC = IKSolutionWarningLedA = IKSolutionError

gosub ReadButtonsgosub WriteLedsgosub Ps2Input

goto main;--------------------------------------------------------------------;[ReadButtons] Reading input buttons from the ABBReadButtons:

input P4input P5input P6

prev_butA = butAprev_butB = butBprev_butC = butC

butA = IN4butB = IN5butC = IN6

return;--------------------------------------------------------------------;[WriteLEDs] Updates the state of the leds

WriteLEDs:if ledA = 1 then

low p4endifif ledB = 1 then

low p5endifif ledC = 1 then

low p6endif

return;--------------------------------------------------------------------;[PS2Input] reads the input data from the Wiiremote and processes the;data to the parameters.Ps2Input:

low PS2SELshiftout PS2CMD,PS2CLK,FASTLSBPRE,[$1\8]shiftin PS2DAT,PS2CLK,FASTLSBPOST,[DS2Mode\8]high PS2SELpause 1

low PS2SELshiftout 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 PS2SELpause 1

if DS2Mode <> PadMode thenlow PS2SELshiftout PS2CMD,PS2CLK,FASTLSBPRE,

[$1\8,$43\8,$0\8,$1\8,$0\8] ;CONFIG_MODE_ENTERhigh PS2SELpause 1

low PS2SELshiftout PS2CMD,PS2CLK,FASTLSBPRE,

[$01\8,$44\8,$00\8,$01\8,$03\8,$00\8,$00\8,$00\8,$00\8] ;SET_MODE_AND_LOCKhigh PS2SELpause 1

low PS2SELshiftout PS2CMD,PS2CLK,FASTLSBPRE,[$01\8,$4F\8,$00\8,$FF\

8,$FF\8,$03\8,$00\8,$00\8,$00\8] ;SET_DS2_NATIVE_MODEhigh PS2SELpause 1

low PS2SELshiftout 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 PS2SELpause 1

low PS2SELshiftout PS2CMD,PS2CLK,FASTLSBPRE,

[$01\8,$43\8,$00\8,$00\8,$00\8,$00\8,$00\8,$00\8,$00\8] ;CONFIG_MODE_EXIThigh PS2SELnap 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 ;Triangle Button test

BodyPosY = 30endif

if (DualShock(2).bit5 = 0) and LastButton(1).bit5 then ;Circle Button test

BodyPosY = 0endif

if (DualShock(1).bit4 = 0) and LastButton(0).bit4 then ;Up Button test

BodyPosY = BodyPosY+10endif

if (DualShock(1).bit6 = 0) and LastButton(0).bit6 then ;Down Button test

BodyPosY = BodyPosY-10endif

if (DualShock(2).bit2 = 0) then ;L1 Button testBodyPosX = (Dualshock(5) - 128)/2BodyPosZ = -(Dualshock(6) - 128)/3

elseif (DualShock(2).bit0 = 0) ;L2 Button testBodyRotX = (Dualshock(6) - 128)/10BodyRotY = (Dualshock(3) - 128)/8BodyRotZ = (Dualshock(5) - 128)/10

else ;WalkTravelLengthX = -(Dualshock(5) - 128)/2TravelLengthZ = (Dualshock(6) - 128)/2TravelRotationY = -(Dualshock(3) - 128)/5

endif

LastButton(0) = DualShock(1)LastButton(1) = DualShock(2)

return;--------------------------------------------------------------------;[GAIT]

Gait [GaitLegNr, GaitPosX, GaitPosY, GaitPosZ, GaitRotY]

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 = GaitPosX - (TravelLengthX/4) GaitPosY = 0 GaitPosZ = GaitPosZ - (TravelLengthZ/4) GaitRotY = GaitRotY - (TravelRotationY/4) ENDIF ENDIF ;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 AngleDegGetSinCos [AngleDeg]

;Get the absolute value of AngleDegIF AngleDeg < 0.0 THEN ABSAngleDeg = AngleDeg *-1.0ELSE ABSAngleDeg = AngleDegENDIF

;Shift rotation to a full circle of 360 deg -> AngleDeg // 360IF AngleDeg < 0.0 THEN ;Negative values

AngleDeg = 360.0-(ABSAngleDeg-TOFLOAT(360*(TOINT(ABSAngleDeg/360.0))))

ELSE ;Positive valuesAngleDeg = ABSAngleDeg-TOFLOAT(360*(TOINT(ABSAngleDeg/360.0)))

ENDIF

IF AngleDeg < 180.0 THEN ;Angle between 0 and 180;Subtract 90 to shift rangeAngleDeg = AngleDeg -90.0;Convert degree to radialsAngleRad = (AngleDeg*3.141592)/180.0

Sinus = FCOS(AngleRad) ;Sin o to 180 deg = cos(Angle Rad - 90deg)

Cosinus = -FSIN(AngleRad) ;Cos 0 to 180 deg = -sin(Angle Rad - 90deg)

ELSE ;Angle between 180 and 360;Subtract 270 to shift rangeAngleDeg = AngleDeg -270.0;Convert degree to radialsAngleRad = (AngleDeg*3.141592)/180.0

Sinus = -FCOS(AngleRad) ;Sin 180 to 360 deg = -cos(Angle Rad - 270deg)

Cosinus = FSIN(AngleRad) ;Cos 180 to 360 deg = sin(Angle Rad - 270deg)

ENDIFreturn;--------------------------------------------------------------------;[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 PIIF(BoogTanY >= 0) THEN

BoogTan = 0.0ELSE

BoogTan = 3.141592ENDIF

ELSE

IF(BoogTanY = 0) THEN ; Y=0 -> +/- Pi/2IF(BoogTanX > 0) THEN

BoogTan = 3.141592 / 2.0ELSE

BoogTan = -3.141592 / 2.0ENDIF

ELSE

IF(BoogTanY > 0) THEN ;BOOGTAN(X/Y)BoogTan = FATAN(TOFLOAT(BoogTanX) /

TOFLOAT(BoogTanY))ELSE

IF(BoogTanX > 0) THEN ;BOOGTAN(X/Y) + PIBoogTan = FATAN(TOFLOAT(BoogTanX) /

TOFLOAT(BoogTanY)) + 3.141592ELSE

;BOOGTAN(X/Y) - PIBoogTan = FATAN(TOFLOAT(BoogTanX) /

TOFLOAT(BoogTanY)) - 3.141592ENDIF

ENDIFENDIF

ENDIFreturn;--------------------------------------------------------------------;[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 RotationBodyIK [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 + PitchYreturn

;--------------------------------------------------------------------;[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 degreesLegIK [IKFeetPosX, IKFeetPosY, IKFeetPosZ]

;Reset all Solution optionsIKSolution = FALSEIKSolutionWarning = FALSEIKSolutionError = FALSE

;Length between the Coxa and FeetIKFeetPosXZ = TOINT(FSQRT(TOFLOAT((IKFeetPosX*IKFeetPosX)+

(IKFeetPosZ*IKFeetPosZ))))

;IKSW - Length between shoulder and wristIKSW = FSQRT(TOFLOAT(((IKFeetPosXZ-CoxaLength)*(IKFeetPosXZ-

CoxaLength))+(IKFeetPosY*IKFeetPosY)))

;IKA1 - Angle between SW line and the ground in radgosub GetBoogTan [IKFeetPosXZ-CoxaLength, IKFeetPosY]IKA1 = BoogTan

;IKA2 - ?IKA2 = FACOS((TOFLOAT((FemurLength*FemurLength) -

(TibiaLength*TibiaLength)) + (IKSW*IKSW)) / (TOFLOAT(2*Femurlength) * IKSW))

;IKFemurAngleIKFemurAngle = (TOINT(((IKA1 + IKA2) * 180.0) / 3.141592)*-1)+90

;IKTibiaAngleIKTibiaAngle = (90-TOINT(((FACOS((TOFLOAT((FemurLength*FemurLength) +

(TibiaLength*TibiaLength)) - (IKSW*IKSW)) / TOFLOAT(2*Femurlength*TibiaLength)))*180.0) / 3.141592)) * -1

;IKCoxaAnglegosub GetBoogTan [IKFeetPosZ, IKFeetPosX]IKCoxaAngle = TOINT((BoogTan*180.0) / 3.141592)

;Set the Solution qualityIF(IKSW < TOFLOAT(FemurLength+TibiaLength-30)) THEN

IKSolution = TRUEELSE

IF(IKSW < TOFLOAT(FemurLength+TibiaLength)) THENIKSolutionWarning = TRUE

ELSEIKSolutionError = TRUE

ENDIF

ENDIF

return;--------------------------------------------------------------------;[CHECK ANGLES] Checks the mechanical limits of the servosCheckAngles: 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_MAXreturn;--------------------------------------------------------------------;[SERVO DRIVER] Updates the positions of the servosServoDriver:

;Wait for previous commands to be completedSSCWait: 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 servosFreeServos

for Index = 0 to 31serout SSC_OUT,SSC_BAUTE,["#",DEC Index,"P0"]

nextserout SSC_OUT,SSC_BAUTE,[13]

return;--------------------------------------------------------------------