;Project: BlackWido ;Description: "Phoenix" style bot controlled by a wii remote ;Software version: V1.0 ;Date: 31-05-2008 ;Programmer: Jeroen Janssen (aka Xan) ; ;Hardware setup: ABB with ATOM 28 Pro, SSC32, BlueSmirf (See further for connections) ; ;CURRENT STATE V1.0 ; - Inverse Kinematics for legs implemented. ; - Inverse Kinematics for body implemented. ; - Connection to Wii remote functional ; ;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 Gait ;-------------------------------------------------------------------- ;[CONSTANDS] TRUE con 1 FALSE con 0 BUTTON_DOWN con 0 BUTTON_UP con 1 ;-------------------------------------------------------------------- ;[SERIAL CONNECTIONS] BT_IN con P14 ;Output pin for BlueSmirf BT_OUT con P15 ;Input pin for BlueSmirf BT_BAUTE con i9600 ;BlueSmirf Baute rate SSC_OUT con P13 ;Output pin for SSC32 on BotBoard SSC_IN con P12 ;Input pin for SSC32 on BotBoard SSC_BAUTE con i38400 ;SSC32 Baute rate ;-------------------------------------------------------------------- ;[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 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 ;-------------------------------------------------------------------- ;[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 con 19 ;Middle Right leg Hip Horizontal RMFemurOffset con 16 ;Middle Right leg Hip Vertical RMTibiaOffset con -44 ;Middle Right leg Knee RRCoxaOffset con -75 ;Rear Right leg Hip Horizontal RRFemurOffset con 12 ;Rear Right leg Hip Vertical RRTibiaOffset con 66 ;Rear Right leg Knee LFCoxaOffset con 42 ;Front Left leg Hip Horizontal LFFemurOffset con -3 ;Front Left leg Hip Vertical LFTibiaOffset con -66 ;Front Left leg Knee LMCoxaOffset con -33 ;Middle Left leg Hip Horizontal LMFemurOffset con 21 ;Middle Left leg Hip Vertical LMTibiaOffset 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 con -58 ;Mechanical limits of the Right Middle Leg RMCoxa_MAX con 96 RMFemur_MIN con -92 RMFemur_MAX con 95 RMTibia_MIN con -105 RMTibia_MAX con 72 RRCoxa_MIN con -100 ;Mechanical limits of the Right Rear Leg RRCoxa_MAX con 52 RRFemur_MIN con -96 RRFemur_MAX con 95 RRTibia_MIN con -96 RRTibia_MAX con 82 LFCoxa_MIN con -80 ;Mechanical limits of the Left Front Leg LFCoxa_MAX con 95 LFFemur_MIN con -97 LFFemur_MAX con 96 LFTibia_MIN con -81 LFTibia_MAX con 104 LMCoxa_MIN con -95 ;Mechanical limits of the Left Middle Leg LMCoxa_MAX con 58 LMFemur_MIN con -94 LMFemur_MAX con 96 LMTibia_MIN con -81 LMTibia_MAX con 102 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 ;Distance X from center of the body to the Right Front coxa RFOffsetZ con -88 ;Distance Z from center of the body to the Right Front coxa RMOffsetX con -65 ;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 -42 ;Distance X from center of the body to the Right Rear coxa RROffsetZ 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 coxa LFOffsetZ con -88 ;Distance Z from center of the body to the Left Front coxa LMOffsetX con 65 ;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 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 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 ;GetSinCos AngleDeg var float ;Input Angle in degrees ABSAngleDeg var float ;Absolute value of the Angle in Degrees AngleRad var float ;Angle in Radian Sinus var float ;Output Sinus of the given Angle Cosinus 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 sbyte 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 BodyOffsetX var sbyte ;Input Offset betweeen the body and Coxa X BodyOffsetZ var sbyte ;Input Offset betweeen the body and Coxa Z PitchY var sword ;PitchY offset, needs to be added to the BodyPosY RollY var sword ;RollY offset, needs to be added to the BodyPosY 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 ;-------------------------------------------------------------------- ;[WiiRemote] WiiMoteX var byte ;Wii remote inputs WiiMoteY var byte WiiMoteZ var byte NunChukX var byte NunChukY var byte NunChukZ var byte JoyX var byte JoyY var byte Buttons1 var byte Buttons2 var byte Checksum var byte Prev_Buttons1 var byte ;Previous state of the wii buttons Prev_Buttons2 var byte ;-------------------------------------------------------------------- ;[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 ;Start positions of the Right Front leg RFPosY = 15 RFPosZ = -75 RMPosX = 110 ;Start positions of the Right Middle leg RMPosY = 15 RMPosZ = 0 RRPosX = 80 ;Start positions of the Right Rear leg RRPosY = 15 RRPosZ = 75 LFPosX = 80 ;Start positions of the Left Front leg LFPosY = 15 LFPosZ = -75 LMPosX = 110 ;Start positions of the Left Middle leg LMPosY = 15 LMPosZ = 0 LRPosX = 80 ;Start positions of the Left Rear leg LRPosY = 15 LRPosZ = 75 ;Body Positions BodyPosX = 0 BodyPosY = 0 BodyPosZ = 0 ;Body Rotations BodyRotX = 0 BodyRotY = 0 BodyRotZ = 0 ;-------------------------------------------------------------------- ;[MAIN] main: ;Button A if (butA = BUTTON_DOWN) AND (prev_butA = BUTTON_UP) then sound P9,[100\4000] ;Used for test code endif ;Button B if (butB = BUTTON_DOWN) AND (prev_butB = BUTTON_UP) then sound P9,[100\4000] ;Used for test code endif ;Button C if (butC = BUTTON_DOWN) AND (prev_butC = BUTTON_UP) then sound P9,[100\4000] ;Used for test code endif ;Front Right leg gosub BodyIK [-RFPosX+BodyPosX, RFPosZ+BodyPosZ, RFOffsetX, RFOffsetZ] gosub LegIK [RFPosX-BodyPosX+BodyIKPosX, RFPosY+BodyPosY+BodyIKPosY, RFPosZ+BodyPosZ-BodyIKPosZ] RFCoxaAngle = IKCoxaAngle + 60 ;60 degree for the basic setup for the front leg RFFemurAngle = IKFemurAngle RFTibiaAngle = IKTibiaAngle ;Middle Right leg gosub BodyIK [-RMPosX+BodyPosX, RMPosZ+BodyPosZ, RMOffsetX, RMOffsetZ] gosub LegIK [RMPosX-BodyPosX+BodyIKPosX, RMPosY+BodyPosY+BodyIKPosY, RMPosZ+BodyPosZ-BodyIKPosZ] RMCoxaAngle = IKCoxaAngle RMFemurAngle = IKFemurAngle RMTibiaAngle = IKTibiaAngle ;Rear Right leg gosub BodyIK [-RRPosX+BodyPosX, RRPosZ+BodyPosZ, RROffsetX, RROffsetZ] gosub LegIK [RRPosX-BodyPosX+BodyIKPosX, RRPosY+BodyPosY+BodyIKPosY, RRPosZ+BodyPosZ-BodyIKPosZ] RRCoxaAngle = IKCoxaAngle - 60 ;60 degree for the basic setup for the front leg RRFemurAngle = IKFemurAngle RRTibiaAngle = IKTibiaAngle ;Front Left leg gosub BodyIK [LFPosX-BodyPosX, LFPosZ+BodyPosZ, LFOffsetX, LFOffsetZ] gosub LegIK [LFPosX+BodyPosX-BodyIKPosX, LFPosY+BodyPosY+BodyIKPosY, LFPosZ+BodyPosZ-BodyIKPosZ] LFCoxaAngle = IKCoxaAngle + 60 ;60 degree for the basic setup for the front leg LFFemurAngle = IKFemurAngle LFTibiaAngle = IKTibiaAngle ;Middle Left leg gosub BodyIK [LMPosX-BodyPosX, LMPosZ+BodyPosZ, LMOffsetX, LMOffsetZ] gosub LegIK [LMPosX+BodyPosX-BodyIKPosX, LMPosY+BodyPosY+BodyIKPosY, LMPosZ+BodyPosZ-BodyIKPosZ] LMCoxaAngle = IKCoxaAngle LMFemurAngle = IKFemurAngle LMTibiaAngle = IKTibiaAngle ;Rear Left leg gosub BodyIK [LRPosX-BodyPosX, LRPosZ+BodyPosZ, LROffsetX, LROffsetZ] gosub LegIK [LRPosX+BodyPosX-BodyIKPosX, LRPosY+BodyPosY+BodyIKPosY, LRPosZ+BodyPosZ-BodyIKPosZ] 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 WiiInput 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 ;-------------------------------------------------------------------- ;[WiiInput] reads the input data from the Wiiremote and processes the ;data to the parameters. Beeps on a wrong checksum. ;Input using BT_IN, frame format ; ; ;Output: WiiMoteX, WiiMoteY, WiiMoteZ, NunChukX, NunChukY, NunChukZ, JoyX, JoyY, Buttons1, Buttons2 WiiInput: WaitForFrame: ;Waiting for start byte SERIN BT_IN, BT_BAUTE, [ihex2 Checksum] IF(Checksum <> 255) THEN GOTO WaitForFrame ENDIF ;Store Previous buttons state Prev_Buttons1 = Buttons1 Prev_Buttons2 = Buttons2 SERIN BT_IN, BT_BAUTE, [ihex2 WiiMoteX, ihex2 WiiMoteY, ihex2 WiiMoteZ, ihex2 NunChukX, ihex2 NunChukY, ihex2 NunChukZ, | ihex2 JoyX, ihex2 JoyY, ihex2 Buttons1, ihex2 Buttons2 , ihex2 Checksum] IF(Checksum <> (WiiMoteX + WiiMoteY + WiiMoteZ + NunChukX + NunChukY + NunChukZ + JoyX + JoyY + Buttons1 + Buttons2)//255) THEN sound p9, [50\2250] ; Beep ! Wrong checksum ; SEROUT S_OUT, S_BAUTE, ["WX", hex WiiMoteX\2," WY", hex WiiMoteY\2," WZ", hex WiiMoteZ\2," CX", hex NunChukX\2, | ; " CY", hex NunChukY\2," CZ", hex NunChukZ\2, " JX", hex JoyX\2, " JY", hex JoyY\2, | ; " BA", hex Buttons1\2, " BB", hex Buttons2\2, " Checksum", hex Checksum\2, 13] ELSE ;Correct data IF(Buttons1.bit1) THEN ;button B BodyRotZ = (WiiMoteX - 128) BodyRotX = -WiiMoteY - 128 BodyRotY = (NunChukX - 128) / 2 ENDIF IF(Buttons1.bit3 = 1) AND (Prev_Buttons1.bit5 = 0) THEN ;Button home BodyRotZ=0 BodyRotX=0 BodyRotY=0 ENDIF IF(Buttons1.bit5 = 1) AND (Prev_Buttons1.bit5 = 0) THEN ;Button one gosub walkinit ENDIF IF(Buttons1.Bit6 = 1) AND (Prev_Buttons1.bit6 = 0) THEN ;Button two IF (tempgaiton = 0) THEN tempgaiton = 1 ELSE Tempgaiton = 0 ENDIF ENDIF IF(Buttons2.bit4) THEN ;Button C (16) BodyPosY = -(NunChukY - 128) * 2 ENDIF IF(Buttons2.bit5) THEN ;Button Z (32) BodyPosX = (JoyX - 128) / 2 BodyPosZ = (JoyY - 128) / 2 ENDIF ENDIF return ;-------------------------------------------------------------------- ;[GAIT] ;NOT YET IMPLEMENTED ;-------------------------------------------------------------------- ;[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-(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 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 range AngleDeg = AngleDeg -270.0 ;Convert degree to radials AngleRad = (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) 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 ;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] ;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)] 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)*(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,["T400",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 ;--------------------------------------------------------------------