Hola Dragonet80 !
Ya he visto que la foto no se ve pero es que no recuerdo que foto era ni donde la tengo (hace mucho que no visito este hilo del foro), pero si te pasas por
http://migranjadigital.blogspot.com " onclick="window.open(this.href);return false;
podrás ver a S.A.M. en acción en un video grabado el agosto pasado además de algunas fotos y detalles del proyecto.
Por cierto la controladora de pololu tiene buena pinta, que vas a usar, una por cada par de patas?

Salu2 😉

bek

alguien me puede hechar una mano con lo que comentaba antes.

Ademas no se porque pero cuando alimento solo la parte de logica, los servos tambien se mueven y eso que tengo separadas las alimentaciones (con el jumper), parece como si lo hiciese la placa de recepcion del mando de PS2,

Supongo que habras probado a medir continuidad entre la alimentacion de los servos y la del mando de play no?

-Hola , intento pasar el codigo de atomb pro 28 (v1.3) del hexapodo a basicx24 pero no consigo hacerlo funcionar, no se que estoy haciendo mal, necesito ayuda. Pongo el codigo.

'Option Explicit

'Project Lynxmotion Phoenix
'Description: Phoenix, controlled by a PS2 remote
'Software version: V1.3
'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.3
' - Changed controls L1+ right stick
' - Balance calculations Thanks to Kåre Halvorsen (aka Zenta)

'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
' - Square Switch Balance mode on or off
' - L1 & L. Stick Shift body X/Z
' - L1 & R. Stick Shift body Y and rotate body Y
' - L2 & Sticks Rotate body X/Y/Z
' - R2 & L. Stick Double gait travel length

'KNOWN BUGS:
' - None at the moment 😉
'Pin 16 Basicx SSC_Out p11 ABB2
Const LedA_Pin As Byte = 9 'p4 ABB2
Const LedB_Pin As Byte = 10 'p5 ABB2
Const LedC_Pin As Byte = 11 'p6 ABB2
Const PS2DAT As Byte = 17 'p12 ABB2 PS2 Controller DAT (Brown)
Const PS2CMD As Byte = 18 'p13 ABB2 PS2 controller CMD (Orange)
Const PS2SEL As Byte = 19 'p14 ABB2 PS2 Controller SEL (Blue)
Const PS2CLK As Byte = 20 'p15 ABB2 PS2 Controller CLK (White)
Public Const PadMode As Byte = &H79

'====================================================================
'[CONSTANDS]
Public Const Verdad As Byte = 1
Public Const Falso As Byte = 0

'--------------------------------------------------------------------
'[PIN NUMBERS]

Public Const RRCoxaP As Byte = 24 'Rear Right leg Hip Horizontal
Public Const RRFemurP As Byte = 25 'Rear Right leg Hip Vertical
Public Const RRTibiaP As Byte = 26 'Rear Right leg Knee

Public Const RMCoxaP As Byte = 20 'Middle Right leg Hip Horizontal
Public Const RMFemurP As Byte = 21 'Middle Right leg Hip Vertical
Public Const RMTibiaP As Byte = 22 'Middle Right leg Knee

Public Const RFCoxaP As Byte = 16 'Front Right leg Hip Horizontal
Public Const RFFemurP As Byte = 17 'Front Right leg Hip Vertical
Public Const RFTibiaP As Byte = 18 'Front Right leg Knee

Public Const LRCoxaP As Byte = 8 'Rear Left leg Hip Horizontal
Public Const LRFemurP As Byte = 9 'Rear Left leg Hip Vertical
Public Const LRTibiaP As Byte = 10 'Rear Left leg Knee

Public Const LMCoxaP As Byte = 4 'Middle Left leg Hip Horizontal
Public Const LMFemurP As Byte = 5 'Middle Left leg Hip Vertical
Public Const LMTibiaP As Byte = 6 'Middle Left leg Knee

Public Const LFCoxaP As Byte = 0 'Front Left leg Hip Horizontal
Public Const LFFemurP As Byte = 1 'Front Left leg Hip Vertical
Public Const LFTibiaP As Byte = 2 'Front Left leg Knee

'--------------------------------------------------------------------
'[MIN/MAX ANGLES]
Public Const RRCoxa_MIN As Integer = -55 'Mechanical limits of the Right Rear Leg
Public Const RRCoxa_MAX As Integer = 60
Public Const RRFemur_MIN As Integer = -95
Public Const RRFemur_MAX As Integer = 90
Public Const RRTibia_MIN As Integer = -97
Public Const RRTibia_MAX As Integer = 75

Public Const RMCoxa_MIN As Integer = -53 'Mechanical limits of the Right Middle Leg
Public Const RMCoxa_MAX As Integer = 53
Public Const RMFemur_MIN As Integer = -95
Public Const RMFemur_MAX As Integer = 90
Public Const RMTibia_MIN As Integer = -97
Public Const RMTibia_MAX As Integer = 75

Public Const RFCoxa_MIN As Integer = -35 'Mechanical limits of the Right Front Leg
Public Const RFCoxa_MAX As Integer = 50
Public Const RFFemur_MIN As Integer = -95
Public Const RFFemur_MAX As Integer = 90
Public Const RFTibia_MIN As Integer = -97
Public Const RFTibia_MAX As Integer = 75

Public Const LRCoxa_MIN As Integer = -60 'Mechanical limits of the Left Rear Leg
Public Const LRCoxa_MAX As Integer = 55
Public Const LRFemur_MIN As Integer = -90
Public Const LRFemur_MAX As Integer = 95
Public Const LRTibia_MIN As Integer = -75
Public Const LRTibia_MAX As Integer = 97

Public Const LMCoxa_MIN As Integer = -53 'Mechanical limits of the Left Middle Leg
Public Const LMCoxa_MAX As Integer = 53
Public Const LMFemur_MIN As Integer = -90
Public Const LMFemur_MAX As Integer = 95
Public Const LMTibia_MIN As Integer = -75
Public Const LMTibia_MAX As Integer = 97

Public Const LFCoxa_MIN As Integer = -50 'Mechanical limits of the Left Front Leg
Public Const LFCoxa_MAX As Integer = 35
Public Const LFFemur_MIN As Integer = -90
Public Const LFFemur_MAX As Integer = 95
Public Const LFTibia_MIN As Integer = -75
Public Const LFTibia_MAX As Integer = 97
'--------------------------------------------------------------------
'[BODY DIMENSIONS]
Public Const CoxaLength As Byte = 20 'Length of the Coxa [mm]
Public Const FemurLength As Byte = 80 'Length of the Femur [mm]
Public Const TibiaLength As Byte = 130 'Lenght of the Tibia [mm]

Public Const CoxaAngle As Byte = 35 'Default Coxa setup angle

Public Const RFOffsetX As Integer = -40 'Distance X from center of the body to the Right Front coxa
Public Const RFOffsetZ As Integer = -80 'Distance Z from center of the body to the Right Front coxa
Public Const RMOffsetX As Integer = -60 'Distance X from center of the body to the Right Middle coxa
Public Const RMOffsetZ As Integer = 0 'Distance Z from center of the body to the Right Middle coxa
Public Const RROffsetX As Integer = -40 'Distance X from center of the body to the Right Rear coxa
Public Const RROffsetZ As Integer = 80 'Distance Z from center of the body to the Right Rear coxa

Public Const LFOffsetX As Integer = 40 'Distance X from center of the body to the Left Front coxa
Public Const LFOffsetZ As Integer = -80 'Distance Z from center of the body to the Left Front coxa
Public Const LMOffsetX As Integer = 60 'Distance X from center of the body to the Left Middle coxa
Public Const LMOffsetZ As Integer = 0 'Distance Z from center of the body to the Left Middle coxa
Public Const LROffsetX As Integer = 40 'Distance X from center of the body to the Left Rear coxa
Public Const LROffsetZ As Integer = 80 'Distance Z from center of the body to the Left Rear coxa
'--------------------------------------------------------------------
'[REMOTE]
Public Const TravelDeadZone As Byte = 4 'The deadzone for the analog input from the remote
'=====================================================================
'[ANGLES]
Public RFCoxaAngle As Integer 'Actual Angle of the Right Front Leg
Public RFFemurAngle As Integer
Public RFTibiaAngle As Integer

Public RMCoxaAngle As Integer 'Actual Angle of the Right Middle Leg
Public RMFemurAngle As Integer
Public RMTibiaAngle As Integer

Public RRCoxaAngle As Integer 'Actual Angle of the Right Rear Leg
Public RRFemurAngle As Integer
Public RRTibiaAngle As Integer

Public LFCoxaAngle As Integer 'Actual Angle of the Left Front Leg
Public LFFemurAngle As Integer
Public LFTibiaAngle As Integer

Public LMCoxaAngle As Integer 'Actual Angle of the Left Middle Leg
Public LMFemurAngle As Integer
Public LMTibiaAngle As Integer

Public LRCoxaAngle As Integer 'Actual Angle of the Left Rear Leg
Public LRFemurAngle As Integer
Public LRTibiaAngle As Integer
'--------------------------------------------------------------------
'[POSITIONS]
Public RFPosX As Integer 'Actual Position of the Right Front Leg
Public RFPosY As Integer
Public RFPosZ As Integer

Public RMPosX As Integer 'Actual Position of the Right Middle Leg
Public RMPosY As Integer
Public RMPosZ As Integer

Public RRPosX As Integer 'Actual Position of the Right Rear Leg
Public RRPosY As Integer
Public RRPosZ As Integer

Public LFPosX As Integer 'Actual Position of the Left Front Leg
Public LFPosY As Integer
Public LFPosZ As Integer

Public LMPosX As Integer 'Actual Position of the Left Middle Leg
Public LMPosY As Integer
Public LMPosZ As Integer

Public LRPosX As Integer 'Actual Position of the Left Rear Leg
Public LRPosY As Integer
Public LRPosZ As Integer
'--------------------------------------------------------------------
'[VARIABLES]

'GetSinCos
Public AngleDeg As Single 'Input Angle in degrees
Public ABSAngleDeg As Single 'Absolute value of the Angle in Degrees
Public AngleRad As Single 'Angle in Radian
Public sinA As Single 'Output Sinus of the given Angle
Public cosA As Single 'Output Cosinus of the given Angle

'GetBoogTan
Public BoogTanX As Integer 'Input X
Public BoogTanY As Integer 'Input Y
Public BoogTan As Single 'Output BOOGTAN2(X/Y)

'Body position
Public BodyPosX As Integer 'Global Input for the position of the body
Public BodyPosY As Integer
Public BodyPosZ As Integer

'Body Inverse Kinematics
Public BodyRotX As Integer 'Global Input pitch of the body
Public BodyRotY As Integer 'Global Input rotation of the body
Public BodyRotZ as Integer 'Global Input roll of the body
Public PosX As Integer 'Input position of the feet X
Public PosZ As Integer 'Input position of the feet Z
Public PosY As Integer 'Input position of the feet I
Public RotationY As Integer 'Input for rotation of a single feet for the gait
Public BodyOffsetX As Integer 'Input Offset betweeen the body and Coxa X
Public BodyOffsetZ As Integer 'Input Offset betweeen the body and Coxa Z
Public sinB As Single 'Sin buffer for BodyRotX calculations
Public cosB As Single 'Cos buffer for BodyRotX calculations
Public sinG As Single 'Sin buffer for BodyRotZ calculations
Public cosG As Single 'Cos buffer for BodyRotZ calculations
Public TotalX As Integer 'Total X distance between the center of the body and the feet
Public TotalZ As Integer 'Total Z distance between the center of the body and the feet
Public DistCenterBodyFeet As Single 'Total distance between the center of the body and the feet
Public AngleCenterBodyFeetX As Single 'Angle between the center of the body and the feet
Public BodyIKPosX As Integer 'Output Position X of feet with Rotation
Public BodyIKPosY As Integer 'Output Position Y of feet with Rotation
Public BodyIKPosZ As Integer 'Output Position Z of feet with Rotation

'Leg Inverse Kinematics
Public IKFeetPosX As Integer 'Input position of the Feet X
Public IKFeetPosY As Integer 'Input position of the Feet Y
Public IKFeetPosZ As Integer 'Input Position of the Feet Z
Public IKFeetPosXZ As Integer 'Length between the coxa and feet
Public IKSW As Single 'Length between shoulder and wrist
Public IKA1 As Single 'Angle between SW line and the ground in rad
Public IKA2 As Single '?
Public IKSolution As Boolean 'Output true if the solution is possible
Public IKSolutionWarning As Boolean 'Output true if the solution is NEARLY possible
Public IKSolutionError As Boolean 'Output true if the solution is NOT possible
Public IKFemurAngle As Integer 'Output Angle of Femur in degrees
Public IKTibiaAngle As Integer 'Output Angle of Tibia in degrees
Public IKCoxaAngle As Integer 'Output Angle of Coxa in degrees
'--------------------------------------------------------------------
'[Ps2 Controller]
Public DualShock(1 to 7) As Byte
Public LastButton (1 to 2) As Byte'(2)
Public DS2Mode As Byte
Public BodyYShift As Integer
'--------------------------------------------------------------------
'[TIMING]
Public CycleTime As New UnsignedInteger 'Total Cycle time

Public SSCTime As New UnsignedInteger 'Time for servo updates
Public PrevSSCTime As New UnsignedInteger 'Previous time for the servo updates

Public InputTimeDelay As Byte 'Delay that depends on the input to get the "sneaking" effect
'--------------------------------------------------------------------
'[GLOABAL]
Public HexOn As Boolean 'Switch to turn on Phoenix
'--------------------------------------------------------------------
'[BALANCE]
Public BalanceMode As Byte
Public TravelHeightY As Integer
Public TotalTransX As Integer
Public TotalTransZ As Integer
Public TotalTransY As Integer
Public TotalYBal As Integer
Public TotalXBal As Integer
Public TotalZBal As Integer
Public TotalY As Integer
'------------------------------------------------------------------------
'[gait]
Public GaitType As Byte 'Gait type
Public NomGaitSpeed As Byte 'Nominal speed of the gait

Public LegLiftHeight As Byte 'Current Travel height
Public TravelLengthX As Integer 'Current Travel length X
Public TravelLengthZ As Integer 'Current Travel length Z
Public TravelRotationY As Integer 'Current Travel Rotation Y

Public TLDivFactor As Byte 'Number of steps that a leg is on the floor while walking
Public NrLiftedPos As Byte 'Number of positions that a single leg is lifted (1-3)
Public HalfLiftHeigth As Byte 'If TRUE the outer positions of the ligted legs will be half height

Public GaitInMotion As Boolean 'Temp to check if the gait is in motion
Public StepsInGait As Byte 'Number of steps in gait
Public LastLeg As Boolean 'TRUE when the current leg is the last leg of the sequence
Public GaitStep As Byte 'Actual Gait step

Public RFGaitLegNr As Byte 'Init position of the leg
Public RMGaitLegNr As Byte 'Init position of the leg
Public RRGaitLegNr As Byte 'Init position of the leg
Public LFGaitLegNr As Byte 'Init position of the leg
Public LMGaitLegNr As Byte 'Init position of the leg
Public LRGaitLegNr As Byte 'Init position of the leg

Public GaitLegNr As Byte 'Input Number of the leg
Public TravelMulti As Integer 'Multiplier for the length of the step

Public RFGaitPosX As Integer 'Relative position corresponding to the Gait
Public RFGaitPosY As Integer
Public RFGaitPosZ As Integer
Public RFGaitRotY As Integer 'Relative rotation corresponding to the Gait

Public RMGaitPosX As Integer
Public RMGaitPosY As Integer
Public RMGaitPosZ As Integer
Public RMGaitRotY As Integer

Public RRGaitPosX As Integer
Public RRGaitPosY As Integer
Public RRGaitPosZ As Integer
Public RRGaitRotY As Integer

Public LFGaitPosX As Integer
Public LFGaitPosY As Integer
Public LFGaitPosZ As Integer
Public LFGaitRotY As Integer

Public LMGaitPosX As Integer
Public LMGaitPosY As Integer
Public LMGaitPosZ As Integer
Public LMGaitRotY As Integer

Public LRGaitPosX As Integer
Public LRGaitPosY As Integer
Public LRGaitPosZ As Integer
Public LRGaitRotY As Integer

Public GaitPosX As Integer 'In-/Output Pos X of feet
Public GaitPosY As Integer 'In-/Output Pos Y of feet
Public GaitPosZ As Integer 'In-/Output Pos Z of feet
Public GaitRotY As Integer 'In-/Output Rotation Y of feet
'====================================================================
''[TIMER INTERRUPT INIT]
''ONASMINTERRUPT TIMERWINT, Handle_TIMERW
'====================================================================
'[INIT]
Public Sub Main()

'Turning off all the leds
Call Putpin (LedA_Pin, bxOutputLow)
Call Putpin (LedB_Pin, bxOutputLow)
Call Putpin (LedC_Pin, bxOutputLow)

'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
BalanceMode = 0
LegLiftHeight = 50
GaitStep = 1
Call GaitSelect 'GOSUB GaitSelect

'Timer
Register.TCCR1A = 0
Register.TCCR1B = 0
Register.TCNT1H = 0
Register.TCNT1L = 0
Const ICF1 As Byte = bx00100000
Register.TIFR = ICF1
Const TOV1 As Byte = bx00000100
Register.TIFR = TOV1
Const ICES1 As Byte = bx01000000
Register.TCCR1B = ICES1
Register.TCCR1B = Register.TCCR1B + 4

'PS2 controller
Call PutPin(PS2CLK, bxOutPutHigh)
LastButton(0) = 255
LastButton(1) = 255
BodyYShift = 0

'SSC
SSCTime = 150
HexOn = False
'====================================================================
'[MAIN]
Do
''Register.TCCR1B = Register.TCCR1B + 4

Dim HasOverflowed As Boolean, TIFRcopy As Byte
HasOverflowed = False
Do
TIFRcopy = Register.TIFR
'Bail out if timer overflows.
If ((TIFRcopy And TOV1) = TOV1) Then
HasOverflowed = True
Exit Do
End If
Loop Until ((TIFRcopy And ICF1) = ICF1) ' Stop if edge is detected.

'Reset IKsolution indicators
IKSolution = False
IKSolutionWarning = False
IKSolutionError = False

'Gait
Call GaitSeq

'Balance Calculations
TotalTransX = 0 'reset values used for calculation of balance
TotalTransZ = 0
TotalTransY = 0
TotalXBal = 0
TotalYBal = 0
TotalZBal = 0
IF (BalanceMode>0) THEN
Call BalCalcOneLeg (-RFPosX+BodyPosX+RFGaitPosX, RFPosZ+BodyPosZ+RFGaitPosZ,RFGaitPosY, RFOffsetX, RFOffsetZ)
Call BalCalcOneLeg (-RMPosX+BodyPosX+RMGaitPosX, RMPosZ+BodyPosZ+RMGaitPosZ,RMGaitPosY, RMOffsetX, RMOffsetZ)
Call BalCalcOneLeg (-RRPosX+BodyPosX+RRGaitPosX, RRPosZ+BodyPosZ+RRGaitPosZ,RRGaitPosY, RROffsetX, RROffsetZ)
Call BalCalcOneLeg (LFPosX-BodyPosX+LFGaitPosX, LFPosZ+BodyPosZ+LFGaitPosZ,LFGaitPosY, LFOffsetX, LFOffsetZ)
Call BalCalcOneLeg (LMPosX-BodyPosX+LMGaitPosX, LMPosZ+BodyPosZ+LMGaitPosZ,LMGaitPosY, LMOffsetX, LMOffsetZ)
Call BalCalcOneLeg (LRPosX-BodyPosX+LRGaitPosX, LRPosZ+BodyPosZ+LRGaitPosZ,LRGaitPosY, LROffsetX, LROffsetZ)
Call BalanceBody
END IF

'Reset IKsolution indicators
IKSolution = False
IKSolutionWarning = False
IKSolutionError = False

'Right Front leg

Call BodyIk (-RFPosX+BodyPosX+RFGaitPosX, RFPosZ+BodyPosZ+RFGaitPosZ, RFPosY+BodyPosY+RFGaitPosY, RFOffsetX, RFOffsetZ, RFGaitRotY)
Call LegIK (RFPosX-BodyPosX+BodyIKPosX-RFGaitPosX, RFPosY+BodyPosY-BodyIKPosY+RFGaitPosY, RFPosZ+BodyPosZ-BodyIKPosZ+RFGaitPosZ)
RFCoxaAngle = IKCoxaAngle + CInt(CoxaAngle) 'Angle for the basic setup for the front leg
RFFemurAngle = IKFemurAngle
RFTibiaAngle = IKTibiaAngle

'Right Middle leg
Call BodyIK (-RMPosX+BodyPosX+RMGaitPosX, RMPosZ+BodyPosZ+RMGaitPosZ, RMPosY+BodyPosY+RMGaitPosY, RMOffsetX, RMOffsetZ, RMGaitRotY)
Call LegIK (RMPosX-BodyPosX+BodyIKPosX-RMGaitPosX, RMPosY+BodyPosY-BodyIKPosY+RMGaitPosY, RMPosZ+BodyPosZ-BodyIKPosZ+RMGaitPosZ)
RMCoxaAngle = IKCoxaAngle
RMFemurAngle = IKFemurAngle
RMTibiaAngle = IKTibiaAngle

'Right Rear leg
Call BodyIK (-RRPosX+BodyPosX+RRGaitPosX, RRPosZ+BodyPosZ+RRGaitPosZ, RRPosY+BodyPosY+RRGaitPosY, RROffsetX, RROffsetZ, RRGaitRotY)
Call LegIK (RRPosX-BodyPosX+BodyIKPosX-RRGaitPosX, RRPosY+BodyPosY-BodyIKPosY+RRGaitPosY, RRPosZ+BodyPosZ-BodyIKPosZ+RRGaitPosZ)
RRCoxaAngle = IKCoxaAngle - CInt(CoxaAngle) 'Angle for the basic setup for the front leg
RRFemurAngle = IKFemurAngle
RRTibiaAngle = IKTibiaAngle

'Left Front leg
Call BodyIK (LFPosX-BodyPosX+LFGaitPosX, LFPosZ+BodyPosZ+LFGaitPosZ, LFPosY+BodyPosY+LFGaitPosY, LFOffsetX, LFOffsetZ, LFGaitRotY)
Call LegIK (LFPosX+BodyPosX-BodyIKPosX+LFGaitPosX, LFPosY+BodyPosY-BodyIKPosY+LFGaitPosY, LFPosZ+BodyPosZ-BodyIKPosZ+LFGaitPosZ)
LFCoxaAngle = IKCoxaAngle + CInt(CoxaAngle) 'Angle for the basic setup for the front leg
LFFemurAngle = IKFemurAngle
LFTibiaAngle = IKTibiaAngle

'Left Middle leg
Call BodyIK (LMPosX-BodyPosX+LMGaitPosX, LMPosZ+BodyPosZ+LMGaitPosZ, LMPosY+BodyPosY+LMGaitPosY, LMOffsetX, LMOffsetZ, LMGaitRotY)
Call LegIK (LMPosX+BodyPosX-BodyIKPosX+LMGaitPosX, LMPosY+BodyPosY-BodyIKPosY+LMGaitPosY, LMPosZ+BodyPosZ-BodyIKPosZ+LMGaitPosZ)
LMCoxaAngle = IKCoxaAngle
LMFemurAngle = IKFemurAngle
LMTibiaAngle = IKTibiaAngle

'Left Rear leg
Call BodyIK (LRPosX-BodyPosX+LRGaitPosX, LRPosZ+BodyPosZ+LRGaitPosZ, LRPosY+BodyPosY+LRGaitPosY, LROffsetX, LROffsetZ, LRGaitRotY)
Call LegIK (LRPosX+BodyPosX-BodyIKPosX+LRGaitPosX, LRPosY+BodyPosY-BodyIKPosY+LRGaitPosY, LRPosZ+BodyPosZ-BodyIKPosZ+LRGaitPosZ)
LRCoxaAngle = IKCoxaAngle - CInt(CoxaAngle) 'Angle for the basic setup for the front leg
LRFemurAngle = IKFemurAngle
LRTibiaAngle = IKTibiaAngle

Call CheckAngles

Call WriteLeds
'Read input
''Do
Call Ps2Input

''Debug.Print CStr(DualShock(1)) & "*" & CStr(DualShock(2)) & "*" & CStr(DualShock(3)) _
''& "*" & CStr(DualShock(4)) & "*" & CStr(DualShock(5)) & "*" & CStr(DualShock(6))

''Call Sleep (100)

''Loop

Dim LowByte As Byte, HighByte As Byte, Count As New UnsignedInteger

LowByte = Register.ICR1L
HighByte = Register.ICR1H

Count = CuInt(HighByte) * 256 + CuInt(LowByte)

CycleTime = Count ''* 3.47222222E-5 'WTIMERTICSPERMS

''Register.TCCR1B = 0

IF (HexOn) THEN
'Wait for previous commands to be completed while walking
IF ((ABS(TravelLengthX)>CInt(TravelDeadZone)) Or _
(ABS(TravelLengthZ)>CInt(TravelDeadZone)) Or _
(ABS(TravelRotationY*2)>CInt(TravelDeadZone))) THEN

IF (PrevSSCTime - CycleTime) > 0 THEN
Call Delay (CSng(PrevSSCTime - CycleTime)) 'MIN 1 ' Min 1 ensures that there alway is a value in the pause command
ELSE
Call Delay(0.1)
END IF

IF (BalanceMode = 0) THEN
SSCTime = CUInt((NomGaitSpeed) + (InputTimeDelay*2))
ELSE
SSCTime = CUInt((NomGaitSpeed) + (InputTimeDelay*2)) + 100
END IF

ELSE
SSCTime = 200 'NomGaitSpeed
END IF

Call ServoDriver
ELSE
'Turn the bot off
Call FreeServos
END IF
Loop
'GoTo Main
End Sub

'====================================================================
'[WriteLEDs] Updates the state of the leds
Public Sub WriteLeds()
if IKSolutionWarning = True THEN
Call Putpin (LedC_Pin, bxOutPutHigh)
Else
Call Putpin (LedC_Pin, bxOutPutLow)
END IF
if IKSolutionError = True THEN
Call Putpin (LedA_Pin, bxOutPutHigh)

Else
Call Putpin (LedA_Pin, bxOutPutLow)
END IF

End Sub
'--------------------------------------------------------------------

Public Sub Ps2Input()

Dim I As Byte
Dim PutData (1) As Byte
PutData(&H1)=FlipBits(&H1)
PutData(&H42)=FlipBits(&H42)
PutData(&H43)=FlipBits(&H43)
PutData(&H0)=FlipBits(&H0)
PutData(&H01)=FlipBits(&H01)
PutData(&H44)=FlipBits(&H44)
PutData(&H00)=FlipBits(&H00)
PutData(&H03)=FlipBits(&H03)
PutData(&H4F)=FlipBits(&H4F)
PutData(&HFF)=FlipBits(&HFF)
PutData(&H5A)=FlipBits(&H5A)

Call PutPin (PS2SEL, bxOutPutLow)
Call shiftout (PS2CMD,PS2CLK,8,PutData(&H1))
DS2Mode = shiftin(PS2DAT,PS2CLK,8)
DS2Mode = FlipBits(DS2Mode)
Call PutPin(PS2SEL, bxOutPutHigh)
Call Sleep (1)

Call PutPin(PS2SEL, bxOutPutLow)
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&H1))
Call shiftout (PS2CMD, PS2CLK, 8, PutData(&H42))
For I =0 to 6
DualShock(I) = shiftin (PS2DAT,PS2CLK, 8)
DualShock(I) = FlipBits(DualShock(I))
Next
Call PutPin (PS2SEL, bxOutPutHigh)
Call Sleep (1)

if DS2Mode <> PadMode THEN
Call PutPin (PS2SEL, bxOutPutLow)
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&H1))
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&H43))
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&H0))
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&H1))
Call shiftout (PS2CMD,PS2CLK, 8, PuTData(&H0)) 'CONFIG_MODE_ENTER
Call PutPin (PS2SEL, bxOutPutHigh)
Call Sleep (1)

Call PutPin (PS2SEL, bxOutPutLow)
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&H01))
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&H44))
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&H00))
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&H01))
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&H03))
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&H00))
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&H00))
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&H00))
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&H00)) 'SET_MODE_AND_LOCK
Call PutPin (PS2SEL, bxOutPutHigh)
Call Sleep (1)

Call PutPin (PS2SEL, bxOutPutLow)
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&H01))
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&H4F))
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&H00))
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&HFF))
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&HFF))
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&H03))
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&H00))
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&H00))
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&H00)) 'SET_DS2_NATIVE_MODE
Call PutPin (PS2SEL, bxOutPutHigh)
Call Sleep (1)

Call PutPin (PS2SEL, bxOutPutLow)
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&H01))
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&H43))
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&H00))
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&H00))
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&H5A))
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&H5A))
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&H5A))
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&H5A))
Call shiftout (PS2CMD,PS2CLK, 8, PutData(&H5A)) 'CONFIG_MODE_EXIT_DS2_NATIVE
Call PutPin (PS2SEL, bxOutPutHigh)
Call Sleep (1)

Call PutPin (PS2SEL, bxOutPutLow)
Call shiftout (PS2CMD,PS2CLK, 8, &H01)
Call shiftout (PS2CMD,PS2CLK, 8, &H43)
Call shiftout (PS2CMD,PS2CLK, 8, &H00)
Call shiftout (PS2CMD,PS2CLK, 8, &H00)
Call shiftout (PS2CMD,PS2CLK, 8, &H00)
Call shiftout (PS2CMD,PS2CLK, 8, &H00)
Call shiftout (PS2CMD,PS2CLK, 8, &H00)
Call shiftout (PS2CMD,PS2CLK, 8, &H00)
Call shiftout (PS2CMD,PS2CLK, 8, &H00) 'CONFIG_MODE_EXIT
Call PutPin (PS2SEL, bxOutPutHigh)
Call Sleep (100)

Call FreQout (14, 4000, 4500, 100)
Call FreQout (14, 5000, 0, 100)

END IF

Dim A As Byte, A1 As Byte, B As Byte, B1 As Byte, _
C As Byte, C1 As Byte, D As Byte, D1 As Byte, _
E as Byte, E1 As Byte, F As Byte, F1 As Byte, _
G As Byte, G1 As Byte, H As Byte, H1 As Byte, _
J As Byte, K As Byte, L As Byte, M As Byte

A = GetBit(Dualshock(1), 3)
A1 = GetBit(LastButton(0), 3)

IF A = 0 And A1 THEN
'Start Button test
IF(HexOn) THEN
'Turn off
Call FreqOut (14, 5000, 0, 100)
Call FreqOut (14, 4500, 0, 80)
Call FreqOut (14, 4000, 0, 60)
BodyPosX = 0
BodyPosY = 0
BodyPosZ = 0
BodyRotX = 0
BodyRotY = 0
BodyRotZ = 0
TravelLengthX = 0
TravelLengthZ = 0
TravelRotationY = 0

SSCTime = 600
Call ServoDriver
HexOn = False
ELSE
'Turn on
Call FreqOut (14, 4000, 0, 60)
Call FreqOut (14, 4500, 0, 80)
Call FreqOut (14, 5000, 0, 100)
SSCTime = 200
HexOn = True
END IF
END IF

IF HexOn THEN
B = GetBit(DualShock(1), 0)
B1 = GetBit(LastButton(0), 0)
IF B = 0 And B1 THEN 'Select Button test
IF (TravelLengthX=0) and _
(TravelLengthZ=0) and _
(TravelRotationY=0) THEN

'Switch to next Gait type
IF GaitType<7 THEN
Call FreqOut (14, 4000, 0, 50)
GaitType = GaitType+1
ELSE
Call FreqOut (14, 4000, 4500, 50)
GaitType = 0
END IF

Call GaitSelect
END IF
END IF

C = GetBit(DualShock(1), 4)
C1 = GetBit(LastButton(0), 4)
IF C = 0 And C1 THEN 'Up Button test
BodyPosY = BodyPosY+10
END IF

D = Getbit(DualShock(1), 6)
D1 = Getbit(Lastbutton(0), 6)
IF D = 0 And D1 THEN 'Down Button test
BodyPosY = BodyPosY-10
END IF

E = Getbit(Dualshock(2), 4)
E1 = Getbit(Lastbutton(1), 4)
IF E = 0 And E1 THEN 'Triangle Button test
BodyPosY = 35
END IF

F = Getbit(DualShock(2), 5)
F1 = Getbit(Lastbutton(1), 5)
IF F = 0 And F1 THEN 'Circle Button test
BodyPosY = 0
END IF

G = Getbit(Dualshock(2), 6)
G1 = Getbit(Lastbutton(1), 6)
IF G = 0 And G1 THEN 'Cross Button test
END IF

H = Getbit(Dualshock(2), 7)
H1 = Getbit(Lastbutton(1), 7)
IF H = 0 And H1 THEN 'Square Button test
If BalanceMode = 0 Then
BalanceMode = 1
Call FreqOut(14, 4000, 0, 100)
Call FreqOut(14, 8000, 0, 50)
ELSE
BalanceMode = 0
Call FreqOut(14, 3000, 0, 250)
END IF
END IF

J = Getbit(Dualshock(2), 3)
IF J = 0 THEN 'R1 Button test
LegLiftHeight = 80
ELSE
LegLiftHeight = 50
END IF

BodyYShift = 0
K = Getbit(Dualshock(2), 2)
If K = 0 THEN 'L1 Button test
BodyPosX = CInt(Dualshock(5) - 128)2
BodyPosZ = CInt(-(Dualshock(6) - 128))3
BodyRotY = CInt(Dualshock(3) - 128)6
BodyYShift = Cint(-(Dualshock(4) - 128))2
IF (BodyYShift < (BodyPosY - 10)) Then
BodyYShift = BodyPosY - 10
END IF

L = Getbit(Dualshock(2), 0)
ELSEIF L = 0 THEN 'L2 Button test
BodyRotX = CInt(Dualshock(6) - 128)8
BodyRotY = CInt(Dualshock(3) - 128)6
BodyRotZ = CInt(Dualshock(5) - 128)8
BodyYShift = CInt(-(Dualshock(4) - 128)2)
IF BodyYShift < (BodyPosY -10) Then
BodyYShift = BodyPosY - 10
END IF

ELSE 'Walk
' BodyPosX = 0
'BodyPosZ = 0

' BodyRotX = 0
' BodyRotY = 0
'BodyRotZ = 0

'END IF
'END IF
'END IF
'END IF
END IF

M = Getbit(Dualshock(2), 1)
IF M = 0 THEN 'R2 Button test
TravelLengthX = CInt(-(Dualshock(5) - 128))
TravelLengthZ = CInt(Dualshock(6) - 128)
ELSE
TravelLengthX = CInt(-(Dualshock(5) - 128))2
TravelLengthZ = CInt(Dualshock(6) - 128)2
END IF

TravelRotationY = CInt(-(Dualshock(3) - 128))4
END IF
'Calculate walking time delay
InputTimeDelay = 128 - (ABS(Dualshock(5) - 128))
IF 128 - (ABS(Dualshock(6) - 128)) < InputTimeDelay Then
InputTimeDelay = 128- (ABS(Dualshock(6) - 128))

ELSE

IF 128 - (ABS(Dualshock(3) - 128)) < InputTimeDelay Then
InputTimeDelay = 128 - (ABS(Dualshock(3) - 128))

END IF
END IF

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

End Sub
'--------------------------------------------------------------------
'[GAIT Select]
Public Sub 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
END IF

IF (GaitType = 1) THEN 'Ripple Gait 12 steps
LRGaitLegNr = 1
RFGaitLegNr = 3
LMGaitLegNr = 5
RRGaitLegNr = 7
LFGaitLegNr = 9
RMGaitLegNr = 11

NrLiftedPos = 3
HalfLiftHeigth = Verdad
TLDivFactor = 8
StepsInGait = 12
NomGaitSpeed = 100
END IF

IF (GaitType = 2) THEN 'Quadripple 9 steps
LRGaitLegNr = 1
RFGaitLegNr = 2
LMGaitLegNr = 4
RRGaitLegNr = 5
LFGaitLegNr = 7
RMGaitLegNr = 8

NrLiftedPos = 2
HalfLiftHeigth = Falso
TLDivFactor = 6
StepsInGait = 9
NomGaitSpeed = 150
END IF

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
END IF

IF (GaitType = 4) THEN 'Tripod 6 steps
LRGaitLegNr = 4
RFGaitLegNr = 1
LMGaitLegNr = 1
RRGaitLegNr = 1
LFGaitLegNr = 4
RMGaitLegNr = 4

NrLiftedPos = 2
HalfLiftHeigth = Falso
TLDivFactor = 4
StepsInGait = 6
NomGaitSpeed = 150
END IF

IF (GaitType = 5) THEN 'Tripod 8 steps
LRGaitLegNr = 5
RFGaitLegNr = 1
LMGaitLegNr = 1
RRGaitLegNr = 1
LFGaitLegNr = 5
RMGaitLegNr = 5

NrLiftedPos = 3
HalfLiftHeigth = Verdad
TLDivFactor = 4
StepsInGait = 8
NomGaitSpeed = 110
END IF

IF (GaitType = 6) THEN 'Wave 12 steps
LRGaitLegNr = 7
RFGaitLegNr = 1
LMGaitLegNr = 9
RRGaitLegNr = 5
LFGaitLegNr = 11
RMGaitLegNr = 3

NrLiftedPos = 1
HalfLiftHeigth = Falso
TLDivFactor = 10
StepsInGait = 12
NomGaitSpeed = 120
END IF

IF (GaitType = 7) THEN 'Wave 18 steps
LRGaitLegNr = 10
RFGaitLegNr = 1
LMGaitLegNr = 13
RRGaitLegNr = 7
LFGaitLegNr = 16
RMGaitLegNr = 4

NrLiftedPos = 2
HalfLiftHeigth = Falso
TLDivFactor = 16
StepsInGait = 18
NomGaitSpeed = 100
END IF

'return
End sub
'--------------------------------------------------------------------
'[GAIT Sequence]
Public Sub GaitSeq()
'Calculate Gait sequence
LastLeg = False
Call Gait (LRGaitLegNr, LRGaitPosX, LRGaitPosY, LRGaitPosZ, LRGaitRotY)
LRGaitPosX = GaitPosX
LRGaitPosY = GaitPosY
LRGaitPosZ = GaitPosZ
LRGaitRotY = GaitRotY

Call Gait (RFGaitLegNr, RFGaitPosX, RFGaitPosY, RFGaitPosZ, RFGaitRotY)
RFGaitPosX = GaitPosX
RFGaitPosY = GaitPosY
RFGaitPosZ = GaitPosZ
RFGaitRotY = GaitRotY

Call Gait (LMGaitLegNr, LMGaitPosX, LMGaitPosY, LMGaitPosZ, LMGaitRotY)
LMGaitPosX = GaitPosX
LMGaitPosY = GaitPosY
LMGaitPosZ = GaitPosZ
LMGaitRotY = GaitRotY

Call Gait (RRGaitLegNr, RRGaitPosX, RRGaitPosY, RRGaitPosZ, RRGaitRotY)
RRGaitPosX = GaitPosX
RRGaitPosY = GaitPosY
RRGaitPosZ = GaitPosZ
RRGaitRotY = GaitRotY

Call Gait (LFGaitLegNr, LFGaitPosX, LFGaitPosY, LFGaitPosZ, LFGaitRotY)
LFGaitPosX = GaitPosX
LFGaitPosY = GaitPosY
LFGaitPosZ = GaitPosZ
LFGaitRotY = GaitRotY

LastLeg = True
Call Gait (RMGaitLegNr, RMGaitPosX, RMGaitPosY, RMGaitPosZ, RMGaitRotY)
RMGaitPosX = GaitPosX
RMGaitPosY = GaitPosY
RMGaitPosZ = GaitPosZ
RMGaitRotY = GaitRotY
'return
End Sub
'--------------------------------------------------------------------
'[GAIT]

Public Sub Gait(Byval GaitLegNr As Byte, Byval GaitPosX As Integer, _
Byval GaitPosY As Integer, Byval GaitPosZ As Integer, _
Byval GaitRotY As Integer)

'Check IF the Gait is in motion
GaitInMotion = ((ABS(TravelLengthX)>CInt(TravelDeadZone)) or _
(ABS(TravelLengthZ)>CInt(TravelDeadZone)) or _
(ABS(TravelRotationY)>Cint(TravelDeadZone)))

'Leg middle up position
'Gait in motion Gait NOT in motion, return to home position
IF (GaitInMotion and (NrLiftedPos=1)) or _
((NrLiftedPos=3) and (GaitStep=GaitLegNr)) or _
((GaitInMotion=FALSE) and (GaitStep=GaitLegNr)) and _
(ABS(GaitPosX)>2) or (ABS(GaitPosZ)>2) or (ABS(GaitRotY)>2) THEN 'Up
GaitPosX = 0
GaitPosY = CInt(-LegLiftHeight)
GaitPosZ = 0
GaitRotY = 0
ELSE

'Optional Half heigth Rear
IF ((NrLiftedPos=2) and (GaitStep=GaitLegNr)) or _
((NrLiftedPos=3) and (GaitStep=GaitLegNr-1)) or _
((GaitStep=GaitLegNr+StepsInGait-1) and GaitInMotion) THEN
GaitPosX = -TravelLengthX2
GaitPosY = CInt(-LegLiftHeight)(CInt(HalfLiftHeigth)+1)
GaitPosZ = -TravelLengthZ2
GaitRotY = -TravelRotationY2
ELSE

'Optional half heigth front
IF ((NrLiftedPos>=2) And (GaitStep=GaitLegNr+1)) or _
(GaitStep=GaitLegNr-(StepsInGait-1)) And GaitInMotion THEN
GaitPosX = TravelLengthX2
GaitPosY = CInt(-LegLiftHeight)(CInt(HalfLiftHeigth)+1)
GaitPosZ = TravelLengthZ2
GaitRotY = TravelRotationY2
ELSE

'Leg front down position
IF ((GaitStep=GaitLegNr+NrLiftedPos) or _
(GaitStep=GaitLegNr-(StepsInGait-NrLiftedPos))) And _
(GaitPosY<0) THEN
GaitPosX = TravelLengthX2
GaitPosY = 0
GaitPosZ = TravelLengthZ2
GaitRotY = TravelRotationY2

'Move body forward
ELSE
GaitPosX = GaitPosX - (TravelLengthXCInt(TLDivFactor))
GaitPosY = 0
GaitPosZ = GaitPosZ - (TravelLengthZCInt(TLDivFactor))
GaitRotY = GaitRotY - (TravelRotationYCInt(TLDivFactor))
END IF
END IF
END IF
END IF

'Advance to the next step
IF LastLeg THEN 'The last leg in this step
GaitStep = GaitStep+1
IF GaitStep>StepsInGait THEN
GaitStep = 1
END IF
END IF

'return
End sub
'--------------------------------------------------------------------
'[BalCalcOneLeg]
Public Sub BalCalcOneLeg(Byval PosX As Integer, Byval PosZ As Integer, _
Byval PosY As Integer, Byval BodyOffsetX As Integer, _
Byval BodyOffsetZ As Integer)
'Calculating totals from center of the body to the feet
TotalZ = BodyOffsetZ+PosZ
TotalX = BodyOffsetX+PosX
TotalY = 150 + PosY' using the value 150 to lower the centerpoint of rotation 'BodyPosY +
TotalTransY = TotalTransY + PosY
TotalTransZ = TotalTransZ + TotalZ
TotalTransX = TotalTransX + TotalX
Call GetBoogTan (TotalX, TotalZ)
TotalYbal = TotalYbal + CInt((BoogTan*180.0) / 3.141592)
Call GetBoogTan (TotalX, TotalY)
TotalZbal = TotalZbal + CInt((BoogTan*180.0) / 3.141592)
Call GetBoogTan (TotalZ, TotalY)
TotalXbal = TotalXbal + CInt((BoogTan*180.0) / 3.141592)
'serout S_OUT, i9600, ["BalOneLeg PosX=", sdec PosX," PosZ=", sdec PosZ," TotalXTransZ=", sdec TotalTransZ, 13]
END SUB
'-----------------------------------------------------------------------------
'[BalanceBody]
Public Sub BalanceBody()
TotalTransZ = TotalTransZ6
TotalTransX = TotalTransX6
TotalTransY = TotalTransY6
if TotalYbal < -180 then 'Tangens fix caused by +/- 180 deg
TotalYbal = TotalYbal + 360
end if
if TotalZbal < -180 then 'Tangens fix caused by +/- 180 deg
TotalZbal = TotalZbal + 360
end if
if TotalXbal < -180 then 'Tangens fix caused by +/- 180 deg
TotalXbal = TotalXbal + 360
end if

'Balance rotation
TotalYBal = TotalYbal6
TotalXBal = TotalXbal6
TotalZBal = -TotalZbal6

'Balance translation
LFGaitPosZ = LFGaitPosZ - TotalTransZ
LMGaitPosZ = LMGaitPosZ - TotalTransZ
LRGaitPosZ = LRGaitPosZ - TotalTransZ
RFGaitPosZ = RFGaitPosZ - TotalTransZ
RMGaitPosZ = RMGaitPosZ - TotalTransZ
RRGaitPosZ = RRGaitPosZ - TotalTransZ

LFGaitPosX = LFGaitPosX - TotalTransX
LMGaitPosX = LMGaitPosX - TotalTransX
LRGaitPosX = LRGaitPosX - TotalTransX
RFGaitPosX = RFGaitPosX - TotalTransX
RMGaitPosX = RMGaitPosX - TotalTransX
RRGaitPosX = RRGaitPosX - TotalTransX

LFGaitPosY = LFGaitPosY - TotalTransY
LMGaitPosY = LMGaitPosY - TotalTransY
LRGaitPosY = LRGaitPosY - TotalTransY
RFGaitPosY = RFGaitPosY - TotalTransY
RMGaitPosY = RMGaitPosY - TotalTransY
RRGaitPosY = RRGaitPosY - TotalTransY
END SUB

'[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
Public Sub GetSinCos(Byval AngleDeg As Single)

'Get the absolute value of AngleDeg
IF AngleDeg < 0.0 THEN
ABSAngleDeg = AngleDeg*(-1.0)
ELSE
ABSAngleDeg = AngleDeg
END IF

'Shift rotation to a full circle of 360 deg -> AngleDeg // 360
IF AngleDeg < 0.0 THEN 'Negative values
AngleDeg = 360.0-(ABSAngleDeg-CSng(360*(CInt(ABSAngleDeg/360.0))))
ELSE 'Positive values
AngleDeg = ABSAngleDeg-CSng(360*(CInt(ABSAngleDeg/360.0)))
END IF

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 = COS(AngleRad) 'Sin o to 180 deg = cos(Angle Rad - 90deg)
CosA = -SIN(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 = -COS(AngleRad) 'Sin 180 to 360 deg = -cos(Angle Rad - 270deg)
CosA = SIN(AngleRad) 'Cos 180 to 360 deg = sin(Angle Rad - 270deg)
END IF
'return
End sub
'--------------------------------------------------------------------
'[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)
Public Sub GetBoogTan(Byval BoogTanX As Integer, Byval BoogTanY As Integer)
IF(BoogTanX = 0) THEN ' X=0 -> 0 or PI
IF(BoogTanY >= 0) THEN
BoogTan = 0.0
ELSE
BoogTan = 3.141592
END IF
ELSE

IF(BoogTanY = 0) THEN ' Y=0 -> +/- Pi/2
IF(BoogTanX > 0) THEN
BoogTan = 3.141592 / 2.0
ELSE
BoogTan = -3.141592 / 2.0
END IF
ELSE

IF(BoogTanY > 0) THEN 'BOOGTAN(X/Y)
BoogTan = ATN(CSng(BoogTanX) / CSng(BoogTanY))
ELSE
IF(BoogTanX > 0) THEN 'BOOGTAN(X/Y) + PI
BoogTan = ATN(CSng(BoogTanX) / CSng(BoogTanY)) + 3.141592
ELSE 'BOOGTAN(X/Y) - PI
BoogTan = ATN(CSng(BoogTanX) / CSng(BoogTanY)) - 3.141592
END IF
END IF
END IF
END IF
END SUB
'--------------------------------------------------------------------
'[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

Public Sub BodyIk(Byval Posx As Integer, Byval PosZ As Integer, _
Byval PosY As Integer, Byval BodyOffsetX As Integer, _
Byval BodyOffsetZ As Integer, Byval RotationY As Integer)

'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:
Call GetSinCos (CSng(BodyRotX+TotalXBal))
SinG = SinA
CosG = CosA
Call GetSinCos (CSng(BodyRotZ+TotalZBal))
SinB = SinA
CosB = CosA
Call GetSinCos (CSng(BodyRotY+RotationY+TotalYBal))

'Calcualtion of rotation matrix:
BodyIKPosX =(TotalX-CInt(CSng(TotalX)*CosA*CosB - _
CSng(TotalZ)*CosB*SinA + CSng(PosY)*SinB))
BodyIKPosZ =(TotalZ-CInt(CSng(TotalX)*CosG*SinA + _
CSng(TotalX)*CosA*SinB*SinG + CSng(TotalZ)*CosA*CosG - _
CSng(TotalZ)*SinA*SinB*SinG - CSng(PosY)*CosB*SinG))
BodyIKPosY =(PosY - CInt(CSng(TotalX)*SinA*SinG - _
CSng(TotalX)*CosA*CosG*SinB + Csng(TotalZ)*CosA*SinG + _
CSng(TotalZ)*CosG*SinA*SinB + CSng(PosY)*CosB*CosG))

End Sub '-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
Public Sub LegIK(Byval IKFeetPosX As Integer, Byval IKFeetPosY As Integer, _
Byval IKFeetPosZ As Integer)

'Length between the Coxa and Feet
IKFeetPosXZ = CInt(Sqr(CSng(IKFeetPosX*IKFeetPosX + IKFeetPosZ*IKFeetPosZ)))
'IKSW - Length between shoulder and wrist
IKSW = Sqr(CSng((IKFeetPosXZ-CInt(CoxaLength)*IKFeetPosXZ-CInt(CoxaLength)) + IKFeetPosY*IKFeetPosY))

'IKA1 - Angle between SW line and the ground in rad
Call GetBoogTan (IKFeetPosXZ-CInt(CoxaLength), IKFeetPosY)
IKA1 = BoogTan

'IKA2 - ?
IKA2 = ACOS((CSng((FemurLength*FemurLength) - (TibiaLength*TibiaLength)) + _
(IKSW*IKSW)) / (CSng(2*Femurlength) * IKSW))

'IKFemurAngle
IKFemurAngle = (CInt(((IKA1 + IKA2)*180.0) / 3.141592)*(-1)) + 90

'IKTibiaAngle
IKTibiaAngle = (90-CInt(((ACOS((CSng((FemurLength*FemurLength) + (TibiaLength*TibiaLength)) - _
(IKSW*IKSW)) / CSng(2*Femurlength*TibiaLength)))*180.0) / 3.141592)) * (-1)

'IKCoxaAngle
Call GetBoogTan (IKFeetPosZ, IKFeetPosX)
IKCoxaAngle = CInt(CSng((BoogTan*180.0) / 3.141592))

'Set the Solution quality
IF(IKSW < CSng(FemurLength+TibiaLength-30)) THEN
IKSolution = True
ELSE
IF(IKSW < CSng(FemurLength+TibiaLength)) THEN
IKSolutionWarning = True
ELSE
IKSolutionError = True
END IF
END IF

End Sub '-return
'--------------------------------------------------------------------
'[CHECK ANGLES] Checks the mechanical limits of the servos
Public Sub CheckAngles()

IF RFCoxaAngle > RFCoxa_MAX Then
RFCoxaAngle = RFCoxa_MAX
END IF
IF RFCoxaAngle < RFCoxa_MIN Then
RFCoxaAngle = RFCoxa_MIN
END IF
IF RFFemurAngle > RFFemur_MAX THEN
RFFemurAngle = RFFemur_MAX
END IF
IF RRFemurAngle < RFFemur_MIN THEN
RRFemurAngle = RRFemur_MIN
END IF
IF RFTibiaAngle > RFTibia_MAX THEN
RFTibiaAngle = RFTibia_MAX
END IF
IF RFTibiaAngle < RFTibia_MIN THEN
RFTibiaAngle = RFTibia_MIN
END IF

IF RMCoxaAngle > RMCoxa_Max THEN
RMCoxaAngle = RMCoxa_Max
END IF
IF RMCoxaAngle < RMCoxa_MIN THEN
RMCoxaAngle = RMCoxa_MIN
END IF
IF RMFemurAngle > RMFemur_MAX THEN
RMFemurAngle = RMFemur_MAX
END IF
IF RMFemurAngle < RMFemur_MIN THEN
RMFemurAngle = RMFemur_MIN
END IF
IF RMTibiaAngle > RMTibia_MAX THEN
RMTibiaAngle = RMFemur_MAX
END IF
IF RMTibiaAngle < RMTibia_MIN THEN
RMTibiaAngle = RMTibia_MIN
END IF

IF RRCoxaAngle > RRCoxa_MAX THEN
RRCoxaAngle = RRCoxa_MAX
END IF
IF RRCoxaAngle < RRCoxa_MIN THEN
RRCoxaAngle = RRCoxa_MIN
END IF
IF RRFemurAngle > RRFemur_MAX THEN
RRFemurAngle = RRFemur_MAX
END IF
IF RRFemurAngle < RRFemur_MIN THEN
RRFemurAngle = RRFemur_MIN
END IF
IF RRTibiaAngle < RRTibia_MIN THEN
RRTibiaAngle = RRTibia_MIN
END IF
IF RRTibiaAngle > RRTibia_MAX THEN
RRTibiaAngle = RRTibia_MAX
END IF

IF LFCoxaAngle > LFCoxa_MAX THEN
LFCoxaAngle = LFCoxa_MAX
END IF
IF LFCoxaAngle < LFCoxa_MIN THEN
LFCoxaAngle = LFCoxa_MIN
END IF
IF LFFemurAngle > LFFemur_MAX THEN
LFFemurAngle = LFFemur_MAX
END IF
IF LFFemurAngle < LFFemur_MIN THEN
LFFemurAngle = LFFemur_MIN
END IF
IF LFTibiaAngle > LFTibia_MAX THEN
LFTibiaAngle = LFTibia_MAX
END IF
IF LFTibiaAngle < LFTibia_MIN THEN
LFTibiaAngle = LFTibia_MIN
END IF

IF LMCoxaAngle > LMCoxa_MAX THEN
LMCoxaAngle = LMCoxa_MAX
END IF
IF LMCoxaAngle < LMCoxa_MIN THEN
LMCoxaAngle = LMCoxa_MIN
END IF
IF LMFemurAngle > LMFemur_MAX THEN
LMFemurAngle = LMFemur_MAX
END IF
IF LMFemurAngle < LMFemur_MIN THEN
LMFemurAngle = LMFemur_MIN
END IF
IF LMTibiaAngle > LMTibia_MAX THEN
LMTibiaAngle = LMTibia_MAX
END IF
IF LMTibiaAngle < LMTibia_MIN THEN
LMTibiaAngle = LMTibia_MIN
END IF

IF LRCoxaAngle > LRCoxa_MAX THEN
LRCoxaAngle = LRCoxa_MAX
END IF
IF LRCoxaAngle < LRCoxa_MIN THEN
LRCoxaAngle = LRCoxa_MIN
END IF
IF LRFemurAngle > LRFemur_MAX THEN
LRFemurAngle = LRFemur_MAX
END IF
IF LRFemurAngle < LRFemur_MIN THEN
LRFemurAngle = LRFemur_MIN
END IF
IF LRTibiaAngle > LRTibia_MAX THEN
LRTibiaAngle = LRTibia_MAX
END IF
IF LRTibiaAngle < LRTibia_MIN THEN
LRTibiaAngle = LRTibia_MIN
END IF

End Sub
'--------------------------------------------------------------------
'[SERVO DRIVER] Updates the positions of the servos
Public Sub ServoDriver()

Dim OCom(1 to 30) As Byte
Dim ICom(1 to 10) As Byte
Dim Move As String * 20

Call OpenQueue(OCom, 30)
Call OpenQueue(ICom, 10)
Call DefineCom3(0, 16, bx0000_1000) 'serial port, inverted, 8 bits. no paridad
Call OpenCom(3, 9600, ICom, OCom)
Call Delay(0.1)

Move = "#" & Cstr(RFCoxaP) & "P" & Cstr(CInt(CSng(-RFCoxaAngle +90)/0.10588238)+650)
Call PutQueueStr(Ocom, Move)
Move = "#" & CStr(RFFemurP) & "P" & CStr(CInt(CSng(-RFFemurAngle+90)/0.10588238)+650)
Call PutQueueStr(Ocom, Move)
Move = "#" & CStr(RFTibiaP) & "P" & CStr(CInt(CSng(-RFTibiaAngle+90)/0.10588238)+650)
Call PutQueueStr(Ocom, Move)
Move = "#" & CStr(RMCoxaP) & "P" & CStr(CInt(CSng(-RMCoxaAngle +90)/0.10588238)+650)
Call PutQueueStr(Ocom, Move)
Move = "#" & CStr(RMFemurP) & "P" & CStr(CInt(CSng(-RMFemurAngle+90)/0.10588238)+650)
Call PutQueueStr(Ocom, Move)
Move = "#" & CStr(RMTibiaP) & "P" & CStr(CInt(CSng(-RMTibiaAngle+90)/0.10588238)+650)
Call PutQueueStr(Ocom, Move)
Move = "#" & CStr(RRCoxaP) & "P" & CStr(CInt(Csng(-RRCoxaAngle +90)/0.10588238)+650)
Call PutQueueStr(Ocom, Move)
Move = "#" & CStr(RRFemurP) & "P" & CStr(CInt(CSng(-RRFemurAngle+90)/0.10588238)+650)
Call PutQueueStr(Ocom, Move)
Move = "#" & CStr(RRTibiaP) & "P" & CStr(Cint(Csng(-RRTibiaAngle+90)/0.10588238)+650)
Call PutQueueStr(Ocom, Move)
Move = "#" & CStr(LFCoxaP) & "P" & CStr(CInt(CSng(LFCoxaAngle +90)/0.10588238)+650)
Call PutQueueStr(Ocom, Move)
Move = "#" & CStr(LFFemurP) & "P" & CStr(CInt(CSng(LFFemurAngle+90)/0.10588238)+650)
Call PutQueueStr(Ocom, Move)
Move = "#" & CStr(LFTibiaP) & "P" & CStr(CInt(CSng(LFTibiaAngle+90)/0.10588238)+650)
Call PutQueueStr(Ocom, Move)
Move = "#" & CStr(LMCoxaP) & "P" & CStr(CInt(CSng(LMCoxaAngle +90)/0.10588238)+650)
Call PutQueueStr(Ocom, Move)
Move = "#" & CStr(LMFemurP) & "P" & CStr(CInt(CSng(LMFemurAngle+90)/0.10588238)+650)
Call PutQueueStr(Ocom, Move)
Move = "#" & CStr(LMTibiaP) &"P" & CStr(CInt(CSng(LMTibiaAngle+90)/0.10588238)+650)
Call PutQueueStr(Ocom, Move)
Move = "#" & CStr(LRCoxaP) & "P" & CStr(CInt(CSng(LRCoxaAngle +90)/0.10588238)+650)
Call PutQueueStr(Ocom, Move)
Move = "#" & CStr(LRFemurP) & "P" & CStr(CInt(CSng(LRFemurAngle+90)/0.10588238)+650)
Call PutQueueStr(Ocom, Move)
Move = "#" & CStr(LRTibiaP) & "P" & CStr(CInt(CSng(LRTibiaAngle+90)/0.10588238)+650)
Call PutQueueStr(Ocom, Move)
Move = "T" & CStr(SSCTime) & Chr(13)
PrevSSCTime = SSCTime
Call PutQueueStr(OCom, Move)

Call Delay (0.1)

End Sub
'--------------------------------------------------------------------
'[FREE SERVOS] Frees all the servos
Public Sub FreeServos()

Dim OCom(1 to 30) As Byte
Dim ICom(1 to 10) As Byte
Dim Move As String * 20
Dim Index As Byte

Call OpenQueue(OCom, 30)
Call OpenQueue(ICom, 10)
Call DefineCom3(0, 16, bx0000_1000) 'serial port, inverted, 8 bits. no paridad
Call OpenCom(3, 9600, ICom, OCom)

Call Delay(0.1)

for Index = 0 to 31
Move = "#" & CStr(Index) & "P0"
Call PutQueueStr(Ocom, Move)
next
Move = "T200" & Chr(13)

Call PutQueueStr(OCom, Move)
Call Delay (0.1)

End sub

Buenas, creo que nadie va a poder ayudarte así... Has puesto un código excesivamente largo como para que alguien se pare a leerlo y ni tan si quiera dices qué es lo que falla... Así no se pide ayuda amigo.

Un saludo.