Futaba T7C Phoenix 1.3/2.0 code

Lynxmotion Legged Robots
1

Howdy everyone,

I decided to start my own thread Jim gave me a friendly reminder about how other things were getting cluttered so why not.

So here’s the scoop, I have a Phoenix along with Futaba T7C transmitter “Helicopter” and receiver mounted onto the 6 legged money pit :slight_smile:… I have noticed that RC control here as a topic has been something that really seems like a lot of people don’t do “other than the super transmitter Jim has had made, ps2 control, and bluetooth, Xbee, or other forms of communication” I’m speaking off the shelf RC parts… My main reason for buying it was seeing what Zenta did with his phoenix and was hands down really neat so I invested.

So the code below is code that Zenta shared with me for an un-modified T7C controller. I have played around with things and have gotten support from Zenta and Xan and Kurte along with Innerbreed and others in the past with help along with adding Individual leg control into the 1.3 code that never had it to begin with “I believe”. This code uses the old Pulsin command to grab the numerical values for each of the pins assigned to read the pwm data from your receiver. It’s ok and all but for the phoenix code chugging up most all cpu cycles the bot walks like it has some issues… either way code is below if you want to peek at it.
pastebin.com/ZHsewcUR I posted this off site to save the length of my post.

Next up is that same 1.3 code I was working on and after I got a hold of the 2.0 code I decided to rip Kurtes magical assembly language out of it and place it within the 1.3 phoenix code I was working with and it was amazing, the differences were visually fixed along with it being as smooth as a ps2 controller was connected to it. I was impressed!.. I know Kurte has had his assembly language to read PWM data out for a while although I just didn’t look at it close enough until about 2 weeks ago, seeing it in the 2.0 DIY config file really helped me tear into it!
With that said here is the same code as above but with the new assembly routines reading the PWM data
pastebin.com/9ZYb6gST

If anyone needs help making their phoenix RC worthy feel free to PM me.

The next bit of code I used Kurtes assembly routines to make an application that simply allows you to calibrate your transmitters output so that all the channels are centered at 1500, I found it necessary to have this tool when trying to diagnose the 2.0 code that I cant seem to run right on my Phoenix.
Here is the test code pastebin.com/fD7FPW0v

I have an issue with the code I posted above, when I turn on my bot its side stepping in one direction and when I lower the bot with channel 5 the bot sidesteps slow when I raise it up with channel 5 the bot sidesteps faster. I have calibrated my transmitter so that all the values are centered so Its like its mixing channel values somewhere and I can’t figure it out for the life of me. Help would be great on this subject.

the basic setup of this is each channel of your receiver needs to be connected to p0 - p6 on anything that is done with any of the 3 urls I posted, its the way Kurte wrote the assembly and isn’t changeable for custom pin configurations, its the nature of the code, I don’t know assembly but that’s just the way the cookie crumbles :slight_smile:.

1 ) Currently I would like to get the 1.3 assembly code working properly which I’m kinda stuck with at the moment.
2 ) I need some assistance in trying to generate a 2.0 config that would be “dumbed down” to nothing really that just tells the phoenix to turn on, only thing I can see is the code

Hexon = 1

I’m imagining the “DIY Config” section I’m looking to build will be very very limited amount of code to tell the pheonix to turn on. From their I believe I can start building a config for my T7C “7 Channels”

Phew that’s it, comments, suggustions, jokes, welcome.

–Aaron

So with that said moving onto better things, I need some help to get a basic “dummed down” config for the 2.0 code that simply turns on when I turn on the phoenix.

2

Hi Aaron.
Good to see you have made a post about the problems you are having.
now we can start work!.

ok i have gone though the code you posted
pastebin.com/9ZYb6gST
and i found a few sections that needed looking at. i have amended it and submitted it to pastebin
pastebin.com/VPeXEhXa
[Hexon]

ok the phoenix code allows you to apply power, stand back and push a button to turn the hex on. and also off. without having to unplug battery. this is a great feature.
but from what i understand you would like to be able to apply power and the hex stand up as soon as the battery is connected, correct?

would look like this…

[code];-------------------Power up
IF (DualShock(1).bit3 = 0) and LastButton(0).bit3 THEN ;Start Button test
IF(HexOn) THEN
'Turn off
Sound P9,[100\5000,60\4000]
BodyPosX = 0
BodyPosY = 0
BodyPosZ = 0
BodyRotX = 0
BodyRotY = 0
BodyRotZ = 0
TravelDirectionX = 0
TravelDirectionZ = 0
TravelRotationY = 0

  SSCTime = 600
  GOSUB ServoDriver
  HexOn = False
ELSE
  'Turn on
  Sound P9,[60\4000,100\5000]
  SSCTime = 200
  HexOn = True	
ENDIF

ENDIF[/code]

if im correct with what i just said then, i have also added HexOn to the Initialization section so it will stand as soon as power is applied.

i hope this has helped.
:wink:

3

I’m not sure that’s right, I do not have a dual-shock controller…

or is that right?

–Aaron

4

yes its ps2.
the section of code iv posted here is just an example of how it works. but you wont need this part if you wish it to turn on automatically.

5

yours would look something like this:

[code] ;Turn off
IF ((Buttons=0 AND LastButtons<>0)) THEN
IF HexOn THEN ;0 Button test
ControlMode=WalkMode
BodyPosX = 0
BodyPosY = 0
BodyPosZ = 0
BodyRotX = 0
BodyRotY = 0
BodyRotZ = 0
TravelLengthX = 0
TravelLengthZ = 0
TravelRotationY = 0

    SelectedLeg=0
    SLHold=false
	RFPosY=cRFPosY
	'RMPosY=cRMPosY
	RRPosY=cRRPosY
	LFPosY=cLFPosY
	'LMPosY=cLMPosY
	LRPosY=cLRPosY
	
    HexOn = False
  ELSE
    ;Turn on
    HexOn = True	
  ENDIF
ENDIF[/code]

if you used this feature you would only need to edit the first line for your controller.

6

Thanks Innerbreed I’ll give this a try.

However I would like to get the 1.3 code working properly before I start learning this 2.0 code

Below is the 1.3 code with Kurtes assembly goodness in place of the pulsin command. after all the code I shal explain my problem.

[code]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 P14 ;black wire ;Output pin for (SSC32 RX) on BotBoard (Yellow)
SSC_IN con P15 ;white wire ;Input pin for (SSC32 TX) on BotBoard (Blue)
SSC_BAUTE con i38400 ;SSC32 Baute rate
;--------------------------------------------------------------------
;[RC Controller] Futaba T7C Heli
RCCh0 con P0 ;RC Remote Right Stick Left/Right (ch1 on T7C)
RCCh1 con P1 ;RC Remote Right Stick Up/Down (ch2 on T7C)
RCCh2 con P2 ;RC Remote Left Stick Up/Down (ch3 on T7C)
RCCh3 con P3 ;RC Remote Left Stick Left/Right (ch4 on T7C)
RCCh4 con P4 ;RC Remote E-switch (G switch on A model) 3-state (ch5 on T7C)
RCCh5 con P5 ;RC Remote Vr-pot (ch6 on T7C)
RCCh6 con P6 ;RC Remote H-switch (B switch on A model) 2-state (ch7 on T7C)
awPulsesIn var word(7)
bPulseTimeout var byte

;--------------------------------------------------------------------
;[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]
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
;--------------------------------------------------------------------
;[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 ;was 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 ;was 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

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:
;-------------------------Begin Crazyness------------------------------------------------
PMR5 = 0
PCR5 = 0
mov.b #0x7f, r1l
mov.l #250000,er2
_PI7_WAIT_FOR_ALL_LOW:
mov.b @PDR5:8, r0l
and.b r1l, r0l
beq _PI7_WAIT_FOR_NEXT_IO_TO_GO_HIGH:8
dec.l #1,er2
bne _PI7_WAIT_FOR_ALL_LOW:8

bra _P17_RETURN_STATUS:16
_PI7_WAIT_FOR_NEXT_IO_TO_GO_HIGH:
mov.l #250000,er2
_PI7_WAIT_FOR_NEXT_IO_TO_GO_HIGH2:
mov.b @PDR5:8, r0l
and.b r1l, r0l
bne _P17_IO_WENT_HIGH:8
dec.l #1,er2
bne _PI7_WAIT_FOR_NEXT_IO_TO_GO_HIGH2:8
bra _P17_RETURN_STATUS:16
_P17_IO_WENT_HIGH:
xor.w r2,r2
xor.b r0h, r0h
mov.l #AWPULSESIN,er3
_P17_WHICH_BIT_LOOP:
shlr.b r0l
bcs _P17_WHICH_BIT_LOOP_DONE:8
inc.b r0h
inc.l #2, er3
add.w #18,r2
bra _P17_WHICH_BIT_LOOP:8
_P17_WHICH_BIT_LOOP_DONE:
xor.b r1h,r1h
bset.b r0h,r1h
bclr.b r0h,r1l
_P17_WAIT_FOR_IO_GO_BACK_LOW:
mov.b @PDR5:8, r0l
and.b r1h, r0l
beq _P17_IO_WENT_BACK_LOW:8
add.w #18,r2
bcc _P17_WAIT_FOR_IO_GO_BACK_LOW:8
bset.b r0h, r1l
bra _P17_RETURN_STATUS:8
_P17_IO_WENT_BACK_LOW:
add.w #22,r2
shlr.w r2
shlr.w r2
shlr.w r2
shlr.w r2
mov.w r2,@er3
or r1l,r1l
bne _PI7_WAIT_FOR_NEXT_IO_TO_GO_HIGH:16
_P17_RETURN_STATUS:
mov.b r1l,@BPULSETIMEOUT
;-------------------------End Crazyness--------------------------------------------------------------
;serout S_OUT, i9600, “SSCTime=”, sdec Ssctime, " “,“InputTimeDelay=”, sdec InputTimeDelay,” ",sdec RcInput(0), 13]

BalanceMode = False

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

if awPulsesIn(6) < 1500 then
TwoStateSW = False
else
TwoStateSW = True
endif

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

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

   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
  	
  	
  	TravelLengthX = (awPulsesIn(0)-1500)/4
    TravelLengthZ = (awPulsesIn(1)-1500)/4
    TravelRotationY = (awPulsesIn(3)-1500)/12
    BodyPosY =  (awPulsesIn(5)-1000)/8 
    serout S_OUT, i9600, "TravelLengthX=", sdec TravelLengthX," TravelHeightY=", sdec TravelHeightY," TravelLengthZ=", sdec TravelLengthZ, "BodyPosY=", sdec BodyPosY, 13]

;***********************************************************************************
;*** Move mode, toogle between translating and rotating body with 2-state button ***
;***********************************************************************************
elseif Mode = 2
if TwoStateSw then 'Toogle movement method
if ToogleMovement then
ToogleMovement = False
sound P9,[100\800,150\1800]
else
ToogleMovement = True
sound P9,[100\1800,150\800]
endif
endif
if ToogleMovement then
'Body translate:
BodyPosX = -(awPulsesIn(0)-1500)/10
BodyPosZ = -(awPulsesIn(1)-1500)/8
BodyRotY = -(awPulsesIn(3)-1500)/20
BodyPosY = (awPulsesIn(5)-1000)/8
else
'Body rotate:
BodyRotX = -(awPulsesIn(1)-1500)/20
BodyRotY = -(awPulsesIn(3)-1500)/20
BodyRotZ = (awPulsesIn(0)-1500)/20
BodyPosY = (awPulsesIn(5)-1000)/8
endif

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

	If awPulsesIn(6) > 1600 then
	Whatleg = Whatleg + 1
	elseif Whatleg > 7
	Whatleg = 1
	endif

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

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

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

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

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
;--------------------------------------------------------------------
;[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]

Alright its after the code
I have a single line of debug output placed within the “walking mode” loop

serout S_OUT, i9600, "TravelLengthX=", sdec TravelLengthX," TravelHeightY=", sdec TravelHeightY," TravelLengthZ=", sdec TravelLengthZ, "BodyPosY=", sdec BodyPosY, 13]

I have sample output from it

TravelLengthX=-40 TravelHeightY=0 TravelLengthZ=0BodyPosY=19

Here is the issue, when I adjust bodypositionY with channel 6 which is AwPulsesIn(5) it for some reason starts adjusting the value of TravelLengthX which is roughly double that of bodupositionY… I CANT FIGURE THIS OUT and its driving me coo coo, cause I want to take this thing into work and show a couple individuals.

This question though is so unique to people with phoenix’s and who have used Kurtes assembly snippit!

I’m clueless… Thanks in advance.

–Aaron

7

I responded to the PM. My guess is maybe a WTimer interrupt happens during the processing…

8

Thanks!

I’ll post that in here during/before/after I make sense of it all.

Thanks.

–Aaron

9

It’s probably best to use the file attachment function for code this long… I know I read every post on the forum, and my scrolling finger is worn out! :stuck_out_tongue: The code box function is truly meant for short programs or snippets.

10

From your PM, I am guessing one possible reason would be if you are getting interrupts while in the assembly code, which can mess up the timings. So I suggest you disable the timer interrupt while in that code. But with timerW you can get something like: 30+ interrupts per second (16000000 /8 /65536), where with TimerA you can get this down to something like: 7.6 times per second (16000000/8192/256) so as I don’t think the RC cycle should exceed 1/8th of a second, you should not lose any interrupts. I suggested where the code changes might be. I did a quick edit of your posted code to do it. It compiles but I have not tested… All of the changes are under #ifdef USETIMERA

Kurt
rc phoenix with timera.bas (49 KB)

11

Cool!

I’ll have to check these when I get home from work, I think I sorta grasp what changes you did but I could be way off.
Kurte

By this comment are you saying you don’t believe that the assembly code you wrote will run 1 cycle in less then 1/8th of a second?

As for the timerA/B comments I should probably read the basic atom manual to figure this out, although by the looks of it are these commands used to keep track of the point at which the gait is currently at? … I know its keeping track of time based off the speed of the cpu…

either way I’ll try this code when I get home today. These problems don’t exist in the 2.0 code correct?

Beth — I did not see the upload attachment feature! I’ll do this next time.

–Aaron

12

No the exact opposite. What I was trying to say is that the cycle won’t take that long as such we should never lose a interrupt. I believe that most of these RC receivers do a cycle in maybe 20ms or about 50 times per second.

13

Oh ok that’s right…

I remember you had explained this to me in the past
7x20ms would tie the atom pro board up for 140ms each time it would run though all 7 pulsin commands. Why the receiver would waste either 19ms or 18ms of low before the next read is a design oddity if you ask me.

It would have been nice if they would have allowed 3ms pulsewidths so the time wasted would be very minimal.

–Aaron

14

Hi Kurte,

I tried your code, Unfortunately it does the same thing.

Here is the output I still get the same values
TravelLengthX=-20 TravelHeightY=0 TravelLengthZ=0BodyPosY=9

Are there any outputs I can watch via a Serout command that I can paste in here to assist in trying to debug the values that are incorrect?

–Aaron

15

Hi Aaron,

Sometimes hard to know what to do in cases like this. It might help to see what the values are in the awPulsesIn array when things are not right. May also be good to see what they are when they are right… What would really help is to see an output of a logic Analyzer when this is happening, to see what the actual pulses are.

This code makes the assumption that only one pulse will be active at a time. It does not care what order they happen in. That worked fine with the Hitec receiver as well as the spectrum. An example from the spectrum:
http://i416.photobucket.com/albums/pp245/Kurts_Robots/Spectrumreceiver.png
Each of the pulses are probably within the range of 500-2500us in length. This shows the 7 pulses for the receiver (disregard the input labels, I had not reset them). In the above example the total time for the 7 channels is maybe 11ms. These signals repeat about every 20ms.

16

well I think the only thing that I have the ability to tell you is that channel 6 is mixing with channel 1
The other thing I can tell you is that “BodyPosY” is half that of “TravelLengthX”.

–Aaron

If you have any variables you want values for while the program is running let me know and I can add that in and paste the values for you.

17

Hi Aaron,

As I mentioned you may want to print out the values returned from the Pulsein7 function to verify that the actual values are off.

The problem may be I am having too much fun with other projects :smiley:. Today I would like to get my phoenix to walk and run sequences using the Arc32. All of the code is in place, now just need to finish centering, make some minor mods, etc.

But back on your issues. My next guess would be that maybe when you change your channel 5 it causes the signals to overlap, which the code was not designed to handle. That is where the analyzer would be great!

If it were my project and I did not have the hardware to verify this stuff, would be to write the beginnings of another multiple pulsin function to see if this was the case. I would probably write it in assembly language as to get the timing right, but I don’t have the energy to do the complete version right now. Here is the beginnings of a basic version that might print out when each bit transition is and a loop counter was. This way you might be able to see if things are overlapping. Maybe add a call to this when you know that something is not right, like when TravelLengthX is > ???

Note, there may be and probably are bugs in this, it compiled, but I did not try it out…

If this shows overlap then you might be able to hack up this code to make it work for you. Instead of assembly language you
could continue with the code and change from using the loop counter to using a timer register. Could probably setup to use TimerW…

Good Luck
Kurt

[code]
;=============================================================================
; this is just the outline of a function. Should convert to assembly language.
;==============================================================================
wLoopCntr var word ; how many times we went through the loop
bMask var byte ; Our mask of the bits we are still interested in
bIOVal var byte ; The IO port value (masked)
bIOValPrev var byte ; The previous IO port value (masked)
bIODif var byte ; what changed - What IO bits were turned off

abIOStates var byte(15) ; what state we were at
awIOTimes var word(15) ; Value of the loop counter.
cSaveStates var byte ; index into our saved array.

MPulseinV2:
; 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…

bMask = 0x7f		; what we are still looking for...

; first wait until all of the IOs are low 
while (PDR5 & bMask )
    ; 
wend 

; initialize for our main loop
cSaveStates = 0		; no saved states
wLoopCntr = 0		; clear counter
bIOValPrev = 0x0	; no bits were on.

; main loop - to be used as actual values all code paths through this loop would need
; to have a consistent timings...
while bMask and (cSaveStates < 15)
	bIOVal = (PDR5 & bMask )				; get the masked value
	if bIOVal <> bIOValPrev then
		abIOStates(cSaveStates) = bIOVal	; save away the new value
		awIOTimes(cSaveStates) = wLoopCntr	; save away the time
		cSaveStates = cSaveStates + 1		; increment how many states we have saved
		
		; Turn Off any bits in our mask that were on that are now off...
		bMask = bMask & ((!bIOValPrev) | bIOVal)
		bIOValPrev = bIOVal					; remember the previous value
	endif
	wLoopCntr = wLoopCntr + 1				; increment our loop counter
wend		

; now print out masks and timers to see what is happening...
for wLoopCntr = 0 to cSaveStates
	serout s_out, i9600, [hex abIOStates(wLoopCntr), ":", dec awIOTimes(wLoopCntr), " "]
	if (wLoopCntr & 0x7) = 0x7 then
		serout s_out, i9600, [13]
	endif
next
serout s_out, i9600, [13]

return ][/code]

18

Thanks,

I’ll reply in more detail later when I’m at home able to load this up and give it a whirl.

Thanks,
–Aaron

*edit

So I did however make a program to test the outputs of all 7 channels and all of the values came out as expected. “This was the program that was just your assembly code and did a serout of all 7 variables in decimal to verify all 7 values are at 1500 while the positions of the gymbals/switches are centered” … .So taking that info I’m sure that some other code within the phoenix codebase is causing your assembly code to spit out incorrect values.

I can also take that same serout line that takes all 7 values and put that within the main loop and log the values of all 7 values of AwPulsin(0) - AwPulsein(6)… and I’ll post em here.

19

Hi,
For about 14 months ago I used the logic analyzer to see how the futaba receiver sent out the pulses on each channel. I belive Kurt has a point about the signal overlaping. Actually, every channel overlap just a little. Except for ch1,ch6 and ch7, they overlap alot!

FutabaLogicAnalyze.jpg
FutabaLogicAnalyze.jpg805×362 58 KB

I hope this can be at any help.

20

Thanks Zenta ,

As they say a picture is worth a 1000 words :open_mouth:. That confirms the problem. The current assembly code is not set up to handle the overlap. The little overlap is usually not a problem as it is very minimal, probly less than 1us. But the 6,7,1 is a different story!

Options:
If you can live without Channel 5, you can simply remove that bit at the start of the assembly code:
PMR5 = 0
PCR5 = 0
mov.b #0x7f, r1l
Change the 7f to a 5f…

Could go back to the old pulsin commands for each channel. Will take up a longer time. Should optimize for order of the inputs. By numbers in Zentas trace. 1,3,5,7,2,4,6 - Could take up to about 3 cycles to complete…

Could implement the code like I mentioned in the prevoius post. Not sure if doing it in Basic will give you the accuracy you need. But the code in Basic might look again something like:

; like 2 but setup to use timerW instead of loop ctr
MPulseinV3:
    ; 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... 
    
    
    bMask = 0x7f		; what we are still looking for...
    
    ; first wait until all of the IOs are low 
    while (PDR5 & bMask )
        ; 
    wend 

    ; setup timerW
    TCRW = 0x30	; setup for clock/8
    TMRW = 0x80	; make sure it is running
    TCNT = 0	;

	; initialize for our main loop
	cSaveStates = 0		; no saved states
	bIOValPrev = 0x0	; no bits were on.
	
	; main loop - to be used as actual values all code paths through this loop would need
	; to have a consistent timings...
	while bMask and (cSaveStates < 15)
		bIOVal = (PDR5 & bMask )				; get the masked value
		if bIOVal <> bIOValPrev then
			abIOStates(cSaveStates) = bIOVal	; save away the new value
			awIOTimes(cSaveStates) = TCNT		; save away the time from timerW
			cSaveStates = cSaveStates + 1		; increment how many states we have saved
			
			; Turn Off any bits in our mask that were on that are now off...
			bMask = bMask & ((!bIOValPrev) | bIOVal)
			bIOValPrev = bIOVal					; remember the previous value
		endif
		wLoopCntr = wLoopCntr + 1				; increment our loop counter
	wend		
	
	; Now to post process the data
	for wLoopCntr = 1 to cSaveStates-1
		; Assumes that not two will end at the same time... Could handle this as well
		bIOVal = abIOStates(wLoopCntr-1) & !(abIOStates(wLoopCntr))  ; I think should give us a mask of the bits that turned off
		
		if bIOVal then
			
			; reuse bMask
			bMask = wLoopCntr-1
			while (bMask > 0) and (abIOStates(bMask) & bIOVal)	; loop back to find out when this bit was turned on.
				bMask = bMask - 1
			wend
			if !(abIOStates(bMask) & bIOVal) then
				bMask = bMask + 1
			endif
				
			; figure out which bit...
			bIOValPrev = 1
			while ((bIOVal & 0x1) = 0)
				bIOValPrev = bIOValPrev + 1
				bIOVal = bIOVal >> 1
			wend
			awPulsesIn(bIOValPrev) = (awIOTimes(wLoopCntr) - awIOTimes(bMask)) / 2 ; End - start /2 as clock is /8 
		endif
	next	
    return

Again I compiled this, but did not load it on a processor or test it in any way. So probably are issues… Also in the secondary loop I reused variables, so …

Kurt