Introducing the STALKER! (project ended)

[zenta]

@innerbreed

the coxa servo shafts are tilted down away from the body by about 43° as oppose to say 90° stright up… i can take a photo and add a diagram to the picture to show this??

I think I got it, but a photo would also be nice.

BTW, is it possible to adjust it to exactly 45° ?

[innerbreed]

@zenta

@innerbreed

the coxa servo shafts are tilted down away from the body by about 43° as oppose to say 90° stright up… i can take a photo and add a diagram to the picture to show this??

I think I got it, but a photo would also be nice.

BTW, is it possible to adjust it to exactly 45° ?

ok i have taken an exact measurement of the angle i think you need?
picture showing 50° angle! so the angle isnt as tilted as first thought.
i cannot get an exact 45° as the upper body is restricting it!
i hope this picture helps.
http://i531.photobucket.com/albums/dd355/innerbreed/100_0948-1.jpg

I guess you would be telling the program to have each leg IK working as if you are using body IK (rotate, roll, pitch, etc), kind of like walking in constant Roll pos but implimenting these values into each side/leg! i havent explained it right but i do know what you mean.

thanks again Zenta.

[zenta]

Hi,

The picture was at great help, its the 40 deg value we are going to use.

Have you got all your parts/servos yet?

Exactly what have you done with the code so far?
We might need to start from scratch at first by altering as little as possible of the code.

1. Remove the code that involve the middle legs.
2. Calibrate the coxa, femur and tibia servos as if the coxa where mounted in a horisontal position (like we do for the Phoenix).
3. Enter the body and leg dimensions into the code, finding the correct body dimension (X and Z values) might be a bit tricky. The correct coordinate is a common centering point for the coxa servo when its horisontal and tilted. Enter the CoxaAngle const = 0 deg (You might wonder why we don’t use 45 deg, but the the rotation kinematics I mentioned are going to take care of this).
4. Make sure that the code work OK by control without doing more changes, at this point the IK won’t be correct.
5. Now the hard part start . Let me know when you are here

There are going to be a lot of trial and error before we get there I think.

I hope you are keeping the weight down since (as mentioned before) the coxa servos are going to get heavily loaded when its out of its centering position.

[innerbreed]

@zenta

Hi,

The picture was at great help, its the 40 deg value we are going to use.

Have you got all your parts/servos yet?

Exactly what have you done with the code so far?
We might need to start from scratch at first by altering as little as possible of the code.

I HAVE ALREADY DONE MOST OF THIS BUT AS I DONT HAVE ALL THE
SERVOS (MISSING 7)
missing:
4 Tibia
3 Femur

Edited:
**To keep the weight down i am not adding the top section of the body
i will also be adding springs to help with the torque and instead if 1mm body pannels i have .5mm. every little helps. even drilling out parts of the brackets! **

Here is what we have: might be best to C&P into IDE to view.

[code] ;*************************************************
;=BotBoard II with Basic ATOM Pro 28, SSC-32 V2=
;================& PS2 DualShock================
;=================S.T.A.L.K.E.R================**
;=Surveillant Taciturn Articulated Locomotory Kinematical Enhanced Robot=
;**************************************************************************
; http://www.lynxmotion.net/viewtopic.php?t=3601

;Description: Stalker Robot With Symmetrical Design
;Software version: V1.0
;Programmer/: Jonny Poole (aka Innerbreed)
;
;With Credit & Thanks to…
;Jeroen Janssen (aka Xan) for programming structure phoen1.3.bas (Black Widow)
;KÃ¥re Halvorsen (aka Zenta) for Balance Calculation (Phoenix)

'==========================================================================
’ Quadruped Stalker Overview (Fig:1)
'--------------------
’ ±± <--------------------Gun Cannon} (rotates to follow Rotate direction, with Left Stick)
’ /^^
’ /-\ /-\
’ (]-]_ / /]-]) <---------------Gun Turrets} (up & down with Body, with D-pad)
’ ||
’ /]\ <------------------Upper Body (see Fig:2b)
’ ]_/] } (rotates to follow Walk/Strafe direction, with Right Stick)
’ / // \ \ <-------Lower Body
’ /]=====] ]=====]
’ // \
’ | // \ | <-----Legs (see Fig:2a)
’ // \/
’ \ /
’ \ /
’
'====================================================
’ Layout Diagram & X/Y/Z plane (Fig:2)
'---------------------- note the SSC is installed Upside-Down with Serial Port Facing the Front
'(a) Back
’ 22 06
’ 21 05 Y+ Z+
’ 20_____04 | /
’ R | | L | /
’ i | | e |/
’ g | | f 0—X
’ h |s| t |
’ t 24 08 |
’ 25 09 |
’ 26 10 Y- Z-
’ Front
’
’
’
'(b) Back
’
’ R /–\ L
’ i /--------\ e
’ g /##| ## |##\ f
’ h // \ / \ t
’ t ## __/ ##
’
’ Front
’
'====================================================
’ 3DOF Leg Diagram (Fig:3) showing Rear left as example
'----------------------
’ [0]
’ / //
’ /[2]=====[1] 0= Coxa servo
’ //
’ | // 1= Femur servo
’ //
’ \ 2= Tibia servo
’ \
’
'====================================================

;--------------------------------------------------------------------
;-----------------------------[CONTROL INTERFACE]
;Start Turn Quad on/off
;Select Change gaits
;Left Joystick Walk/Strafe
;Right Joystick Rotate/Translation
;D-Pad Up Body up 10mm
;D-Pad Down Body down 10mm
;Square Turns Balance mode on/off
;Triangle Unsigned
;Circle Unsigned
;Cross Unsigned

;Shoulder Button Functions (Hold):
;L1+Both Jsticks Left= shifts body along the X/Z axis; Right= shifts and rotates body along Y axis
;L2+Both Jsticks Shift body along X/Y/Z axis
;R2+Left Jstick Doubles gait travel length
;
;====================================================================
;-----------------------------[CONSTANTS]----------------------------

;-----------------------------[SERIAL CONNECTIONS]
SSC_OUTPUT con P11 ;Output pin for (SSC32 RX) on BotBoard (Yellow)
SSC_INPUT con P10 ;Input pin for (SSC32 TX) on BotBoard (Red)
SSC_BAUD con i38400 ;SSC32 Data Modulation Rate
;--------------------------------------------------------------------
;-----------------------------[PS2 DualShock 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
;--------------------------------------------------------------------
;-----------------------------[SSC PIN NUMBERS]
RRCoxaPin con P20 ;Rear Right leg Hip Horizontal
RRFemurPin con P21 ;Rear Right leg Hip Vertical
RRTibiaPin con P22 ;Rear Right leg Knee

RFCoxaPin con P24 ;Front Right leg Hip Horizontal
RFFemurPin con P25 ;Front Right leg Hip Vertical
RFTibiaPin con P26 ;Front Right leg Knee

LRCoxaPin con P4 ;Rear Left leg Hip Horizontal
LRFemurPin con P5 ;Rear Left leg Hip Vertical
LRTibiaPin con P6 ;Rear Left leg Knee

LFCoxaPin con P8 ;Front Left leg Hip Horizontal
LFFemurPin con P9 ;Front Left leg Hip Vertical
LFTibiaPin con P10 ;Front Left leg Knee

BodyPin con P19 ;Body rotation

;--------------------------------------------------------------------
;-----------------------------[MIN/MAX ANGLES in degrees]
;Take the servo Pulse, subtract 1500, divide by 750, and mutply by 90
;(180 degree ranged servos)
;e.g 2400-1500/750*90=108
RRCoxa_MIN con -40 ;Mechanical limits of the RIGHT REAR Leg
RRCoxa_MAX con 40

RRFemur_MIN con -75
RRFemur_MAX con 75

RRTibia_MIN con -45
RRTibia_MAX con 77
;-------------------
RFCoxa_MIN con -40 ;Mechanical limits of the RIGHT FRONT Leg
RFCoxa_MAX con 40

RFFemur_MIN con -75
RFFemur_MAX con 75

RFTibia_MIN con -45
RFTibia_MAX con 77
;-------------------
LRCoxa_MIN con -40 ;Mechanical limits of the LEFT REAR Leg
LRCoxa_MAX con 40

LRFemur_MIN con -75
LRFemur_MAX con 75

LRTibia_MIN con -77
LRTibia_MAX con 45
;-------------------
LFCoxa_MIN con -40 ;Mechanical limits of the LEFT FRONT Leg
LFCoxa_MAX con 40

LFFemur_MIN con -75
LFFemur_MAX con 75

LFTibia_MIN con -45
LFTibia_MAX con 45
;-------------------
Body_Min con -100 ;Mechanical limits of the BODY ROTATE
Body_Max con 100
;--------------------------------------------------------------------
;-----------------------------[BODY/LEG DIMENSIONS]
;Phoenix setup
;CoxaLength con 29 ;Length of the Coxa [mm]
;FemurLength con 76 ;Length of the Femur [mm]
;TibiaLength con 106 ;Lenght of the Tibia [mm]

;Stalker setup 124/106*100=# -100 =#%
CoxaLength con 55 ;Length of the Coxa [mm] 47.0% bigger
FemurLength con 96 ;Length of the Femur [mm] 26.3% bigger
TibiaLength con 140 ;Lenght of the Tibia [mm] 32.0% bigger
; These percentages show size difference to the Phoenix

CoxaAngle con 90 ;Default Coxa setup angle was 60

RFOffsetX con -60 ;Distance X from center of the body to the Right Front coxa
RFOffsetZ con -75 ;Distance Z from center of the body to the Right Front coxa

RROffsetX con -60 ;Distance X from center of the body to the Right Rear coxa
RROffsetZ con 75 ;Distance Z from center of the body to the Right Rear coxa

LFOffsetX con 60 ;Distance X from center of the body to the Left Front coxa
LFOffsetZ con -75 ;Distance Z from center of the body to the Left Front coxa

LROffsetX con 60 ;Distance X from center of the body to the Left Rear coxa
LROffsetZ con 75 ;Distance Z from center of the body to the Left Rear coxa
;--------------------------------------------------------------------
;-----------------------------[REMOTE]
TravelDeadZone con 4 ;The deadzone for the analog input from the remote

POSITIVE con 1 ;if the solution is NEARLY possible
NEGITIVE con 0 ;if the solution is NOT possible

BUTTON_RELEASE con 1
BUTTON_HOLD con 0

;====================================================================
;-----------------------------[VARIABLES]----------------------------

;-----------------------------[ANGLES]
RFCoxaAngle var sword ;Actual Angle of the Right Front Leg
RFFemurAngle var sword
RFTibiaAngle 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

LRCoxaAngle var sword ;Actual Angle of the Left Rear Leg
LRFemurAngle var sword
LRTibiaAngle var sword

BodyAngle var sword ;Actual Angle of the Body rotate
;--------------------------------------------------------------------
;-----------------------------[POSITIONS]
RFPosX var sword ;Actual Position of the Right Front Leg
RFPosY var sword
RFPosZ 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

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
;--------------------------------------------------------------------
Index var byte ;Index used for freeing the servos
SSCDone var byte ;To check if SSC cycle is done
;GetSinCos
AngleDeg var float ;Input Angle in degrees
ABSAngleDeg var float ;Absolute value of the Angle in Degrees
AngleRad var float ;Angle in Radian
sinA var float ;Output Sinus of the given Angle
cosA var float ;Output Cosinus of the given Angle
;GetBoogTan
BoogTanX var sword ;Input X
BoogTanY var sword ;Input Y
BoogTan var float ;Output BOOGTAN2(X/Y)
;Body position
BodyPosX var sbyte ;Global Input for the position of the body
BodyPosY var sword
BodyPosZ var sbyte

;-----------------------------[Body Inverse Kinematics]
BodyRotX var sbyte ;Global Input pitch of the body
BodyRotY var sbyte ;Global Input rotation of the body
BodyRotZ var sbyte ;Global Input roll of the body
PosX var sword ;Input position of the feet X
PosZ var sword ;Input position of the feet Z
PosY var sword ;Input position of the feet Y
RotationY var sbyte ;Input for rotation of a single feet for the gait
BodyOffsetX var sbyte ;Input Offset betweeen the body and Coxa X
BodyOffsetZ var sbyte ;Input Offset betweeen the body and Coxa Z
sinB var float ;Sin buffer for BodyRotX calculations
cosB var float ;Cos buffer for BodyRotX calculations
sinG var float ;Sin buffer for BodyRotZ calculations
cosG var float ;Cos buffer for BodyRotZ calculations
TotalX var sword ;Total X distance between the center of the body and the feet
TotalZ var sword ;Total Z distance between the center of the body and the feet
DistCenterBodyFeet var float ;Total distance between the center of the body and the feet
AngleCenterBodyFeetX var float ;Angle between the center of the body and the feet
BodyIKPosX var sword ;Output Position X of feet with Rotation
BodyIKPosY var sword ;Output Position Y of feet with Rotation
BodyIKPosZ var sword ;Output Position Z of feet with Rotation

;-----------------------------[Leg Inverse Kinematics]
IKFeetPosX var sword ;Input position of the Feet X
IKFeetPosY var sword ;Input position of the Feet Y
IKFeetPosZ var sword ;Input Position of the Feet Z
IKFeetPosXZ var sword ;Length between the coxa and feet
IKCF var float ;Length between coxa and femur
IKA1 var float ;Angle between coxa and femur line and the ground in rad
IKA2 var float ;Angle between femur and tibia line and the ground in rad
IKSolution var bit ;Output POSITIVE if the solution is possible
IKSolutionWarning var bit ;Output POSITIVE if the solution is NEARLY possible
IKSolutionError var bit ;Output POSITIVE if the solution is NOT possible
IKFemurAngle var sword ;Output Angle of Femur in degrees
IKTibiaAngle var sword ;Output Angle of Tibia in degrees
IKCoxaAngle var sword ;Output Angle of Coxa in degrees
;--------------------------------------------------------------------
;-----------------------------[PS2 DualShock Controller]
DualShock var Byte(7)
LastButton var Byte(2)
PS2Mode var Byte
PS2Index var byte
BodyYShift var sbyte
;--------------------------------------------------------------------
;-----------------------------[TIMING]
lTimerWOverflowCnt var long ;used in WTimer overflow. Will keep a 16 bit overflow so we have a 32 bit timer
lCurrentTime var long
lTimerStart var long ;Start time of the calculation cycles
lTimerEnd var long ;End time of the calculation cycles
CycleTime var byte ;Total Cycle time

SSCTime var word ;This changes the servos speed directly.
PrevSSCTime var word ;Previous time for the changes the servos speed.

InputTimeDelay var byte ;Delay that depends on the input to get the “sneaking” effect
;--------------------------------------------------------------------
;-----------------------------[GLOBAL]
QuadOn var bit ;Power up Stalker (Standby)
;--------------------------------------------------------------------
;-----------------------------[BALANCE]
BalanceMode var bit
TravelHeightY var sword
TotalTransX var sword
TotalTransZ var sword
TotalTransY var sword
TotalYbal var sword
TotalXBal var sword
TotalZBal var sword
TotalY var sword ;Total Y distance between the center of the body and the feet

;-----------------------------[GAIT]
GaitType var byte ;Gait type
TitularGSpeed var byte ;Supposed speed of the gait

LegLiftHeight var byte ;Current Travel height
TravelLengthX var sword ;Current Travel length X (Dualshock(5) Left joystick (Left - Right)
TravelLengthZ var sword ;Current Travel length Z (Dualshock(6) Left joystick (Up - Down)
TravelRotationY var sword ;Current Travel Rotation Y (Dualshock(3) Right joystick (Left - Right)

TLDivFactor var byte ;Number of steps that a leg is on the floor while walking
NrLiftedPos var nib ;Number of positions that a single leg is lifted (1-3)
HalfLiftHeigth var bit ;If POSITIVE the outer positions of the ligted legs will be half height

GaitInMotion var bit ;Temp to check if the gait is in motion
StepsInGait var byte ;Number of steps in gait
LastLeg var bit ;POSITIVE when the current leg is the last leg of the sequence
GaitStep var byte ;Actual Gait step

RFGaitLegNr var byte ;Init position of the leg
RRGaitLegNr var byte
LFGaitLegNr var byte
LRGaitLegNr var byte

GaitLegNr var byte ;Input Number of the leg
TravelMulti var sbyte ;Multiplier for the length of the step

RFGaitPosX var sbyte ;Relative position corresponding to the Gait
RFGaitPosY var sbyte
RFGaitPosZ var sbyte
RFGaitRotY var sbyte ;Relative rotation corresponding to the Gait

RRGaitPosX var sbyte ;Relative position corresponding to the Gait
RRGaitPosY var sbyte
RRGaitPosZ var sbyte
RRGaitRotY var sbyte ;Relative rotation corresponding to the Gait

LFGaitPosX var sbyte ;Relative position corresponding to the Gait
LFGaitPosY var sbyte
LFGaitPosZ var sbyte
LFGaitRotY var sbyte ;Relative rotation corresponding to the Gait

LRGaitPosX var sbyte ;Relative position corresponding to the Gait
LRGaitPosY var sbyte
LRGaitPosZ var sbyte
LRGaitRotY var sbyte ;Relative rotation corresponding to the Gait

GaitPosX var sbyte ;In-/Output Pos X of feet
GaitPosY var sword ;In-/Output Pos Y of feet
GaitPosZ var sbyte ;In-/Output Pos Z of feet
GaitRotY var sbyte ;In-/Output Rotation Y of feet
;====================================================================
;-----------------------------[INITIALISATION]
;Turning off all the leds
LedA = 0
LedB = 0
LedC = 0

;Feet Positions in mm
RFPosX = 53 ;125 ;Start positions of the Right Front leg
RFPosY = 25 ;25
RFPosZ = -91 ;-117

RRPosX = 53 ;125 ;Start positions of the Right Rear leg
RRPosY = 25 ;25
RRPosZ = 91 ;117

LFPosX = 53 ;125 ;Start positions of the Left Front leg
LFPosY = 25 ;25
LFPosZ = -91 ;-117

LRPosX = 53 ;125 ;Start positions of the Left Rear leg
LRPosY = 25 ;25
LRPosZ = 91 ;117

;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
GOSUB GaitSelect
;--------------------------------------------------------------------
;====================================================================
;-----------------------------[TIMER INTERRUPT INIT]
ONASMINTERRUPT TIMERWINT, Handle_TIMERW
;Timer
WTIMERTICSPERMS con 2000 ;we have 16 clocks per ms and we are incrementing every 8 so divide again by 2
TCRW = 0x30 ;clears TCNT and sets the timer to inc clock cycle / 8
TMRW = 0x80 ;starts the timer counting
lTimerWOverflowCnt = 0
enable TIMERWINT_OVF
;--------------------------------------------------------------------
;PS2 DualShock controller
high PS2CLK
LastButton(0) = 255
LastButton(1) = 255
BodyYShift = 0
;--------------------------------------------------------------------
;SSC
SSCTime = 150 ;0,15sec
QuadOn = NEGITIVE
;====================================================================
;-----------------------------[MAIN]
main:
;Start time
GOSUB GetCurrentTime], lTimerStart

;Reset IKsolution indicators
IKSolution = NEGITIVE
IKSolutionWarning = NEGITIVE
IKSolutionError = NEGITIVE

;Gait
GOSUB 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
gosub BalCalcOneLeg -RFPosX+BodyPosX+RFGaitPosX, RFPosZ+BodyPosZ+RFGaitPosZ,RFGaitPosY, RFOffsetX, RFOffsetZ]
gosub BalCalcOneLeg -RRPosX+BodyPosX+RRGaitPosX, RRPosZ+BodyPosZ+RRGaitPosZ,RRGaitPosY, RROffsetX, RROffsetZ]

gosub BalCalcOneLeg [LFPosX-BodyPosX+LFGaitPosX, LFPosZ+BodyPosZ+LFGaitPosZ,LFGaitPosY, LFOffsetX, LFOffsetZ]
gosub BalCalcOneLeg [LRPosX-BodyPosX+LRGaitPosX, LRPosZ+BodyPosZ+LRGaitPosZ,LRGaitPosY, LROffsetX, LROffsetZ]
gosub BalanceBody

ENDIF

;Reset IKsolution indicators
IKSolution = NEGITIVE
IKSolutionWarning = NEGITIVE
IKSolutionError = NEGITIVE

;Right Front leg
GOSUB BodyIK -RFPosX+BodyPosX+RFGaitPosX, RFPosZ+BodyPosZ+RFGaitPosZ,RFPosY+BodyPosY+BodyYShift+RFGaitPosY, RFOffsetX, RFOffsetZ, RFGaitRotY]
GOSUB LegIK [RFPosX-BodyPosX+BodyIKPosX-RFGaitPosX, RFPosY+BodyPosY+BodyYShift-BodyIKPosY+RFGaitPosY, RFPosZ+BodyPosZ-BodyIKPosZ+RFGaitPosZ]
RFCoxaAngle = IKCoxaAngle + CoxaAngle ;Angle for the basic setup for the front leg right
RFFemurAngle = IKFemurAngle
RFTibiaAngle = IKTibiaAngle

;Right Rear leg
GOSUB BodyIK -RRPosX+BodyPosX+RRGaitPosX, RRPosZ+BodyPosZ+RRGaitPosZ,RRPosY+BodyPosY+BodyYShift+RRGaitPosY, RROffsetX, RROffsetZ, RRGaitRotY]
GOSUB LegIK [RRPosX-BodyPosX+BodyIKPosX-RRGaitPosX, RRPosY+BodyPosY+BodyYShift-BodyIKPosY+RRGaitPosY, RRPosZ+BodyPosZ-BodyIKPosZ+RRGaitPosZ]
RRCoxaAngle = IKCoxaAngle - CoxaAngle ;Angle for the basic setup for the Rear leg right
RRFemurAngle = IKFemurAngle
RRTibiaAngle = IKTibiaAngle

;Left Front leg
GOSUB BodyIK [LFPosX-BodyPosX+LFGaitPosX, LFPosZ+BodyPosZ+LFGaitPosZ,LFPosY+BodyPosY+BodyYShift+LFGaitPosY, LFOffsetX, LFOffsetZ, LFGaitRotY]
GOSUB LegIK [LFPosX+BodyPosX-BodyIKPosX+LFGaitPosX, LFPosY+BodyPosY+BodyYShift-BodyIKPosY+LFGaitPosY, LFPosZ+BodyPosZ-BodyIKPosZ+LFGaitPosZ]
LFCoxaAngle = IKCoxaAngle + CoxaAngle ;Angle for the basic setup for the front leg left
LFFemurAngle = IKFemurAngle
LFTibiaAngle = IKTibiaAngle

;Left Rear leg
GOSUB BodyIK [LRPosX-BodyPosX+LRGaitPosX, LRPosZ+BodyPosZ+LRGaitPosZ,LRPosY+BodyPosY+BodyYShift+LRGaitPosY, LROffsetX, LROffsetZ, LRGaitRotY]
GOSUB LegIK [LRPosX+BodyPosX-BodyIKPosX+LRGaitPosX, LRPosY+BodyPosY+BodyYShift-BodyIKPosY+LRGaitPosY, LRPosZ+BodyPosZ-BodyIKPosZ+LRGaitPosZ]
LRCoxaAngle = IKCoxaAngle - CoxaAngle ;Angle for the basic setup for the Rear leg left
LRFemurAngle = IKFemurAngle
LRTibiaAngle = IKTibiaAngle

GOSUB CheckAngles

LedC = IKSolutionWarning
LedA = IKSolutionError

;Read input
GOSUB Ps2Input
;GOSUB ReadButtons ;I/O used by the PS2 remote
;GOSUB WriteLeds ;I/O used by the PS2 remote

;Get endtime and calculate wait time
GOSUB GetCurrentTime], lTimerEnd
CycleTime = (lTimerEnd-lTimerStart)/WTIMERTICSPERMS

IF(QuadOn)THEN
;Wait for previous commands to be completed while walking
IF(ABS(TravelLengthX)>TravelDeadZone | ABS(TravelLengthZ)>TravelDeadZone | ABS(TravelRotationY*2)>TravelDeadZone) THEN

  pause (PrevSSCTime - CycleTime -50) MIN 1    ;Min 1 ensures that there alway is a value in the pause command
  
  IF(BalanceMode=0)THEN
  	SSCTime = TitularGSpeed + (InputTimeDelay*2)
  ELSE
  	SSCTime = TitularGSpeed + (InputTimeDelay*2) + 100
  ENDIF
  
ELSE
  SSCTime = 200 ;TitularGSpeed 0,20 sec
ENDIF
	
GOSUB ServoDriver

ELSE
;Turn on standby
GOSUB FreeServos
ENDIF
goto main
;====================================================================
;[ReadButtons] Reading input buttons from the ABB
ReadButtons:
input P4
input P5
input P6

prev_butA = butA
prev_butB = butB
prev_butC = butC

butA = IN4
butB = IN5
butC = IN6
return
;--------------------------------------------------------------------
;[WriteLEDs] Updates the state of the leds
WriteLEDs:
if ledA = 1 THEN
low p4
ENDIF
if ledB = 1 THEN
low p5
ENDIF
if ledC = 1 THEN
low p6
ENDIF
return
;-------------------------------------------------------------------------------------------------------------------
;-----------------------------[PS2 INPUT]

Ps2Input:

low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,$1\8]
shiftin PS2DAT,PS2CLK,FASTLSBPOST,[PS2Mode\8]
high PS2SEL
pause 1

low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,$1\8,$42\8]
shiftin PS2DAT,PS2CLK,FASTLSBPOST,[DualShock(0)\8, DualShock(1)\8, DualShock(2)\8, DualShock(3)\8, |
DualShock(4)\8, DualShock(5)\8, DualShock(6)\8]
high PS2SEL
pause 1

if PS2Mode <> PadMode THEN
low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,$1\8,$43\8,$0\8,$1\8,$0\8] ;CONFIG_MODE_ENTER
high PS2SEL
pause 1

low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,$01\8,$44\8,$00\8,$01\8,$03\8,$00\8,$00\8,$00\8,$00\8] ;SET_MODE_AND_LOCK
high PS2SEL
pause 1

low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,$01\8,$4F\8,$00\8,$FF\8,$FF\8,$03\8,$00\8,$00\8,$00\8] ;SET_DS2_NATIVE_MODE
high PS2SEL
pause 1

low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,$01\8,$43\8,$00\8,$00\8,$5A\8,$5A\8,$5A\8,$5A\8,$5A\8] ;CONFIG_MODE_EXIT_PS2_NATIVE
high PS2SEL
pause 1

low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,$01\8,$43\8,$00\8,$00\8,$00\8,$00\8,$00\8,$00\8,$00\8] ;CONFIG_MODE_EXIT
high PS2SEL
pause 100
	
sound P9,[100\4000, 100\4500, 100\5000]
return

ENDIF
;-----------------------------[DUALSHOCK CONTROL INTERFACES]
IF (DualShock(1).bit3 = 0) and LastButton(0).bit3 THEN ;Start Button test
IF(QuadOn) THEN
'Turn off
Sound P9,[100\500,80\4500,100\40000]
BodyPosX = 0
BodyPosY = 0
BodyPosZ = 0
BodyRotX = 0
BodyRotY = 0
BodyRotZ = 0
TravelLengthX = 0
TravelLengthZ = 0
TravelRotationY = 0

  SSCTime = 600 ;0,60 sec
  GOSUB ServoDriver
  QuadOn = NEGITIVE
ELSE
  'Turn on
  Sound P9,[60\400,80\4500,100\5000]
  SSCTime = 200      ;0,20 sec
  QuadOn = POSITIVE	
ENDIF

ENDIF
;------------------
IF QuadOn THEN
IF (DualShock(1).bit0 = 0) and LastButton(0).bit0 THEN ;Select Button test
IF TravelLengthX=0 & TravelLengthZ=0 & TravelRotationY=0 THEN

  ;Switch to next Gait type
	IF GaitType<4 THEN ;was7 
	  Sound P9,[50\4000]
	  GaitType = GaitType+1
  	ELSE
	  Sound P9,[50\4000, 50\4500]
	  GaitType = 0
  	ENDIF

  	GOSUB GaitSelect
  ENDIF
ENDIF

;------------------
IF (DualShock(1).bit4 = 0) and LastButton(0).bit4 THEN ;Up Button test
BodyPosY = BodyPosY+10
ENDIF
;------------------
IF (DualShock(1).bit6 = 0) and LastButton(0).bit6 THEN ;Down Button test
BodyPosY = BodyPosY-10
ENDIF
;------------------
; IF (DualShock(2).bit4 = 0) and LastButton(1).bit4 THEN ;Triangle Button test
; BodyPosY = 0
; ENDIF
;------------------
; IF (DualShock(2).bit5 = 0) and LastButton(1).bit5 THEN ;Circle Button test
;
; ENDIF
;------------------
; IF (DualShock(2).bit6 = 0) and LastButton(1).bit6 THEN ;Cross Button test
;
;ENDIF
;------------------
IF (DualShock(2).bit7 = 0) and LastButton(1).bit7 THEN ;Square Button test
IF BalanceMode = 0 THEN
BalanceMode = 1
sound P9,[100\4000, 50\8000]
ELSE
BalanceMode = 0
sound P9,[250\3000]
ENDIF
ENDIF
;------------------
IF (DualShock(2).bit3 = 0) THEN ;R1 Button test
LegLiftHeight = 80
ELSE
LegLiftHeight = 50
ENDIF
;------------------
BodyYShift = 0
IF (DualShock(2).bit2 = 0) THEN ;L1 Button test
BodyPosX = (Dualshock(5) - 128)/2
BodyPosZ = -(Dualshock(6) - 128)/3
BodyRotY = (Dualshock(3) - 128)/6
BodyYShift = (-(Dualshock(4) - 128)/2)MIN-(BodyPosY-10)

ELSEIF (DualShock(2).bit0 = 0) 	;L2 Button test
  BodyRotX = (Dualshock(6) - 128)/8
  BodyRotY = (Dualshock(3) - 128)/6
  BodyRotZ = (Dualshock(5) - 128)/8
  BodyYShift = (-(Dualshock(4) - 128)/2)MIN-(BodyPosY-10)
			
ELSE ;Walk
  'BodyPosX = 0
  'BodyPosZ = 0

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

;------------------
IF (DualShock(2).bit1 = 0) THEN ;R2 Button test
TravelLengthX = -(Dualshock(5) - 128)
TravelLengthZ = (Dualshock(6) - 128)
ELSE
TravelLengthX = -(Dualshock(5) - 128)/2
TravelLengthZ = (Dualshock(6) - 128)/2
ENDIF

  TravelRotationY = -(Dualshock(3) - 128)/4
ENDIF

		 BodyPin=  (((Body_Max - Body_Min) * ((TravelLengthZ + 64 ) & $7F) |
	/ 127 + Body_Max) min Body_Min) max Body_Max 

;------------------
;Calculate walking time delay
InputTimeDelay = 128 - (ABS((Dualshock(5) - 128)) MIN ABS((Dualshock(6) - 128))) MIN ABS((Dualshock(3) - 128))

ENDIF
;------------------
LastButton(0) = DualShock(1)
LastButton(1) = DualShock(2)
return
;-------------------------------------------------------------------------------------------------------------------
;-----------------------------[GAIT TYPE Select]
GaitSelect
;Gait selector
IF (GaitType = 0) THEN ;Ripple Gait 6 steps
LRGaitLegNr = 1 ;Init position of the leg
RFGaitLegNr = 2
RRGaitLegNr = 4
LFGaitLegNr = 5

NrLiftedPos = 1	     ;Number of positions that a single leg is lifted (1-3)
TLDivFactor = 4	     ;Number of steps that a leg is on the floor while walking
StepsInGait = 6      ;Number of steps in gait
TitularGSpeed = 150  ;Supposed speed of the gait

ENDIF
;------------------
IF (GaitType = 1) THEN ;Ripple Gait 12 steps
LRGaitLegNr = 1
RFGaitLegNr = 3
RRGaitLegNr = 7
LFGaitLegNr = 9

NrLiftedPos = 3
HalfLiftHeigth = POSITIVE
TLDivFactor = 8	  
StepsInGait = 12	
TitularGSpeed = 100

ENDIF
;------------------
IF (GaitType = 2) THEN ;Quadripple 9 steps
LRGaitLegNr = 1
RFGaitLegNr = 2
RRGaitLegNr = 5
LFGaitLegNr = 7

NrLiftedPos = 2
HalfLiftHeigth = NEGITIVE	
TLDivFactor = 6	  
StepsInGait = 9	    
TitularGSpeed = 150

ENDIF
;------------------
IF (GaitType = 3) THEN ;Wave 12 steps
LRGaitLegNr = 7
RFGaitLegNr = 1
RRGaitLegNr = 5
LFGaitLegNr = 11

NrLiftedPos = 1
HalfLiftHeigth = NEGITIVE	
TLDivFactor = 10	  
StepsInGait = 12	    
TitularGSpeed = 120

ENDIF
;------------------
IF (GaitType = 4) THEN ;Wave 18 steps
LRGaitLegNr = 10
RFGaitLegNr = 1
RRGaitLegNr = 7
LFGaitLegNr = 16

NrLiftedPos = 2
HalfLiftHeigth = NEGITIVE	
TLDivFactor = 16	  
StepsInGait = 18	    
TitularGSpeed = 100

ENDIF
return
;--------------------------------------------------------------------
;-----------------------------[GAIT Sequence]
GaitSeq
;Calculate Gait sequence
LastLeg = NEGITIVE
GOSUB Gait [LRGaitLegNr, LRGaitPosX, LRGaitPosY, LRGaitPosZ, LRGaitRotY]
LRGaitPosX = GaitPosX
LRGaitPosY = GaitPosY
LRGaitPosZ = GaitPosZ
LRGaitRotY = GaitRotY

GOSUB Gait [RFGaitLegNr, RFGaitPosX, RFGaitPosY, RFGaitPosZ, RFGaitRotY]
RFGaitPosX = GaitPosX
RFGaitPosY = GaitPosY
RFGaitPosZ = GaitPosZ
RFGaitRotY = GaitRotY

GOSUB Gait [RRGaitLegNr, RRGaitPosX, RRGaitPosY, RRGaitPosZ, RRGaitRotY]
RRGaitPosX = GaitPosX
RRGaitPosY = GaitPosY
RRGaitPosZ = GaitPosZ
RRGaitRotY = GaitRotY

LastLeg = POSITIVE
GOSUB Gait [LFGaitLegNr, LFGaitPosX, LFGaitPosY, LFGaitPosZ, LFGaitRotY]
LFGaitPosX = GaitPosX
LFGaitPosY = GaitPosY
LFGaitPosZ = GaitPosZ
LFGaitRotY = GaitRotY

return
;--------------------------------------------------------------------
;-----------------------------[GAIT]
Gait [GaitLegNr, GaitPosX, GaitPosY, GaitPosZ, GaitRotY]

;Check IF the Gait is in motion
GaitInMotion = ((ABS(TravelLengthX)>TravelDeadZone) | (ABS(TravelLengthZ)>TravelDeadZone) | (ABS(TravelRotationY)>TravelDeadZone) )

;Leg middle up position
;Gait in motion Gait NOT in motion, return to home position
IF (GaitInMotion & (NrLiftedPos=1 | NrLiftedPos=3) & GaitStep=GaitLegNr) | (GaitInMotion=NEGITIVE & GaitStep=GaitLegNr & ((ABS(GaitPosX)>2) | (ABS(GaitPosZ)>2) | (ABS(GaitRotY)>2))) THEN ;Up
GaitPosX = 0
GaitPosY = -LegLiftHeight
GaitPosZ = 0
GaitRotY = 0
ELSE

;Optional Half heigth Rear
IF ((NrLiftedPos=2 & GaitStep=GaitLegNr) | (NrLiftedPos=3 & (GaitStep=GaitLegNr-1 | GaitStep=GaitLegNr+(StepsInGait-1)))) & GaitInMotion THEN
  GaitPosX = -TravelLengthX/2
  GaitPosY = -LegLiftHeight/(HalfLiftHeigth+1)
  GaitPosZ = -TravelLengthZ/2
  GaitRotY = -TravelRotationY/2
ELSE
  
  ;Optional half heigth front
  IF (NrLiftedPos>=2) & (GaitStep=GaitLegNr+1 | GaitStep=GaitLegNr-(StepsInGait-1)) & GaitInMotion THEN
    GaitPosX = TravelLengthX/2
    GaitPosY = -LegLiftHeight/(HalfLiftHeigth+1)
    GaitPosZ = TravelLengthZ/2
    GaitRotY = TravelRotationY/2
  ELSE  	  

  	;Leg front down position
  	IF (GaitStep=GaitLegNr+NrLiftedPos | GaitStep=GaitLegNr-(StepsInGait-NrLiftedPos)) & GaitPosY<0 THEN
      GaitPosX = TravelLengthX/2
      GaitPosY = 0
      GaitPosZ = TravelLengthZ/2
      GaitRotY = TravelRotationY/2

  	;Move body forward      
  	ELSE
      GaitPosX = GaitPosX - (TravelLengthX/TLDivFactor)     
      GaitPosY = 0
      GaitPosZ = GaitPosZ - (TravelLengthZ/TLDivFactor)
      GaitRotY = GaitRotY - (TravelRotationY/TLDivFactor)
    ENDIF
  ENDIF
ENDIF

ENDIF

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

return
;--------------------------------------------------------------------
;-----------------------------[BalCalcOneLeg]
BalCalcOneLeg [PosX, PosZ, PosY, BodyOffsetX, BodyOffsetZ]
;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
gosub GetBoogTan [TotalX, TotalZ]
TotalYbal = TotalYbal + TOINT((BoogTan180.0) / 3.141592)
gosub GetBoogTan [TotalX, TotalY]
TotalZbal = TotalZbal + TOINT((BoogTan180.0) / 3.141592)
gosub GetBoogTan [TotalZ, TotalY]
TotalXbal = TotalXbal + TOINT((BoogTan*180.0) / 3.141592)
'serout S_OUT, i9600, “BalOneLeg PosX=”, sdec PosX," PosZ=", sdec PosZ," TotalXTransZ=", sdec TotalTransZ, 13]
return
;--------------------------------------------------------------------
;-----------------------------[BalanceBody]
BalanceBody:
TotalTransZ = TotalTransZ/6
TotalTransX = TotalTransX/6
TotalTransY = TotalTransY/6
if TotalYbal < -180 then 'Tangens fix caused by +/- 180 deg
TotalYbal = TotalYbal + 360
endif
if TotalZbal < -180 then 'Tangens fix caused by +/- 180 deg
TotalZbal = TotalZbal + 360
endif
if TotalXbal < -180 then 'Tangens fix caused by +/- 180 deg
TotalXbal = TotalXbal + 360
endif

;Balance rotation
TotalYBal = TotalYbal/6
TotalXBal = TotalXbal/6
TotalZBal = -TotalZbal/6
	
;Balance translation
LFGaitPosZ = LFGaitPosZ - TotalTransZ
LRGaitPosZ = LRGaitPosZ - TotalTransZ
RFGaitPosZ = RFGaitPosZ - TotalTransZ
RRGaitPosZ = RRGaitPosZ - TotalTransZ

LFGaitPosX = LFGaitPosX - TotalTransX
LRGaitPosX = LRGaitPosX - TotalTransX
RFGaitPosX = RFGaitPosX - TotalTransX
RRGaitPosX = RRGaitPosX - TotalTransX 

LFGaitPosY = LFGaitPosY - TotalTransY
LRGaitPosY = LRGaitPosY - TotalTransY
RFGaitPosY = RFGaitPosY - TotalTransY
RRGaitPosY = RRGaitPosY - TotalTransY

return
;--------------------------------------------------------------------
;-----------------------------[GETSINCOS]
;Get the sinus and cosinus from the angle +/- multiple circles

GetSinCos [AngleDeg]

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

;Shift rotation to a full circle of 360 deg -> AngleDeg // 360
IF AngleDeg < 0.0 THEN	;Negative values
	AngleDeg = 360.0-(ABSAngleDeg-TOFLOAT(360*(TOINT(ABSAngleDeg/360.0))))
ELSE				    ;Positive values
	AngleDeg = ABSAngleDeg-TOFLOAT(360*(TOINT(ABSAngleDeg/360.0)))
ENDIF

IF AngleDeg < 180.0 THEN	;Angle between 0 and 180
    ;Subtract 90 to shift range
	AngleDeg = AngleDeg -90.0
	;Convert degree to radials
	AngleRad = (AngleDeg*3.141592)/180.0
	
	SinA = FCOS(AngleRad)	;Sin o to 180 deg = cos(Angle Rad - 90deg)
	CosA = -FSIN(AngleRad)	;Cos 0 to 180 deg = -sin(Angle Rad - 90deg)
	
ELSE	;Angle between 180 and 360
	;Subtract 270 to shift range
	AngleDeg = AngleDeg -270.0
	;Convert degree to radials
	AngleRad = (AngleDeg*3.141592)/180.0
	
	SinA = -FCOS(AngleRad)	;Sin 180 to 360 deg = -cos(Angle Rad - 270deg)
	CosA = FSIN(AngleRad)	;Cos 180 to 360 deg = sin(Angle Rad - 270deg)
ENDIF

return
;--------------------------------------------------------------------
;[BOOGTAN2] Gets the Inverse Tangus from X/Y with the where Y can be zero or negative
;BoogTanX - Input X
;BoogTanY - Input Y
;BoogTan - Output BOOGTAN2(X/Y)
GetBoogTan [BoogTanX, BoogTanY]
IF(BoogTanX = 0) THEN ; X=0 -> 0 or Pi
IF(BoogTanY >= 0) THEN
BoogTan = 0.0
ELSE
BoogTan = 3.141592
ENDIF
ELSE

	IF(BoogTanY = 0) THEN	; Y=0 -> +/- Pi/2
		IF(BoogTanX > 0) THEN
			BoogTan = 3.141592 / 2.0
		ELSE
			BoogTan = -3.141592 / 2.0
		ENDIF
	ELSE
		
		IF(BoogTanY > 0) THEN	;BOOGTAN(X/Y)
			BoogTan = FATAN(TOFLOAT(BoogTanX) / TOFLOAT(BoogTanY))
		ELSE	
			IF(BoogTanX > 0) THEN	;BOOGTAN(X/Y) + PI	
				BoogTan = FATAN(TOFLOAT(BoogTanX) / TOFLOAT(BoogTanY)) + 3.141592
			ELSE					;BOOGTAN(X/Y) - PI	
				BoogTan = FATAN(TOFLOAT(BoogTanX) / TOFLOAT(BoogTanY)) - 3.141592
			ENDIF
		ENDIF
	ENDIF
ENDIF

return
;--------------------------------------------------------------------
;-----------------------------[BODY INVERSE KINEMATICS]
BodyIK [PosX, PosZ, PosY, BodyOffsetX, BodyOffsetZ, RotationY]

TotalZ = BodyOffsetZ+PosZ ;Offset betweeen the body and Coxa Z & Input position of the feet Z
TotalX = BodyOffsetX+PosX ;Offset betweeen the body and Coxa X & Input position of the feet X

GOSUB GetSinCos [TOFLOAT(BodyRotX+TotalXBal)] ;Global Input Body pitch
SinG = SinA ;SinG buffer for BodyRotZ / SinA Output of AngleDeg
CosG = CosA ;CosG buffer for BodyRotZ / CosA Output of AngleDeg
GOSUB GetSinCos [TOFLOAT(BodyRotZ+TotalZBal)] ;Global Input Body roll
SinB = SinA ;SinB buffer for BodyRotX / SinA Output of AngleDeg
CosB = CosA ;CosB buffer for BodyRotX / CosA Output of AngleDeg
GOSUB GetSinCos [TOFLOAT(BodyRotY+RotationY+TotalYBal)]

BodyIKPosX = TotalX-TOINT(TOFLOAT(TotalX)CosACosB - TOFLOAT(TotalZ)CosBSinA + TOFLOAT(PosY)SinB)
BodyIKPosZ = TotalZ-TOINT(TOFLOAT(TotalX)CosGSinA + TOFLOAT(TotalX)CosASinBSinG +TOFLOAT(TotalZ)CosACosG-TOFLOAT(TotalZ)SinASinBSinG-TOFLOAT(PosY)CosBSinG)
BodyIKPosY = PosY - TOINT(TOFLOAT(TotalX)SinASinG - TOFLOAT(TotalX)CosACosGSinB + TOFLOAT(TotalZ)CosASinG + TOFLOAT(TotalZ)CosGSinA*SinB + TOFLOAT(PosY)CosBCosG)

return
;--------------------------------------------------------------------
;-----------------------------[LEG INVERSE KINEMATICS]
;Calculates the angles of the tibia and femur for the given position of the feet

LegIK [IKFeetPosX, IKFeetPosY, IKFeetPosZ]

IKFeetPosXZ = TOINT(FSQRT(TOFLOAT((IKFeetPosX*IKFeetPosX)+(IKFeetPosZ*IKFeetPosZ)))) ;Input position of the Feet X/Y/Z

IKCF = FSQRT(TOFLOAT(((IKFeetPosXZ-CoxaLength)*(IKFeetPosXZ-CoxaLength))+(IKFeetPosY*IKFeetPosY)))
	
GOSUB GetBoogTan [IKFeetPosXZ-CoxaLength, IKFeetPosY]
IKA1 = BoogTan

IKA2 = FACOS((TOFLOAT((FemurLength*FemurLength) - (TibiaLength*TibiaLength)) + (IKCF*IKCF)) / (TOFLOAT(2*Femurlength) * IKCF))

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

;IKTibiaAngle
IKTibiaAngle = (90-TOINT(((FACOS((TOFLOAT((FemurLength*FemurLength) + (TibiaLength*TibiaLength)) - (IKCF*IKCF)) / TOFLOAT(2*Femurlength*TibiaLength)))*180.0) / 3.141592)) * -1

;IKCoxaAngle
GOSUB GetBoogTan [IKFeetPosZ, IKFeetPosX]
IKCoxaAngle = TOINT((BoogTan*180.0) / 3.141592)
		
IF(IKCF < TOFLOAT(FemurLength+TibiaLength-30)) THEN
	IKSolution = POSITIVE ;Output POSITIVE IF the solution is possible
ELSE
	IF(IKCF < TOFLOAT(FemurLength+TibiaLength)) THEN
		IKSolutionWarning = POSITIVE ;Output POSITIVE IF the solution is NEARLY possible
	ELSE
		IKSolutionError = POSITIVE	 ;Output POSITIVE IF the solution is NOT possible
	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

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

LRCoxaAngle = (LRCoxaAngle min LRCoxa_MIN) max LRCoxa_MAX
LRFemurAngle = (LRFemurAngle min LRFemur_MIN) max LRFemur_MAX
LRTibiaAngle = (LRTibiaAngle min LRTibia_MIN) max LRTibia_MAX

BodyAngle = (BodyAngle min Body_MIN) max Body_MAX
return
;--------------------------------------------------------------------
;[SERVO DRIVER] Updates the positions of the servos
ServoDriver:
;Front Right leg
serout SSC_OUTPUT,SSC_BAUD,"#",dec RFCoxaPin,“P”,dec TOINT(TOFLOAT(-RFCoxaAngle +90)/0.10588238)+650]
serout SSC_OUTPUT,SSC_BAUD,"#",dec RFFemurPin,“P”,dec TOINT(TOFLOAT(RFFemurAngle+90)/0.10588238)+650]
serout SSC_OUTPUT,SSC_BAUD,"#",dec RFTibiaPin,“P”,dec TOINT(TOFLOAT(-RFTibiaAngle+90)/0.10588238)+650]

;Rear Right leg
serout SSC_OUTPUT,SSC_BAUD,"#",dec RRCoxaPin,“P”,dec TOINT(TOFLOAT(-RRCoxaAngle +90)/0.10588238)+650]
serout SSC_OUTPUT,SSC_BAUD,"#",dec RRFemurPin,“P”,dec TOINT(TOFLOAT(RRFemurAngle+90)/0.10588238)+650]
serout SSC_OUTPUT,SSC_BAUD,"#",dec RRTibiaPin,“P”,dec TOINT(TOFLOAT(-RRTibiaAngle+90)/0.10588238)+650]

;Front Left leg
serout SSC_OUTPUT,SSC_BAUD,"#",dec LFCoxaPin,“P”,dec TOINT(TOFLOAT(LFCoxaAngle +90)/0.10588238)+650]
serout SSC_OUTPUT,SSC_BAUD,"#",dec LFFemurPin,“P”,dec TOINT(TOFLOAT(-LFFemurAngle+90)/0.10588238)+650]
serout SSC_OUTPUT,SSC_BAUD,"#",dec LFTibiaPin,“P”,dec TOINT(TOFLOAT(LFTibiaAngle+90)/0.10588238)+650]

;Rear Left leg
serout SSC_OUTPUT,SSC_BAUD,"#",dec LRCoxaPin,“P”,dec TOINT(TOFLOAT(LRCoxaAngle +90)/0.10588238)+650]
serout SSC_OUTPUT,SSC_BAUD,"#",dec LRFemurPin,“P”,dec TOINT(TOFLOAT(-LRFemurAngle+90)/0.10588238)+650]
serout SSC_OUTPUT,SSC_BAUD,"#",dec LRTibiaPin,“P”,dec TOINT(TOFLOAT(LRTibiaAngle+90)/0.10588238)+650]

;Body Rotate
serout SSC_OUTPUT,SSC_BAUD,"#",dec BodyPin,“P”,dec TOINT(TOFLOAT(BodyAngle +90)/0.10588238)+650]

;Send
serout SSC_OUTPUT,SSC_BAUD,“T”,dec SSCTime,13]

PrevSSCTime = SSCTime
return
;--------------------------------------------------------------------
;[FREE SERVOS] Power-off all the servos
FreeServos
for Index = 0 to 31
serout SSC_OUTPUT,SSC_BAUD,"#",DEC Index,“P0”]
next
serout SSC_OUTPUT,SSC_BAUD,“T200”,13]
return
;-----------------------------------------------------------------------------------
;REGISTER Support
;(Handle TimerW interrupt)
BEGINASMSUB
HANDLE_TIMERW
push.w r1 ; save away register we will use
bclr #7,@TSRW:8 ; clear the overflow bit in the Timer status word
mov.w @LTIMERWOVERFLOWCNT+1:16,r1 ; We will increment the word that is the highword for a clock timer
inc.w #1,r1
mov.w r1, @LTIMERWOVERFLOWCNT+1:16
pop.w r1 ; restore our registers
rte ; and return
return
;-------------------------------------------------------------------------------------
;[Simple function to get the current time and verify that no overflow happened]
GetCurrentTime
lCurrentTime = lTimerWoverflowCnt + TCNT ; calculate the timer
IF lCurrentTime.highword <> lTimerWOverflowcnt.highword THEN
lCurrentTime = lTimerWoverflowCnt + TCNT ; calculate the timer
ENDIF
return lCurrentTIme
;--------------------------------------------------------------------END-----------------------------------[/code]

GOOD LUCK. dont know why im using capitals!? i wasnt shouting! LOL

[innerbreed]

ok id like to add this change. im not sure why i didnt comment this on but these are the initilization measurements.

regarding XXPosY (25) i cannot get an accurate number due to the tilted servo and that id like to play around with the initial start up pose!
so i have kept them as they are for now.

[code]

;The robot’s right front leg is (-,-) on the axis and it’s rear left leg is (+,+).
;Z-axis is front to back.
;X-axis is side to side.
;Y-axis is down (+) and up (-)

;Feet Positions in mm
RFPosX = 115 ;Start positions of the Right Front leg
RFPosY = 25
RFPosZ = -115

RRPosX = 115 ;Start positions of the Right Rear leg
RRPosY = 25
RRPosZ = 115

LFPosX = 115 ;Start positions of the Left Front leg
LFPosY = 25
LFPosZ = -115

LRPosX = 115 ;Start positions of the Left Rear leg
LRPosY = 25
LRPosZ = 115[/code]

hope this helps.

[innerbreed]

HI Zenta
hope you are well.

did you manage to convert the body coordinate values for body (translation and rotation) into the leg coordinates in a rotation matrix?

as you know i dont have the programming back-ground that you do so i have had no such luck.

i hope to be making a deal soon for the rest of the servos so the structure will be complete. hopefully.
money’s tight at moment.

please dont let this take you away from your own work or take up your time.

hope to speak with you soon mate.

[zenta]

@innerbreed

HI Zenta
hope you are well.

did you manage to convert the body coordinate values for body (translation and rotation) into the leg coordinates in a rotation matrix?.

Hi Innerbreed,

I’ve not forgot you And the rotation matrix should not be a big trouble I think. I’m just a bit occupied at the time with other family stuff. I’m also leaving soon for a little easter vacation.

@innerbreed

as you know i dont have the programming back-ground that you do so i have had no such luck.

Well, I can’t say I’m a programming expert either. And to be honest i think the overall programming task are going to be a bit difficult since I don’t have a Stalker on my desk to test with. As Xan mentioned it would be fun to try out this concept (the tilted coxa) on a hexapod one time.

@innerbreed

i hope to be making a deal soon for the rest of the servos so the structure will be complete. hopefully.
money’s tight at moment.

I understand about the money.

Let me know when you have done step 4

4. Make sure that the code work OK by control without doing more changes, at this point the IK won’t be correct.

[innerbreed]

@zenta

@innerbreed

HI Zenta
hope you are well.

did you manage to convert the body coordinate values for body (translation and rotation) into the leg coordinates in a rotation matrix?

.

Hi Innerbreed,

I’ve not forgot you And the rotation matrix should not be a big trouble I think. I’m just a bit occupied at the time with other family stuff. I’m also leaving soon for a little easter vacation.

@innerbreed

as you know i dont have the programming back-ground that you do so i have had no such luck.

Well, I can’t say I’m a programming expert either. And to be honest i think the overall programming task are going to be a bit difficult since I don’t have a Stalker on my desk to test with. As Xan mentioned it would be fun to try out this concept (the tilted coxa) on a hexapod one time.

@innerbreed

i hope to be making a deal soon for the rest of the servos so the structure will be complete. hopefully.
money’s tight at moment.

I understand about the money.

Let me know when you have done step 4

4. Make sure that the code work OK by control without doing more changes, at this point the IK won’t be correct.

hope you have a nice break. i return to work tomorrow after having two weeks off. cant complain…

Well, I can’t say I’m a programming expert either.

dont put yourself down,

once (if i can) get step 4 done ill let you know. thanks again.

[tnay]

Hi innerbreed,

Had been looking at your quad design, as I am looking to build one. Still at code level though. I’m using other chips and programming tool, but still will be refering to your (and phoenix) code.

Just a question, have you tried (manually by hand), manipulating the coxa joint from min to max? Mind to take a picture and post here?

I am looking at this configuration, to be exact.

             
\\         
 \\___________
  ||         ||\\
  ||         || \\
  ||         ||     
  ||         ||   
  ||         || //
  ||_________||//
 //
//

[innerbreed]

@tnay

Hi innerbreed,

Had been looking at your quad design, as I am looking to build one. Still at code level though. I’m using other chips and programming tool, but still will be refering to your (and phoenix) code.

Just a question, have you tried (manually by hand), manipulating the coxa joint from min to max? Mind to take a picture and post here?

I am looking at this configuration, to be exact.

             
\\         
 \\___________
  ||         ||\\
  ||         || \\
  ||         ||     
  ||         ||   
  ||         || //
  ||_________||//
 //
//

hi tnay

i have done these by hand as iv not “powered” up yet.
is this what you are looking for?

[code];-----------------------------[MIN/MAX ANGLES in degrees]
;Take the servo Pulse, subtract 1500, divide by 750, and mutply by 90
;(180 degree ranged servos)
;e.g 2400-1500/750*90=108
RRCoxa_MIN con -40 ;Mechanical limits of the RIGHT REAR Leg
RRCoxa_MAX con 40

RRFemur_MIN con -75
RRFemur_MAX con 75

RRTibia_MIN con -45
RRTibia_MAX con 77
;-------------------
RFCoxa_MIN con -40 ;Mechanical limits of the RIGHT FRONT Leg
RFCoxa_MAX con 40

RFFemur_MIN con -75
RFFemur_MAX con 75

RFTibia_MIN con -45
RFTibia_MAX con 77
;-------------------
LRCoxa_MIN con -40 ;Mechanical limits of the LEFT REAR Leg
LRCoxa_MAX con 40

LRFemur_MIN con -75
LRFemur_MAX con 75

LRTibia_MIN con -77
LRTibia_MAX con 45
;-------------------
LFCoxa_MIN con -40 ;Mechanical limits of the LEFT FRONT Leg
LFCoxa_MAX con 40

LFFemur_MIN con -75
LFFemur_MAX con 75

LFTibia_MIN con -45
LFTibia_MAX con 45
;-------------------
Body_Min con -100 ;Mechanical limits of the BODY ROTATE
Body_Max con 100
;--------------------------------------------------------------------
;-----------------------------[BODY/LEG DIMENSIONS]
;Phoenix setup
;CoxaLength con 29 ;Length of the Coxa [mm]
;FemurLength con 76 ;Length of the Femur [mm]
;TibiaLength con 106 ;Lenght of the Tibia [mm]

;Stalker setup 124/106*100=# -100 =#%
CoxaLength con 55 ;Length of the Coxa [mm] 47.0% bigger
FemurLength con 96 ;Length of the Femur [mm] 26.3% bigger
TibiaLength con 140 ;Lenght of the Tibia [mm] 32.0% bigger
; These percentages show size difference to the Phoenix

CoxaAngle con 90 ;Default Coxa setup angle was 60

RFOffsetX con -60 ;Distance X from center of the body to the Right Front coxa
RFOffsetZ con -75 ;Distance Z from center of the body to the Right Front coxa

RROffsetX con -60 ;Distance X from center of the body to the Right Rear coxa
RROffsetZ con 75 ;Distance Z from center of the body to the Right Rear coxa

LFOffsetX con 60 ;Distance X from center of the body to the Left Front coxa
LFOffsetZ con -75 ;Distance Z from center of the body to the Left Front coxa

LROffsetX con 60 ;Distance X from center of the body to the Left Rear coxa
LROffsetZ con 75 ;Distance Z from center of the body to the Left Rear coxa[/code]

i can take a picture and post it later tonight when im back home.
my angles are slightly less than normal as im using a tilted coxa configuration which makes the legs turn inwards when coxa reachs these min and max points.

you will see what i mean when i get you a photo.

cheers.

[innerbreed]

@tnay

Hi innerbreed,

Had been looking at your quad design, as I am looking to build one. Still at code level though. I’m using other chips and programming tool, but still will be refering to your (and phoenix) code.

Just a question, have you tried (manually by hand), manipulating the coxa joint from min to max? Mind to take a picture and post here?

I am looking at this configuration, to be exact.

             
\\         
 \\___________
  ||         ||\\
  ||         || \\
  ||         ||     
  ||         ||   
  ||         || //
  ||_________||//
 //
//

photos. i dont know how they will help you with your project tho?!

the two coxa joints are set at 40º % -40º (min/max) mechanical limit!
and 50º tilt!
the min/max can be a bit more than this as you can see the quad is sitting with
its body to the floor so the tars (feet) still have some space between them.
this gap tightens up when body is higher off the ground!

obviously your configuration will be different.
top
http://i531.photobucket.com/albums/dd355/innerbreed/100_1016.jpg
front
http://i531.photobucket.com/albums/dd355/innerbreed/100_1017.jpg
http://i531.photobucket.com/albums/dd355/innerbreed/100_0948-1.jpg
[size=75]
sneaky update: ordered 8 more HS-645MG’s today. hopefully they will arrive soon. [/size]

[tnay]

I was considering the typical phoenix leg configuration, and also the 90deg configuration (aka humoniod leg).

The disadvantage of phoenix is that the foot between front and back cannot position too near to each other (due to obstruction by the servo). As a result, the gait will not be as optimise.

Your configuration allows for that. Just need your visual support to verify it.

Anyway, I just completed the FK for your leg configuration, will be working on the IK tonight. Hopefully can complete and post tomorrow.

[innerbreed]

This is great. I also think as the body configuration is tighter and all the measurements are the same between limbs, unlike the elongated phoenix body. be great to see your code. Also any picture of your bot. You should open up a new projects post. Your welcome to post here too.

[tnay]

I’m still not ready to open up a thread, plenty of testing and coding to do. I purchase the phoenix kit (hardware only). Now undecided if I should build it and run as hexapod (and try executing quad gait on it), or build a custom quadruped base and use phoenix legs.

I’m still working on the IK, trying to simplify the equations and match your code. The equations were twice as long!

I thinking of adapting your idea too, (after i decided if I should make a quad instead of hex). If it’s ok with you

[innerbreed]

@tnay

I’m still not ready to open up a thread, plenty of testing and coding to do. I purchase the phoenix kit (hardware only). Now undecided if I should build it and run as hexapod (and try executing quad gait on it), or build a custom quadruped base and use phoenix legs.

I’m still working on the IK, trying to simplify the equations and match your code. The equations were twice as long!

I thinking of adapting your idea too, (after i decided if I should make a quad instead of hex). If it’s ok with you

i think the idea of making a quad using phoenix parts would look cool. why not build the phoenix and have a play about. hex’s are great fun too.

a wise man once said: Share, Use, Improve!
go ahead and grab what you can, make improvements on it.
expanding development is best i think.

[tnay]

Thanks for the encouragement

Btw, I think I have figure out the IK, (got the hint from Zenta earlier post) basically it just transform from global to local coordinate.

[code]
OffsetCoxa = 90 - (any value, measured from x-axis, counter clockwise)

IKFeetPosX_NEW = IKFeetPosXCos(OffsetCoxa) - IKFeetPosYSin(OffsetCoxa)
IKFeetPosY_NEW = IKFeetPosXSin(OffsetCoxa) + IKFeetPosYCos(OffsetCoxa)[/code]

Since your OffsetCoxa is constant, you can do the cos/sin manually and replace the function with constant. Save precious computational time.

Don’t forget to change the IKFeetPosX and IKFeetPosY in the IK subroutine to IKFeetPosX_NEW and IKFeetPosY_NEW.

Let’s hope it works.

[innerbreed]

@tnay

Thanks for the encouragement

Btw, I think I have figure out the IK, (got the hint from Zenta earlier post) basically it just transform from global to local coordinate.

[code]
OffsetCoxa = 90 - (any value, measured from x-axis, counter clockwise)

IKFeetPosX_NEW = IKFeetPosXCos(OffsetCoxa) - IKFeetPosYSin(OffsetCoxa)
IKFeetPosY_NEW = IKFeetPosXSin(OffsetCoxa) + IKFeetPosYCos(OffsetCoxa)[/code]

Since your OffsetCoxa is constant, you can do the cos/sin manually and replace the function with constant. Save precious computational time.

Don’t forget to change the IKFeetPosX and IKFeetPosY in the IK subroutine to IKFeetPosX_NEW and IKFeetPosY_NEW.

Let’s hope it works.

[code]OffsetCoxa con 90 - (any value, measured from x-axis, counter clockwise)

IKFeetPosX_NEW var sword ;Input position of the Feet X
IKFeetPosY_NEW var sword ;Input position of the Feet Y

IKFeetPosX_NEW = IKFeetPosX_NEWCos(OffsetCoxa) - IKFeetPosY_NEWSin(OffsetCoxa)
IKFeetPosY_NEW = IKFeetPosX_NEWSin(OffsetCoxa) + IKFeetPosY_NEWCos(OffsetCoxa)

LegIK [IKFeetPosX_NEW, IKFeetPosY_NEW, IKFeetPosZ]

IKFeetPosXZ = TOINT(FSQRT(TOFLOAT((IKFeetPosX_NEW*IKFeetPosX_NEW)+(IKFeetPosZ*IKFeetPosZ)))) ;Input position of the Feet X/Y/Z

IKCF = FSQRT(TOFLOAT(((IKFeetPosXZ-CoxaLength)*(IKFeetPosXZ-CoxaLength))+(IKFeetPosY_NEW*IKFeetPosY_NEW)))
	
GOSUB GetBoogTan [IKFeetPosXZ-CoxaLength, IKFeetPosY_NEW]
IKA1 = BoogTan

IKA2 = FACOS((TOFLOAT((FemurLength*FemurLength) - (TibiaLength*TibiaLength)) + (IKCF*IKCF)) / (TOFLOAT(2*Femurlength) * IKCF))

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

;IKTibiaAngle
IKTibiaAngle = (90-TOINT(((FACOS((TOFLOAT((FemurLength*FemurLength) + (TibiaLength*TibiaLength)) - (IKCF*IKCF)) / TOFLOAT(2*Femurlength*TibiaLength)))*180.0) / 3.141592)) * -1

;IKCoxaAngle
GOSUB GetBoogTan [IKFeetPosZ, IKFeetPosX_NEW]
IKCoxaAngle = TOINT((BoogTan*180.0) / 3.141592)
		
IF(IKCF < TOFLOAT(FemurLength+TibiaLength-30)) THEN
	IKSolution = POSITIVE ;Output POSITIVE IF the solution is possible
ELSE
	IF(IKCF < TOFLOAT(FemurLength+TibiaLength)) THEN
		IKSolutionWarning = POSITIVE ;Output POSITIVE IF the solution is NEARLY possible
	ELSE
		IKSolutionError = POSITIVE	 ;Output POSITIVE IF the solution is NOT possible
	ENDIF
ENDIF		

return[/code]

[innerbreed]

need to do: picture of full structured quad poses with servos.

Hi, just a quick update…

First i have had to turn the SSC back the right way now as it started to become a pain, having to take the botboard off 'then
disconnecting the servos to access the SSC. Although aesthetically it looked better upside down, “Function over form” has played its card again!
Also I have added a power switch for the servos and will be adding one for the electronics very soon.
and along with this ill be adding an LED “power indicator light” into the third spare space just above the power switches!
http://i531.photobucket.com/albums/dd355/innerbreed/100_1049.jpg

I have added a conventional power supply using Four 1.5v Duracell Alkaline AA cells to power the servos giving 6v 2800mah. One cell attached on each leg!
The main reason for this is to save weight and space. Also i feel its innovative to see a different powering idea. This will change in time once i can afford the right battery!

The PS2 receiver has been placed on top of the body bracket for now, but in till i find a better place for it, its staying there.
The rotation of the body servo is unaffected by its position.

once the program is up and runnig i will neaten up the wires and tidy the whole thing up.

cheers

[xan]

Hi Innerbreed,

@innerbreed

I have added a conventional power supply using Four 1.5v Duracell Alkaline AA cells to power the servos giving 6v 2800mah. One cell attached on each leg! The main reason for this is to save weight and space. Also i feel its innovative to see a different powering idea. This will change in time once i can afford the right battery!

The power option you’re looking at looks cool with a single battery on each leg. You’re talking about 4 1.5V batteries. I think you can better go with an extra battery and use rechargeable ones. (51.2V). Normal onces will make you buy new batteries every 20 minutes… BlackWido is using 51.2V 4900mAh NiMH batteries. It’s got about 20-25 min runtime. Not even halve of the time the lipo does. But it works and makes it possible to place one on each leg.

@innerbreed

The PS2 receiver has been placed on top of the body bracket for now, but in till i find a better place for it, its staying there. The rotation of the body servo is unaffected by its position.

Ah, I was already wondering what that was on the top! I thought it was some kind of accelerate sensor. Did you replace the red led’s for blue ones to? 8)

Xan

[tnay]

Nice build. I like the rugged look on it. Can’t wait for your maiden walk I hope the equation work. Have you tested already?