How to get the Phoenix code running in BasicMicro Studio

Lynxmotion Legged Robots
1

Hi all,

Some of you might have noticed that the Phoenix code isn’t running as it should when you’re using the new BasicMicro Studio. When the Phoenix code is programmed the hexapod will wobble instead of walk. The movements will vary from big steps to tiny steps all the time.

Some interrupt enable features are changed in the new BasicMicro Studio. This causes the Wtimer to be disabled in the phoenix code. Since the synchronisation between the SSC and the BAP depends on this timer it will fail.

This can be easy solved by adding an additional enable without any arguments in the code. The additional enable should enable all interrupts. Just search the code for this line:

enable TIMERWINT_OVF

And add the additional enable like shown here:

enable TIMERWINT_OVF enable
This should be all.

Xan

2

Hello Xan.
Thank you very much for the description and help. It will be a great help. Keep this continue. Your posts are helpful for us.

3

Thanks! And welcome as always :wink:

4

Hi Guys,

I had some new issues with the current Basic Micro Studio (2.0.0.5) in combination with the phoenix code. Kurt was happy to help me out on this and we found the issue.

There is a little change in how the compiler handles the assembly code. So make sure you’ve got the ASM{}, BEGINASMSUB and ENDASMSUB on the correct locations.

The timer functions should look like this.

[code];--------------------------------------------------------------------
;[Handle TimerW interrupt]
BEGINASMSUB
HANDLE_TIMERW
ASM{
push.w r1 ; save away register we will use
bclr #7,@TSRW:8 ; clear the overflow bit in the Timer status word
mov.w @WTIMERWOVERFLOWCNT:16,r1 ; We will increment the word that is the highword for a clock timer
inc.w #1,r1
mov.w r1, @WTIMERWOVERFLOWCNT:16
pop.w r1 ; restore our registers
rte ; and return
}
ENDASMSUB
return
;-------------------------------------------------------------------------------------
;[Simple function to get the current time and verify that no overflow happened]
GetCurrentTime
ASM{
nop ; probably not needed, but to make sure we are in assembly mode
mov.w @WTIMERWOVERFLOWCNT:16, e1
mov.w @TCNT:16, r1
mov.w @WTIMERWOVERFLOWCNT:16, r2
cmp.w r2,e1 ; make sure no overflow happened
beq _GCT_RETURN:8 ; no overflow, so return it

mov.w @WTIMERWOVERFLOWCNT:16, e1
mov.w @TCNT:16, r1

_GCT_RETURN:
mov.l er1, @LCURRENTTIME:16
}
return lCurrentTime ; switched back to basic
;--------------------------------------------------------------------[/code]

If you are also using the RC remote version you should also add the ASM{} brackets in the pulsin7 function

[code];-------------------------------------------------------------------
;[Pulsein7] Read in all 7 servo values in one pass.
Pulsein7:
ASM{
; Make sure all 7 IOs are set to input.
PMR5 = 0 ; all IO lines are general IO
PCR5 = 0 ; All are input (may want to leave bit 7 alone…

…

_P17_RETURN_STATUS:
mov.b r1l,@BPULSETIMEOUT
; finally transisition back to basic and return.
}
return
;--------------------------------------------------------------------[/code]

This solved the problem for me.

Kurt, Thanks again for solving this :slight_smile:

Xan

5

You are welcome and I am glad we have you up and running on Studio.

Kurt

6

LOL, that sure took some time. Didn’t it :wink:

7

Hi Atmel here, I am new to microcontrollers and programming. I have completed the assembly and basic testing of the Phoenix hexapod. That would include the SSC and BB2/Atom pro28. I was able to get the bot servos to come online using the Lynxterm software and update the firmware. All of the servos are aligned and functioning as far as I can tell. The Atom pro/BB2 was successfully initialized using the test ascending beep program. I believe that I have followed the diagrams outlined in Figure 14-3 of the updated schematic for both boards, jumpers off or on where needed according to the schematics, wiring connected just so, R/C connected and on,etc. I then loaded the file and was rewarded with the following error.----> Error: FILE C:\USERS\PAUL\DOWNLOADS\PHOE2PS2\PHOENIX_V20.BAS(LINE 88) : [TOKEN CRRCOXAPIN] : Expected a constant <------
Could you help with this please?

8

Hi Atmel/Paul, :wink:

Make sure you’ve got the files in this order
;Project file order:
; 1. Phoenix_Config_xxx.bas
; 2. Phoenix_Core.bas
; 3. Phoenix_Control_xxx.bas

I think this could solve the issue.

Xan

9

Xan, I don’t know where you found the files listed in your post. The files that open for me using the PHOE2PS2 zip file contain:

phoenix_cfg
Phoenix_control_ps2
phoenix_v20.asm
phoenix_v20
phoenix_v20.pdb
phoenix_v20.prj
This is the order in which the files open upon extract. Is “core” your vernacular for .asm?
I am trying to get this up and running for a group of seniors ( of which I am one) for a sort of “show and tell”. The friends that I have at the Senior Center are really a neat bunch and they love seeing stuff that abounds on the Discovery Channel.
Paul

10

When you open the .prj file in Basic Micro Studio, the project will appear on the left side of the screen. Click on the little + and the project will expand showing all of the files contained in the project. THAT is where you find the file order that Xan is talking about. They have to be in the correct order or it will not compile correctly.

11

Xan is several revisions farther along with the code evolution, so the names are not exactly the same as in the current zip. The latest version has a separate config file for the different control methods. This should help.

phoenix_cfg.bas = Phoenix_Config_xxx.bas
phoenix_V20.bas = Phoenix_Core.bas
phoenix_control_ps2.bas = Phoenix_Control_xxx.bas

12

IT LIVES! Thanks to all of you, Xan, Jim, everybody else that contributed.

IMG_2353.JPG
IMG_2353.JPG1600×1200 308 KB

13

Oh, sorry about that…

Glad you got it to work! Enjoy!

14

I am pleased on your behalf. :slight_smile:

15

Yes me too I would like to learn on that and so that I can use it in my daily work.

It support perth

16

So how would you use it in your daily work?

Alan KM6VV

17

Blast from the past, I’m not sure where to post this although here is good…

I’m trying to compile this old 1.3 phoenix code I edited before I migrate my junk to the 2.0 stuff…

The first problem
It dumps at line 1264 which is this one " inc.w #1,r1" ??? Any ideas???
Is there a specific reason this code does not compile on the 2.0.0.15 IDE?

Also switching gears a bit, is there a specific reason why you can not use the PS2 controller on pins 8,9,10,11 vs 12, 13, 14, 15 ??? I have modified the 2.0 code and the old 1.3 code and neither one of them function on these pins… I want the ability to use the hserin command on the hardware Uart… any assistance would be uber duber…

Thanks,
–Aaron

I’m guessing its something simple, except I’m not an expert in assembly.

Thanks again,

[code];Project Lynxmotion Phoenix
;Description: Phoenix, controlled by a Futaba T7C remote
;Software version: V1.3
;Date: 20-10-2008
;Programmer: Jeroen Janssen (aka Xan)
;
;Hardware setup: ABB2 with ATOM 28 Pro, SSC32 V2, PS2 remote (See further for connections)
;
;NEW IN V1.3
; - Changed controls L1+ right stick
; - Balance calculations Thanks to Kåre Halvorsen (aka Zenta)
;
;RC control:
;Select between walking, movement and Individual leg control mode with the 3-state switch
;Use the two-state switch to toogle between the different gaits, movement or which leg to control
;
;KNOWN BUGS:
; - None at the moment :wink:
;
;====================================================================
;[CONSTANDS]
TRUE con 1
FALSE con 0

BUTTON_DOWN con 0
BUTTON_UP con 1
;--------------------------------------------------------------------
;[SERIAL CONNECTIONS]
SSC_LM_SETUP con 1 ;Changes the SSC pins corresponding to the setup
;1 = Setup with connector to the front
;0 = Setup with connector to the back

SSC_OUT con P0 ;black wire ;Output pin for (SSC32 RX) on BotBoard (Yellow)
SSC_IN con P1 ;white wire ;Input pin for (SSC32 TX) on BotBoard (Blue)
SSC_BAUTE con i38400 ;SSC32 Baute rate
;--------------------------------------------------------------------
;[RC Controller] Futaba T7C Heli
RCCh0 con P8 ;RC Remote Right Stick Left/Right (ch1 on T7C)
RCCh1 con P10 ;RC Remote Right Stick Up/Down (ch2 on T7C)
RCCh2 con P11 ;RC Remote Left Stick Up/Down (ch3 on T7C)
RCCh3 con P12 ;RC Remote Left Stick Left/Right (ch4 on T7C)
RCCh4 con P13 ;RC Remote E-switch (G switch on A model) 3-state (ch5 on T7C)
RCCh5 con P14 ;RC Remote Vr-pot (ch6 on T7C)
RCCh6 con P15 ;RC Remote H-switch (B switch on A model) 2-state (ch7 on T7C)

;--------------------------------------------------------------------
;[PIN NUMBERS]
#IF SSC_LM_SETUP ;Connector to the front
RRCoxaPin con P0 ;Rear Right leg Hip Horizontal
RRFemurPin con P1 ;Rear Right leg Hip Vertical
RRTibiaPin con P2 ;Rear Right leg Knee

RMCoxaPin con P4 ;Middle Right leg Hip Horizontal
RMFemurPin con P5 ;Middle Right leg Hip Vertical
RMTibiaPin con P6 ;Middle 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

LMCoxaPin con P20 ;Middle Left leg Hip Horizontal
LMFemurPin con P21 ;Middle Left leg Hip Vertical
LMTibiaPin con P22 ;Middle 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

;--------------------------------------------------------------------
;[MIN/MAX ANGLES]
RRCoxa_MIN con -26 ;Mechanical limits of the Right Rear Leg
RRCoxa_MAX con 74
RRFemur_MIN con -101
RRFemur_MAX con 95
RRTibia_MIN con -106
RRTibia_MAX con 77

RMCoxa_MIN con -53 ;Mechanical limits of the Right Middle Leg
RMCoxa_MAX con 53
RMFemur_MIN con -101
RMFemur_MAX con 95
RMTibia_MIN con -106
RMTibia_MAX con 77

RFCoxa_MIN con -58 ;Mechanical limits of the Right Front Leg
RFCoxa_MAX con 74
RFFemur_MIN con -101
RFFemur_MAX con 95
RFTibia_MIN con -106
RFTibia_MAX con 77

LRCoxa_MIN con -74 ;Mechanical limits of the Left Rear Leg
LRCoxa_MAX con 26
LRFemur_MIN con -95
LRFemur_MAX con 101
LRTibia_MIN con -77
LRTibia_MAX con 106

LMCoxa_MIN con -53 ;Mechanical limits of the Left Middle Leg
LMCoxa_MAX con 53
LMFemur_MIN con -95
LMFemur_MAX con 101
LMTibia_MIN con -77
LMTibia_MAX con 106

LFCoxa_MIN con -74 ;Mechanical limits of the Left Front Leg
LFCoxa_MAX con 58
LFFemur_MIN con -95
LFFemur_MAX con 101
LFTibia_MIN con -77
LFTibia_MAX con 106
;--------------------------------------------------------------------
;[BODY DIMENSIONS]
CoxaLength con 29 ;Length of the Coxa [mm]
FemurLength con 76 ;Length of the Femur [mm]
TibiaLength con 106 ;Lenght of the Tibia [mm]

CoxaAngle con 60 ;Default Coxa setup angle

RFOffsetX con -43 ;Distance X from center of the body to the Right Front coxa
RFOffsetZ con -82 ;Distance Z from center of the body to the Right Front coxa
RMOffsetX con -63 ;Distance X from center of the body to the Right Middle coxa
RMOffsetZ con 0 ;Distance Z from center of the body to the Right Middle coxa
RROffsetX con -43 ;Distance X from center of the body to the Right Rear coxa
RROffsetZ con 82 ;Distance Z from center of the body to the Right Rear coxa

LFOffsetX con 43 ;Distance X from center of the body to the Left Front coxa
LFOffsetZ con -82 ;Distance Z from center of the body to the Left Front coxa
LMOffsetX con 63 ;Distance X from center of the body to the Left Middle coxa
LMOffsetZ con 0 ;Distance Z from center of the body to the Left Middle coxa
LROffsetX con 43 ;Distance X from center of the body to the Left Rear coxa
LROffsetZ con 82 ;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
;====================================================================
;[ANGLES]
RFCoxaAngle var sword ;Actual Angle of the Right Front Leg
RFFemurAngle var sword
RFTibiaAngle var sword

RMCoxaAngle var sword ;Actual Angle of the Right Middle Leg
RMFemurAngle var sword
RMTibiaAngle var sword

RRCoxaAngle var sword ;Actual Angle of the Right Rear Leg
RRFemurAngle var sword
RRTibiaAngle var sword

LFCoxaAngle var sword ;Actual Angle of the Left Front Leg
LFFemurAngle var sword
LFTibiaAngle var sword

LMCoxaAngle var sword ;Actual Angle of the Left Middle Leg
LMFemurAngle var sword
LMTibiaAngle var sword

LRCoxaAngle var sword ;Actual Angle of the Left Rear Leg
LRFemurAngle var sword
LRTibiaAngle var sword
;--------------------------------------------------------------------
;[POSITIONS]
RFPosX var sword ;Actual Position of the Right Front Leg
RFPosY var sword
RFPosZ var sword

RMPosX var sword ;Actual Position of the Right Middle Leg
RMPosY var sword
RMPosZ var sword

RRPosX var sword ;Actual Position of the Right Rear Leg
RRPosY var sword
RRPosZ var sword

LFPosX var sword ;Actual Position of the Left Front Leg
LFPosY var sword
LFPosZ var sword

LMPosX var sword ;Actual Position of the Left Middle Leg
LMPosY var sword
LMPosZ var sword

LRPosX var sword ;Actual Position of the Left Rear Leg
LRPosY var sword
LRPosZ var sword
;--------------------------------------------------------------------
;[INPUTS]
butA var bit
butB var bit
butC var bit

prev_butA var bit
prev_butB var bit
prev_butC var bit
;--------------------------------------------------------------------
;[OUTPUTS]
LedA var bit ;Red
LedB var bit ;Green
LedC var bit ;Orange
;--------------------------------------------------------------------
;[VARIABLES]
Index var byte ;Index used for freeing the servos
SSCDone var byte ;Char to check if SSC 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
IKSW var float ;Length between shoulder and wrist
IKA1 var float ;Angle between SW line and the ground in rad
IKA2 var float ;?
IKSolution var bit ;Output true if the solution is possible
IKSolutionWarning var bit ;Output true if the solution is NEARLY possible
IKSolutionError var bit ;Output true if the solution is NOT possible
IKFemurAngle var sword ;Output Angle of Femur in degrees
IKTibiaAngle var sword ;Output Angle of Tibia in degrees
IKCoxaAngle var sword ;Output Angle of Coxa in degrees
;--------------------------------------------------------------------
;[RC Remote]
RCInput var word(7)
Buttons var byte
LastButtons var byte
Alive var byte
Mode var Nib ;ch5 Mode switch + twostate switch H = 6 modes
TwoStateSW var bit ;ch 7 ,2-state switch
ToogleMovement var bit
Testleg var nib
Whatleg var nib
ResetInitPos var bit
;--------------------------------------------------------------------
;[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 ;Time for servo updates
PrevSSCTime var word ;Previous time for the servo updates

InputTimeDelay var Byte ;Delay that depends on the input to get the “sneaking” effect
;--------------------------------------------------------------------
;[GLOABAL]
HexOn var bit ;Switch to turn on Phoenix
TurnOff var bit ;Mark to turn off Phoenix
;--------------------------------------------------------------------
;[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
NomGaitSpeed var byte ;Nominal speed of the gait

LegLiftHeight var byte ;Current Travel height
TravelLengthX var sword ;Current Travel length X
TravelLengthZ var sword ;Current Travel length Z
TravelRotationY var sword ;Current Travel Rotation Y

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 TRUE 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 ;TRUE 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
RMGaitLegNr var byte ;Init position of the leg
RRGaitLegNr var byte ;Init position of the leg
LFGaitLegNr var byte ;Init position of the leg
LMGaitLegNr var byte ;Init position of the leg
LRGaitLegNr var byte ;Init position of the leg

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

RMGaitPosX var sbyte
RMGaitPosY var sbyte
RMGaitPosZ var sbyte
RMGaitRotY var sbyte

RRGaitPosX var sbyte
RRGaitPosY var sbyte
RRGaitPosZ var sbyte
RRGaitRotY var sbyte

LFGaitPosX var sbyte
LFGaitPosY var sbyte
LFGaitPosZ var sbyte
LFGaitRotY var sbyte

LMGaitPosX var sbyte
LMGaitPosY var sbyte
LMGaitPosZ var sbyte
LMGaitRotY var sbyte

LRGaitPosX var sbyte
LRGaitPosY var sbyte
LRGaitPosZ var sbyte
LRGaitRotY var sbyte

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
;====================================================================
;[INIT]
;Turning off all the leds
LedA = 0
LedB = 0
LedC = 0

'Feet Positions
RFPosX = 53 ;Start positions of the Right Front leg
RFPosY = 25
RFPosZ = -91

RMPosX = 105 ;Start positions of the Right Middle leg
RMPosY = 25
RMPosZ = 0

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

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

LMPosX = 105 ;Start positions of the Left Middle leg
LMPosY = 25
LMPosZ = 0

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

;Body Positions
BodyPosX = 0
BodyPosY = 0
BodyPosZ = 0

;Body Rotations
BodyRotX = 0
BodyRotY = 0
BodyRotZ = 0

;Gait
GaitType = 0
BalanceMode = 0
LegLiftHeight = 50
GaitStep = 1

;What leg is active variable 1-6

Whatleg = 0

;This resets the Init positions of each leg when they are modified with two leg mode
ResetInitPos = False

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

;SSC
SSCTime = 150
InputTimeDelay = 0
Pause 1000
;====================================================================
;[MAIN]
main:
'Start time
GOSUB GetCurrentTime], lTimerStart
;Read input
GOSUB RCInput1

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

;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 -RMPosX+BodyPosX+RMGaitPosX, RMPosZ+BodyPosZ+RMGaitPosZ,RMGaitPosY, RMOffsetX, RMOffsetZ]
gosub BalCalcOneLeg -RRPosX+BodyPosX+RRGaitPosX, RRPosZ+BodyPosZ+RRGaitPosZ,RRGaitPosY, RROffsetX, RROffsetZ]
gosub BalCalcOneLeg [LFPosX-BodyPosX+LFGaitPosX, LFPosZ+BodyPosZ+LFGaitPosZ,LFGaitPosY, LFOffsetX, LFOffsetZ]
gosub BalCalcOneLeg [LMPosX-BodyPosX+LMGaitPosX, LMPosZ+BodyPosZ+LMGaitPosZ,LMGaitPosY, LMOffsetX, LMOffsetZ]
gosub BalCalcOneLeg [LRPosX-BodyPosX+LRGaitPosX, LRPosZ+BodyPosZ+LRGaitPosZ,LRGaitPosY, LROffsetX, LROffsetZ]
gosub BalanceBody
ENDIF

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

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

;Right Middle leg
GOSUB BodyIK -RMPosX+BodyPosX+RMGaitPosX, RMPosZ+BodyPosZ+RMGaitPosZ,RMPosY+BodyPosY+RMGaitPosY, RMOffsetX, RMOffsetZ, RMGaitRotY]
GOSUB LegIK [RMPosX-BodyPosX+BodyIKPosX-RMGaitPosX, RMPosY+BodyPosY-BodyIKPosY+RMGaitPosY, RMPosZ+BodyPosZ-BodyIKPosZ+RMGaitPosZ]
RMCoxaAngle = IKCoxaAngle
RMFemurAngle = IKFemurAngle
RMTibiaAngle = IKTibiaAngle

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

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

;Left Middle leg
GOSUB BodyIK [LMPosX-BodyPosX+LMGaitPosX, LMPosZ+BodyPosZ+LMGaitPosZ,LMPosY+BodyPosY+LMGaitPosY, LMOffsetX, LMOffsetZ, LMGaitRotY]
GOSUB LegIK [LMPosX+BodyPosX-BodyIKPosX+LMGaitPosX, LMPosY+BodyPosY-BodyIKPosY+LMGaitPosY, LMPosZ+BodyPosZ-BodyIKPosZ+LMGaitPosZ]
LMCoxaAngle = IKCoxaAngle
LMFemurAngle = IKFemurAngle
LMTibiaAngle = IKTibiaAngle

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

GOSUB CheckAngles

LedC = IKSolutionWarning
LedA = IKSolutionError

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

;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
 
  
 Else
 SSCTime = NomGaitSpeed + InputTimeDelay
 ENDIF

GOSUB ServoDriver

goto main
;====================================================================

;[RCInput] reads the input data from the RC-Remote and processes the
;data to the parameters.
RCInput1:

;Read input pulse lengths in the correct order that take shortest time
PULSIN RCCh0, 0, RCInput(0)
PULSIN RCCh2, 0, RCInput(2)
PULSIN RCCh4, 0, RCInput(4)
PULSIN RCCh5, 0, RCInput(5)
PULSIN RCCh1, 0, RCInput(1)
PULSIN RCCh3, 0, RCInput(3)
PULSIN RCCh6, 0, RCInput(6)
;serout S_OUT, i9600, “SSCTime=”, sdec Ssctime, " “,“InputTimeDelay=”, sdec InputTimeDelay,” ",sdec RcInput(0), 13]

BalanceMode = False

if RCInput(6) < 1500 then
  TwoStateSW = False
  else
  TwoStateSW = True
  Whatleg = Whatleg + 1
endif

if RCInput(4) < 1100 then
Mode = 1
elseif ((RCInput(4) > 1400) & (RCInput(4) < 1600))
Mode = 2
else
Mode = 3
endif

If ResetInitPos = true then
Gosub ResetLegs
ResetInitPos = false
endif

;If RCInput(4) < 1100 & TwoStateSW = True then
;Mode = 4
;Elseif ((RCInput(4) > 1400) & (RCInput(4) < 1600)) & TwoStateSW = True
;Mode = 5
;Elseif (RCInput(4) < 1600) & TwoStateSW = true
;Mode = 6
;Endif

if whatleg > 6 then
whatleg = 1
endif

InputTimeDelay = (ABS((RCInput(0)-1500)/4) MIN ABS(((RCInput(1)-1500)/4)) MIN ABS(((RCInput(2)-1500)/4)) MIN ABS(((RCInput(3)-1500)/4)))

;***********************************************************************************
;*** Walking mode, toogle between the different walking gait with 2-state button ***
;***********************************************************************************
if Mode = 1 then
BalanceMode = False

   if TwoStateSW then      'Toggle gait method:
     if GaitType < 7 then      'so far we have 8 gait methods
        GaitType = GaitType +1
        sound P9,[150\(800+(GaitType*100))]
     else
        GaitType = 0
        sound P9,[100\1900,150\2100]
     endif
     gosub GaitSelect
   endif
  	
  	BodyPosY = (RCInput(5)-1000)/8
  	TravelLengthX = (RCInput(0)-1500)/4
    TravelLengthZ = (RCInput(1)-1500)/4
    TravelRotationY = (RCInput(3)-1500)/12

;***********************************************************************************
;*** Move mode, toogle between translating and rotating body with 2-state button ***
;***********************************************************************************
elseif Mode = 2

			LMPosX = ((RCInput(1)-1500)/4) + 105
			LMPosZ = ((RCInput(0)-1500)/4)

;*************************************************************************************
;*** Individual leg control mode choose between 6 legs with 2-state button ***
;*************************************************************************************
elseif Mode = 3
BalanceMode = True
;GaitType = 2

	if whatleg = 1 then
			TestLeg = RFGaitLegNr
     		TravelLengthX = (RCInput(1)-1500)/5   
     		TravelLengthZ = (RCInput(3)-1500)/4
     		TravelHeightY = (RCInput(2)-1500)/4

    Elseif whatleg = 2	 	
     		TestLeg = RMGaitLegNr
     		TravelLengthX = (RCInput(1)-1500)/4   
     		TravelLengthZ = (RCInput(3)-1500)/4
     		TravelHeightY = (RCInput(2)-1500)/4

    Elseif whatleg = 3	 	
     		TestLeg = RRGaitLegNr
     		TravelLengthX = (RCInput(1)-1500)/5   
     		TravelLengthZ = (RCInput(3)-1500)/4
     		TravelHeightY = (RCInput(2)-1500)/4
	
    Elseif whatleg = 4	 	
     		TestLeg = LRGaitLegNr
     		TravelLengthX = (RCInput(1)-1500)/5   
     		TravelLengthZ = (RCInput(3)-1500)/4
     		TravelHeightY = (RCInput(2)-1500)/4

    Elseif whatleg = 5	 	
     		TestLeg = LMGaitLegNr
     		TravelLengthX = (RCInput(1)-1500)/4   
     		TravelLengthZ = (RCInput(3)-1500)/4
     		TravelHeightY = (RCInput(2)-1500)/4
    
    Elseif whatleg = 6	 	
     		TestLeg = LFGaitLegNr
     		TravelLengthX = (RCInput(1)-1500)/5   
     		TravelLengthZ = (RCInput(3)-1500)/4
     		TravelHeightY = (RCInput(2)-1500)/4
    Endif
	;serout S_OUT, i9600, "GaitPosX=", sdec GaitPosX," GaitPosY=", sdec GaitPosY," GaitPosZ=", sdec GaitPosZ, 13]
	;serout S_OUT, i9600, "Whatleg=", sdec Whatleg, 13]

;*************************************************************************************
;*** Mode 4
;*************************************************************************************
Elseif mode = 4
sound P9,[100\1200]
;*************************************************************************************
;*** Mode 5
;*************************************************************************************
Elseif mode = 5
sound P9,[100\1500]
;*************************************************************************************
;*** Mode 6
;*************************************************************************************
Elseif mode = 6
sound P9,[100\1800]

;*************************************************************************************
ENDIF

return
;--------------------------------------------------------------------
;[GAIT Select]
GaitSelect
;Gait selector
IF (GaitType = 0) THEN ;Ripple Gait 6 steps
LRGaitLegNr = 1
RFGaitLegNr = 2
LMGaitLegNr = 3
RRGaitLegNr = 4
LFGaitLegNr = 5
RMGaitLegNr = 6

NrLiftedPos = 1
TLDivFactor = 4
StepsInGait = 6
NomGaitSpeed = 150
ENDIF

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

NrLiftedPos = 3
HalfLiftHeigth = TRUE
TLDivFactor = 8
StepsInGait = 12
NomGaitSpeed = 100
ENDIF

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

NrLiftedPos = 2
HalfLiftHeigth = FALSE
TLDivFactor = 6
StepsInGait = 9
NomGaitSpeed = 150
ENDIF

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

NrLiftedPos = 1
TLDivFactor = 2
StepsInGait = 4
NomGaitSpeed = 150
ENDIF

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

NrLiftedPos = 2
HalfLiftHeigth = FALSE
TLDivFactor = 4
StepsInGait = 6
NomGaitSpeed = 150
ENDIF

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

NrLiftedPos = 3
HalfLiftHeigth = TRUE
TLDivFactor = 4
StepsInGait = 8
NomGaitSpeed = 110
ENDIF

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

NrLiftedPos = 1
HalfLiftHeigth = FALSE
TLDivFactor = 10
StepsInGait = 12
NomGaitSpeed = 120
ENDIF

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

NrLiftedPos = 2
HalfLiftHeigth = FALSE
TLDivFactor = 16
StepsInGait = 18
NomGaitSpeed = 100
ENDIF
return
;--------------------------------------------------------------------
;[GAIT Sequence]
GaitSeq
;Calculate Gait sequence
LastLeg = FALSE
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 [LMGaitLegNr, LMGaitPosX, LMGaitPosY, LMGaitPosZ, LMGaitRotY]
LMGaitPosX = GaitPosX
LMGaitPosY = GaitPosY
LMGaitPosZ = GaitPosZ
LMGaitRotY = GaitRotY

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

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

LastLeg = TRUE
GOSUB Gait [RMGaitLegNr, RMGaitPosX, RMGaitPosY, RMGaitPosZ, RMGaitRotY]
RMGaitPosX = GaitPosX
RMGaitPosY = GaitPosY
RMGaitPosZ = GaitPosZ
RMGaitRotY = GaitRotY
return

;--------------------------------------------------------------------
;[Reset Legs]
ResetLegs

RFPosX = 53 ;Start positions of the Right Front leg
RFPosY = 25
RFPosZ = -91

RMPosX = 105 ;Start positions of the Right Middle leg
RMPosY = 25
RMPosZ = 0

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

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

LMPosX = 105 ;Start positions of the Left Middle leg
LMPosY = 25
LMPosZ = 0

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

Return
;--------------------------------------------------------------------
;[GAIT]
Gait [GaitLegNr, GaitPosX, GaitPosY, GaitPosZ, GaitRotY]
IF Mode = 3 then
IF TestLeg = GaitLegNr then
GaitPosX = TravelLengthX
GaitPosY = -TravelHeightY
GaitPosZ = TravelLengthZ
GaitRotY = 0
ENDIF

ELSE
;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=FALSE & 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
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
LMGaitPosZ = LMGaitPosZ - TotalTransZ
LRGaitPosZ = LRGaitPosZ - TotalTransZ
RFGaitPosZ = RFGaitPosZ - TotalTransZ
RMGaitPosZ = RMGaitPosZ - TotalTransZ
RRGaitPosZ = RRGaitPosZ - TotalTransZ

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

LFGaitPosY = LFGaitPosY - TotalTransY
LMGaitPosY = LMGaitPosY - TotalTransY
LRGaitPosY = LRGaitPosY - TotalTransY
RFGaitPosY = RFGaitPosY - TotalTransY
RMGaitPosY = RMGaitPosY - TotalTransY
RRGaitPosY = RRGaitPosY - TotalTransY
return
;--------------------------------------------------------------------
;[GETSINCOS] Get the sinus and cosinus from the angle +/- multiple circles
;AngleDeg - Input Angle in degrees
;SinA - Output Sinus of AngleDeg
;CosA - Output Cosinus of AngleDeg
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]
;BodyRotX - Global Input pitch of the body
;BodyRotY - Global Input rotation of the body
;BodyRotZ - Global Input roll of the body
;RotationY - Input Rotation for the gait
;PosX - Input position of the feet X
;PosZ - Input position of the feet Z
;BodyOffsetX - Input Offset betweeen the body and Coxa X
;BodyOffsetZ - Input Offset betweeen the body and Coxa Z
;SinB - Sin buffer for BodyRotX
;CosB - Cos buffer for BodyRotX
;SinG - Sin buffer for BodyRotZ
;CosG - Cos buffer for BodyRotZ
;BodyIKPosX - Output Position X of feet with Rotation
;BodyIKPosY - Output Position Y of feet with Rotation
;BodyIKPosZ - Output Position Z of feet with Rotation
BodyIK [PosX, PosZ, PosY, BodyOffsetX, BodyOffsetZ, RotationY]

;Calculating totals from center of the body to the feet
TotalZ = BodyOffsetZ+PosZ
TotalX = BodyOffsetX+PosX
;PosY are equal to a “TotalY”

;Successive global rotation matrix:
;Math shorts for rotation: Alfa (A) = Xrotate, Beta (B) = Zrotate, Gamma (G) = Yrotate
;Sinus Alfa = sinA, cosinus Alfa = cosA. and so on…

;First calculate sinus and cosinus for each rotation:
GOSUB GetSinCos [TOFLOAT(BodyRotX+TotalXBal)]
SinG = SinA
CosG = CosA
GOSUB GetSinCos [TOFLOAT(BodyRotZ+TotalZBal)]
SinB = SinA
CosB = CosA
GOSUB GetSinCos [TOFLOAT(BodyRotY+RotationY+TotalYBal)]

;Calcualtion of rotation matrix:
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
;IKFeetPosX - Input position of the Feet X
;IKFeetPosY - Input position of the Feet Y
;IKFeetPosZ - Input Position of the Feet Z
;IKSolution - Output true IF the solution is possible
;IKSolutionWarning - Output true IF the solution is NEARLY possible
;IKSolutionError - Output true IF the solution is NOT possible
;IKFemurAngle - Output Angle of Femur in degrees
;IKTibiaAngle - Output Angle of Tibia in degrees
;IKCoxaAngle - Output Angle of Coxa in degrees
LegIK [IKFeetPosX, IKFeetPosY, IKFeetPosZ]

;Length between the Coxa and Feet
IKFeetPosXZ = TOINT(FSQRT(TOFLOAT((IKFeetPosXIKFeetPosX)+(IKFeetPosZIKFeetPosZ))))

;IKSW - Length between shoulder and wrist
IKSW = FSQRT(TOFLOAT(((IKFeetPosXZ-CoxaLength)(IKFeetPosXZ-CoxaLength))+(IKFeetPosYIKFeetPosY)))

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

;IKA2 - ?
IKA2 = FACOS((TOFLOAT((FemurLengthFemurLength) - (TibiaLengthTibiaLength)) + (IKSWIKSW)) / (TOFLOAT(2Femurlength) * IKSW))

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

;IKTibiaAngle
IKTibiaAngle = (90-TOINT(((FACOS((TOFLOAT((FemurLengthFemurLength) + (TibiaLengthTibiaLength)) - (IKSWIKSW)) / TOFLOAT(2Femurlength*TibiaLength)))*180.0) / 3.141592)) * -1

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

;Set the Solution quality
IF(IKSW < TOFLOAT(FemurLength+TibiaLength-30)) THEN
IKSolution = TRUE
ELSE
IF(IKSW < TOFLOAT(FemurLength+TibiaLength)) THEN
IKSolutionWarning = TRUE
ELSE
IKSolutionError = TRUE
ENDIF
ENDIF

return
;--------------------------------------------------------------------
;[CHECK ANGLES] Checks the mechanical limits of the servos
CheckAngles:
RFCoxaAngle = (RFCoxaAngle min RFCoxa_MIN) max RFCoxa_MAX
RFFemurAngle = (RFFemurAngle min RFFemur_MIN) max RFFemur_MAX
RFTibiaAngle = (RFTibiaAngle min RFTibia_MIN) max RFTibia_MAX

RMCoxaAngle = (RMCoxaAngle min RMCoxa_MIN) max RMCoxa_MAX
RMFemurAngle = (RMFemurAngle min RMFemur_MIN) max RMFemur_MAX
RMTibiaAngle = (RMTibiaAngle min RMTibia_MIN) max RMTibia_MAX

RRCoxaAngle = (RRCoxaAngle min RRCoxa_MIN) max RRCoxa_MAX
RRFemurAngle = (RRFemurAngle min RRFemur_MIN) max RRFemur_MAX
RRTibiaAngle = (RRTibiaAngle min RRTibia_MIN) max RRTibia_MAX

LFCoxaAngle = (LFCoxaAngle min LFCoxa_MIN) max LFCoxa_MAX
LFFemurAngle = (LFFemurAngle min LFFemur_MIN) max LFFemur_MAX
LFTibiaAngle = (LFTibiaAngle min LFTibia_MIN) max LFTibia_MAX

LMCoxaAngle = (LMCoxaAngle min LMCoxa_MIN) max LMCoxa_MAX
LMFemurAngle = (LMFemurAngle min LMFemur_MIN) max LMFemur_MAX
LMTibiaAngle = (LMTibiaAngle min LMTibia_MIN) max LMTibia_MAX

LRCoxaAngle = (LRCoxaAngle min LRCoxa_MIN) max LRCoxa_MAX
LRFemurAngle = (LRFemurAngle min LRFemur_MIN) max LRFemur_MAX
LRTibiaAngle = (LRTibiaAngle min LRTibia_MIN) max LRTibia_MAX
return
;--------------------------------------------------------------------
;[SERVO DRIVER] Updates the positions of the servos
ServoDriver:
;Front Right leg
serout SSC_OUT,SSC_BAUTE,"#",dec RFCoxaPin,“P”,dec TOINT(TOFLOAT(-RFCoxaAngle +90)/0.10588238)+650]
serout SSC_OUT,SSC_BAUTE,"#",dec RFFemurPin,“P”,dec TOINT(TOFLOAT(-RFFemurAngle+90)/0.10588238)+650]
serout SSC_OUT,SSC_BAUTE,"#",dec RFTibiaPin,“P”,dec TOINT(TOFLOAT(-RFTibiaAngle+90)/0.10588238)+650]

;Middle Right leg
serout SSC_OUT,SSC_BAUTE,"#",dec RMCoxaPin,“P”,dec TOINT(TOFLOAT(-RMCoxaAngle +90)/0.10588238)+650]
serout SSC_OUT,SSC_BAUTE,"#",dec RMFemurPin,“P”,dec TOINT(TOFLOAT(-RMFemurAngle+90)/0.10588238)+650]
serout SSC_OUT,SSC_BAUTE,"#",dec RMTibiaPin,“P”,dec TOINT(TOFLOAT(-RMTibiaAngle+90)/0.10588238)+650]

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

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

;Middle Left leg
serout SSC_OUT,SSC_BAUTE,"#",dec LMCoxaPin,“P”,dec TOINT(TOFLOAT(LMCoxaAngle +90)/0.10588238)+650]
serout SSC_OUT,SSC_BAUTE,"#",dec LMFemurPin,“P”,dec TOINT(TOFLOAT(LMFemurAngle+90)/0.10588238)+650]
serout SSC_OUT,SSC_BAUTE,"#",dec LMTibiaPin,“P”,dec TOINT(TOFLOAT(LMTibiaAngle+90)/0.10588238)+650]

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

;Send
serout SSC_OUT,SSC_BAUTE,“T”,dec SSCTime,13]

PrevSSCTime = SSCTime
return
;--------------------------------------------------------------------
;[FREE SERVOS] Frees all the servos
FreeServos
for Index = 0 to 31
serout SSC_OUT,SSC_BAUTE,"#",DEC Index,“P0”]
next
serout SSC_OUT,SSC_BAUTE,“T200”,13]
return
;-----------------------------------------------------------------------------------
;[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
;----------------------------------------------------[/code]

18

On iPad…

Old issues:

Newer compiler. Issues with inline assembly sometimes compiler interprets… Get around by surrounding assembly with

Asm{
... The code ...
}

Ps2. Another thread viewtopic.php?f=22&t=5958

Kurt

19

Wow! Sometimes its that easy…

I should have looked into this further before posting :\ I can see it right in the 2.0 code vs the older code…

Thanks Kurt.

PS the other question of mine?
is there a specific reason why you can not use the PS2 controller on pins 8,9,10,11 vs 12, 13, 14, 15 ??? I have modified the 2.0 code and the old 1.3 code and neither one of them function on these pins

20

Pin 9 is used for the speaker on the BB2, so I avoid it…

Note: the PS2 requires a Pull up resistor on the DAT line and maybe clock, although I never have needed that. Don’t remember if P8 has an underlying PU or not. I know that P0-P3 usually works. Note: On some receivers the very week internal PU resistor has not been sufficient and I needed to add an external one. This is also try for Arduinos…

EDIT: Forgot to mention: P4-7 did not work, thoughts were that P6,P7 were lower voltage, but not sure… That was why the change from ABB to BB2 was made…