Introducing the STALKER! (project ended)

[innerbreed]

@Xan

@innerbreed

when switching to L2 body moves, the robot “sometimes” starts to walk on its own. ?
doesnt happen all the time which is strange.

also once i initiate walk it never seems to actually stop. the legs seem to keep looping the gait.

This is pretty easy to explain. The analog stick on your PS2 remote doesn’t return to the middle position. There is a threshold that you can increase. One remote is a bit worse then the other one…

;[REMOTE] TravelDeadZone con 4 ;The deadzone for the analog input from the remote

But the best way to go is to build yourself a DIY RC. That’s sooooo much better 8)

Xan

i will look into this when i get a few min’s. thanks xan.
i knew it was a problem with remote not code.
ill give (TravelDeadZone con 5) a try, if not =6.

i will also look into the DIY receiver… does look cool.

tnay
in the vid i was only trying out more body movements.
i wasnt trying to lift legs but as explained 'the problem was in the TravelDeadZone when selecting L2.
in the first vid i i did try use L2 again and you can see it trying to initiate a gait. (faster gait and not wave.) but in the second i never used L2.

[innerbreed]

@Xan

@innerbreed

when switching to L2 body moves, the robot “sometimes” starts to walk on its own. ?
doesnt happen all the time which is strange.

also once i initiate walk it never seems to actually stop. the legs seem to keep looping the gait.

This is pretty easy to explain. The analog stick on your PS2 remote doesn’t return to the middle position. There is a threshold that you can increase. One remote is a bit worse then the other one…

;[REMOTE] TravelDeadZone con 4 ;The deadzone for the analog input from the remote

But the best way to go is to build yourself a DIY RC. That’s sooooo much better 8)

Xan

iv got to TravelDeadZone = 6 and go the result i wanted. thanks guys.

[xan]

@innerbreed

iv got to TravelDeadZone = 6 and go the result i wanted. thanks guys.

Told yah it was easy

Glad to hear it worked! 8)

[innerbreed]

@Xan

@innerbreed

iv got to TravelDeadZone = 6 and go the result i wanted. thanks guys.

Told yah it was easy

Glad to hear it worked! 8)

Everything is easy when you know how.

learning is the best part of this whole project.

thank you so much for helping.

[innerbreed]

@tnay

@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.

Hi innerbreed,

I just found out that there is a small error in the code I gave you. Apparently I miss a neg(-) sign in one of the equation.

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

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

Then, I found that I make another error on the OffsetCoxa direction when I did the IK. By reversing the equation; and here’s the code:

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

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

You might want to try both. If it still doesn’t work, could you take a video of your bot doing some X, Y, Z movement?

To extend your battery life, you might want to try powering 1 leg for trouble-shooting.

after speaking with you earlier today, i have tried to impliment the code.
had a few problems.

the coxa’s now dont line up as before. they turn about 60 degrees front and back after they move to these posistons, there is no movement in the coxa’s. they have frozen.

im not giving up here so i will try and work on it more tonight.

[innerbreed]

What the code does is change the IKFeetPosX and IKFeetPosY from global coordinate to local coordinate (IKFeetPosX_NEW and IKFeetPosY_NEW).

there is a conflict here somewhere so now the IKFeetPos# is getting confused. maybe its more simpler than this i dont know.

I’m not a programmer but somehow i had a different idea for the offsets.
rather than going local with the coords why cant we just allow the tars to adjust accordingly to the curve created by the tilted swing of each leg using some sort of angle/degree constant, ie coxatilt con 45 ;(45º)
allowing the ik to adjust to this using the coxatilt?

i understand that this is the intention of the coordinate change but we are still not telling the Atom that the servo is tilted.

[tnay]

I’m transfering the code to excel worksheet. I’ll do some simulation test tonight (should be morning at your time). I need to get my head around, cause the x,y,z coordinate used in your code is different than mine.

[tnay]

By the way, mind to describe, or post your legs when there are all at 90 deg? Especially femur and tibia servos. The code was written with the following assumptions:

1. Femur at servo neutral position (90deg, 1500us) is no longer horizontal, but incline at 50deg
2. Tibia at servo neutral position (90deg, 1500us) is no longer vertical, but away from the body at 50 deg also.

Phoenix neutral position (1500us, 90deg)

##O#####O # # # # #

Stalker neutral position (1500us, 90deg)

[code]
O
# #

#

#

O #
#
#[/code]

I think you have calibrate the leg otherwise, right? Meaning the femur remain at horizontal and tibia at vertical?

[innerbreed]

its actually more like this.

       
#       
 #      
  O####O
        #
         # 
         #           

off to work now. its 6:30am here now. will catch up with you at some point. thanks.

hope this helps.

[zoomkat]

@zoomkat

Just looking at the angled servos, they will add an extra layer of programming to account for the way the leg will swing in a non horozontal arc when the servo moves the leg. I think it is very important to make a single working leg to study how it will move and how the angle of the rest of the leg will be affected.

I still think a single leg should be focused on first to understand its complex movement and the unusual angles generated.

[zenta]

Hi,

I’m sorry for not have any time for helping you yet. The IK problem won’t be solved only by converting the IKFeetPosX and IKFeetPosY. The tilted coxa affects all axis/coordinates, also the IKFeetPosZ. So to do the convertion I belive using a simplified rotation matrix is the solution.

I can’t promise when I get time to do anything with it. I’m a bit occupied with some outdoor projects at the time. But take a look at the rotation matrix and maybe someone can help you out before me.

[xan]

@innerbreed

its actually more like this.

       
#       
 #      
  O####O
        #
         # 
         #           

off to work now. its 6:30am here now. will catch up with you at some point. thanks.

hope this helps.

Make sure the Tars are perfectly under the tibia (knee) servo. The Fermur joint, tibia joint and tars need to be in a 90 deg angle.

Xan

[tnay]

@zenta

Hi,

I’m sorry for not have any time for helping you yet. The IK problem won’t be solved only by converting the IKFeetPosX and IKFeetPosY. The tilted coxa affects all axis/coordinates, also the IKFeetPosZ. So to do the convertion I belive using a simplified rotation matrix is the solution.

I can’t promise when I get time to do anything with it. I’m a bit occupied with some outdoor projects at the time. But take a look at the rotation matrix and maybe someone can help you out before me.

I have generated the FK, and from observation, the Z-axis is not affected. I need some clarification here:

'(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 '

I’m assuming +y is pointing to the sky, while +z is the front, and +x is the left? By the way, Xan is right, you need to align the Tar under the tibia servo. I had the same experience when assembling the phoenix leg.

[zenta]

@tnay

I have generated the FK, and from observation, the Z-axis is not affected.

When the coxa servo are placed horisontal (as for Phoenix), rotating coxa only affects the X (left to right axis) and Z (back to front axis), the Y (down to up axis) are not affected. But when mounting the coxa servo in a downward position (like Innerbreed did for Stalker) all axis are affected when rotating the coxa servo.

Do you mind sharing your excel model for the generated FK?

[innerbreed]

@Xan

Make sure the Tars are perfectly under the tibia (knee) servo. The Fermur joint, tibia joint and tars need to be in a 90 deg angle.

Xan

they are.

@tnay

@zenta

Hi,

I’m sorry for not have any time for helping you yet. The IK problem won’t be solved only by converting the IKFeetPosX and IKFeetPosY. The tilted coxa affects all axis/coordinates, also the IKFeetPosZ. So to do the convertion I belive using a simplified rotation matrix is the solution.

I can’t promise when I get time to do anything with it. I’m a bit occupied with some outdoor projects at the time. But take a look at the rotation matrix and maybe someone can help you out before me.

I have generated the FK, and from observation, the Z-axis is not affected. I need some clarification here:

'(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 '

I’m assuming +y is pointing to the sky, while +z is the front, and +x is the left? By the way, Xan is right, you need to align the Tar under the tibia servo. I had the same experience when assembling the phoenix leg.

this is correct. thanks for the excel model for the generated FK. here is a link if anyone would like to view it. i dont really know how to use the data it generates.
requires signing into my page!
tech.groups.yahoo.com/group/arac … nd%20Docs/

enless tnay can host it somewhere else.

EDIT: file has been saved and can now be downloaded 10 times. It will be deleted after 90 days

1. Download Link: Click here to download file
   rapidshare.com/files/238563286/Q … _.xls.html

[zenta]

Nice work on the excel model tnay! Did you make it yourself?

I tried the model and as far as I can see all axis are affected when doing coxa rotation for the Stalker. So I still can’t understand how the kinematics can be solved without converting all (X,Y,Z) feet coordinates.

[innerbreed]

ok i have had a play around with the table. great work there!
the readings are great and it works really well. spot on!
i have added the true dimention of the legs so i can get true readings.
http://i531.photobucket.com/albums/dd355/innerbreed/Capturerotation.jpg

http://i531.photobucket.com/albums/dd355/innerbreed/axisToMatrix2.jpg

how can this data be used from the rotation matrix?

here is a copy of the code i am currently running:
ENJOY!

[code]’====================================================
’ Layout Diagram & X/Y/Z plane (Fig:2)
'----------------------
'(a) Back
’ 02 18
’ 01 17 Y+ Z+
’ 00_____16 | /
’ R | | L | /
’ i | | e |/
’ g | | f 0—X
’ h |___| t |
’ t 08 24 |
’ 09 25 |
’ 10 26 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 P0 ;Rear Right leg Hip Horizontal
RRFemurPin con P1 ;Rear Right leg Hip Vertical
RRTibiaPin con P2 ;Rear Right leg Knee

RFCoxaPin con P8 ;Front Right leg Hip Horizontal
RFFemurPin con P9 ;Front Right leg Hip Vertical
RFTibiaPin con P10 ;Front Right leg Knee

LRCoxaPin con P16 ;Rear Left leg Hip Horizontal
LRFemurPin con P17 ;Rear Left leg Hip Vertical
LRTibiaPin con P18 ;Rear Left leg Knee

LFCoxaPin con P24 ;Front Left leg Hip Horizontal
LFFemurPin con P25 ;Front Left leg Hip Vertical
LFTibiaPin con P26 ;Front Left leg Knee

HeadPin 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 -40
RRFemur_MAX con 40

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

RFFemur_MIN con -40
RFFemur_MAX con 40

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

LRFemur_MIN con -40
LRFemur_MAX con 40

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

LFFemur_MIN con -40
LFFemur_MAX con 40

LFTibia_MIN con -45
LFTibia_MAX con 45
;-------------------

HeadPin_Min con -90 ;Mechanical limits of the BODY ROTATE
HeadPin_Max con 90
;--------------------------------------------------------------------
;-----------------------------[BODY/LEG DIMENSIONS]
;124/106*100=# -100 =#%
CoxaLength con 55 ;Length of the Coxa [mm]
FemurLength con 96 ;Length of the Femur [mm]
TibiaLength con 145 ;Lenght of the Tibia [mm]

CoxaAngle con 60 ;Default Coxa setup angle

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

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

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

LROffsetX con 65 ;Distance X from center of the body to the Left Rear coxa
LROffsetZ con 65 ;Distance Z from center of the body to the Left Rear coxa
;--------------------------------------------------------------------
;-----------------------------[REMOTE]
TravelDeadZone con 6 ;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

HeadAngle 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
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. 500 = 0.5sec ;max Speed .018 secs @ 6.0V = 18
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)
TravelLengthXZ var sword ;Current Travel Rotation X/Z (Dualshock(5/6) Left joystick (Left-Right/Up-down)

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

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
;====================================================================
;-----------------------------[TIMER INTERRUPT INIT]
ONASMINTERRUPT TIMERWINT, Handle_TIMERW
;-----------------------------[INITIALISATION]
;Turning off all the leds
LedA = 0
LedB = 0
LedC = 0

;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 = 108 ;Start positions of the Right Front leg
RFPosY = 25
RFPosZ = -108

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

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

LRPosX = 108 ;Start positions of the Left Rear leg
LRPosY = 25
LRPosZ = 108

;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
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 BodyDirection
GOSUB CheckAngles

LedC = IKSolutionWarning
LedA = IKSolutionError

;Read input
GOSUB Ps2Input

;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

;====================================================================
BodyDirection:

;Return to the middle position
HeadAngle=0

;IF ((ABS(TravelLengthX)>TravelDeadZone | ABS(TravelLengthZ)>TravelDeadZone) & TravelLengthZ<TravelDeadZone) | ABS(TravelRotationY*10)>TravelDeadZone THEN
IF ABS(TravelLengthX)>TravelDeadZone | ABS(TravelLengthZ)>TravelDeadZone THEN

;Calculate walking direction X and Z
TravelLengthXZ = SQR((TravelLengthX * TravelLengthX) + TravelLengthZ * TravelLengthZ)
HeadAngle = TOINT(FACOS(TOFLOAT(TravelLengthZ) / TOFLOAT(TravelLengthXZ)) * 180.0 / 3.141592)-180

;Add sign depending on the direction of X
HeadAngle = HeadAngle * (TravelLengthX/ABS(TravelLengthX))

ENDIF

;Calculate body angle depending on rotation
IF ABS(TravelRotationY2)>TravelDeadZone & ABS(TravelRotationY3) > ABS(HeadAngle) THEN
HeadAngle = -TravelRotationY3 ; Rotation max = 166 to get max range of 90 deg.
ENDIF

RETURN
;====================================================================
;[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\400, 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 
	  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 = 35
ENDIF
;------------------
IF QuadOn THEN
IF (DualShock(2).bit5 = 0) and LastButton(1).bit5 THEN ;Circle Button test
IF TravelLengthX=0 & TravelLengthZ=0 & TravelRotationY=0 THEN

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

  	GOSUB GaitSelect
  ENDIF
ENDIF
ENDIF

;------------------
; IF (DualShock(2).bit6 = 0) and LastButton(1).bit6 THEN ;Cross Button test
; SSCTime = SSCTime+100
; 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 ;Return body to center
  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

;------------------
;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 = 4) 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

sound P9,[100\4000]
ENDIF
;------------------
IF (GaitType = 3) THEN ;Ripple Gait 12 steps
LRGaitLegNr = 1
RFGaitLegNr = 3
RRGaitLegNr = 7
LFGaitLegNr = 9

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

sound P9,[100\3000]
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

sound P9,[100\2000]
ENDIF
;------------------
IF (GaitType = 1) THEN ;Wave 12 steps
LRGaitLegNr = 7
RFGaitLegNr = 1
RRGaitLegNr = 5
LFGaitLegNr = 11

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

sound P9,[100\1000]
ENDIF
;------------------
IF (GaitType = 0) 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)
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

HeadAngle = (HeadAngle min HeadPin_MIN) max HeadPin_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 HeadPin,“P”,dec (HeadAngle+90)*10000/1059+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]

[tnay]

Thanks innerbreed for the upload. I had problem with my connection yesterday, it disconnected after I upload the file.

Thanks Zenta! Yup, this is the same base I use for my Sleek project. Except for the 3D rotate, I found it sometime ago, lost the link.

The x,y,z coordinate used in the sheet is different. X is pointing to right, Y is pointing to the front, while Z is pointing to the sky. You may get a bit confuse there

Another thing to note, is leg rotation slide bar, X’ is OffsetCoxa, X is Coxa, Y is Femur, and Z is Tibia.

Say you manipulate Coxa, Femur, Tibia to any position. Then when you manipulate OffsetCoxa, note that the X coordinate, (Y in stalker) does not change.

This is the FK model, not IK (my bad, wrong title!). I generated this to verify the OffsetCoxa and Y-axis is not related. There is a few more sheet, for the IK. I remove it because it’s yet to complete.

[zenta]

@tnay

Say you manipulate Coxa, Femur, Tibia to any position. Then when you manipulate OffsetCoxa, note that the X coordinate, (Y in stalker) does not change.

I believe that you mean the (Z in Stalker) does not change. What you say are correct for your model but not in relative to how Stalker is constructed. When manipulating the OffsetCoxa for the Stalker model all coordinates changes.

Your model are rotating the OffsetCoxa (X’) in a parallell direction to your models X axis, that is not the reality for Stalker. Take a new look to Innerbreeds Stalker… do you see it now?

And thats what make this alot more complicated.

[tnay]

@zenta

@tnay

Say you manipulate Coxa, Femur, Tibia to any position. Then when you manipulate OffsetCoxa, note that the X coordinate, (Y in stalker) does not change.

I believe that you mean the (Z in Stalker) does not change. What you say are correct for your model but not in relative to how Stalker is constructed. When manipulating the OffsetCoxa for the Stalker model all coordinates changes.

Your model are rotating the OffsetCoxa (X’) in a parallell direction to your models X axis, that is not the reality for Stalker. Take a new look to Innerbreeds Stalker… do you see it now?

And thats what make this alot more complicated.

There is another offset, called CoxaAngle, which define the angle offset of the Phoenix front and rear leg. In phoenix, this angle address the problem of the Coxa servo which sit at an odd position. In stalker, this should be 45.

[code] ;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 BodyDirection
GOSUB CheckAngles
[/code]

These codes are bug-free, I like to leave it alone as much as I could. There is 2 sheet of FK model in the excel. If you look at the equations for phoenix (Type 1) and stalker (Type 2), you notice that the FK for stalker is twice as long as phoenix. Imagine the IK equations.

Try this:

[code]CoxaAngle con 45
OffsetCoxa con -40

LegIK [IKFeetPosX, IKFeetPosY, IKFeetPosZ]
IKFeetPosX_NEW = IKFeetPosXCos(OffsetCoxa) - IKFeetPosYSin(OffsetCoxa) ; Rotation of Global coordinate to Local coordinate
IKFeetPosY_NEW = IKFeetPosXSin(OffsetCoxa) + IKFeetPosYCos(OffsetCoxa) ; Rotation of Global coordinate to Local coordinate
[/code]

Remember to change CoxaAngle to 45. Had some problem modeling the IK, but If I’m right, this should work. This is preliminary attempt, not finalized yet.