I saw the article in Robot magazine regarding this method of control for the phoenix. The only problem I have is I can’t find the program “Phoenix_V1.3_RCFutaba.bas”, which is required for operation. This code is supposed to be available on the Robot website, but I can’t seem to locate it. Can anyone help me find this code? Thanks everyone for all the help you’ve given me in the past.
Hi,
I’m not sure if this file is available on the web. That file was the file I made in a hurry to Harry Mueller for his article about Xan’s code. I made that code because Harry needed a code that worked for a non-modified T7C. If you and other people are interested I could mail the file to Lynxmotion so that they could make it puplic on their “Projects and How-To’s”?
Hey not to step on anyone’s toe’s Zenta sent me his creation a while back and Ive been learning the code and modifying it also.
–Aaron
I think Zenta and I are the only 2 people here with RC phoenix’s
except I have a feeling his Phoenix is collecting dust now that he has his amazing A-Pod
I’d love to collaborate with you if you have any questions on the RC stuff if your in the US our time zone’s might be a bit more compatible!
I have modified the code that Zenta had sent me, from my understanding he modified the basic ps2 pheonix code and replaced the ps2 input commands with commands that read RC PWM signals and such.
I took it a step further and added my own little tidbits to the mix.
Is there an update on this? I would like to get away from the PSP controller eventually and use an RC controller.
Hello, Zenta:
Do you have any information for modifying the T7C for Phoenix operation? Does the Tx function better if modified in some way? I would need the code to go with a modified Tx also. Thanks for any info you may be willing to part with.
CefIII
The phoenix can operate with that remote with an UN- modified controller fine.
I believe he modified it to have more mode selection options, however I believe and have done quite a bit of different things with my remote without modifying it.
I didn’t modify mine as I might end up getting some other RC device in the future.
Basicly you can think about this before you modify your transmitter you have a 3 state switch a toggle switch, and a VR dial. Mr Zenta took that dial and made multiple unused switches in the remote position the VR channel into specific positions based off resistors.
See I find it funny how using the T7C for a robotic controller is weird unless your using the t7c mixer controls they are absolutely dead and unused… I would like to use the mixer controls for something but that’s another forum post and would only be really answered by t7c owners.
What I did in the code Zenta provided me was take the toggle switches channel 5 I believe which allows 3 modes “right off the bat” and used the momentary channel 7 switch to toggle modes within the 3 position switch, so you can have an infinitely large number of modes by toggling though them… This is why I did not modify my transmitter.
praelian <— I’m sure Zenta will post his code here for the masses, if your curious what I did send me a pm
Hope that helps. I’m curious what Zenta has done with that recently. Mines been collecting dust… but it looks really cool doing that :\
And honestly
Hi all,
I also had an RC version of a Hex working off an earlier version of Xans Code. The problem with the RC code is that it can eat up a lot of the processing power of your Basic Atom Pro, while it waits for pulsin commands to complete. The following code was setup for a Hitec 6 receiver. It used the 2 joysticks and 2 other channels. On my receiver the two other channels were a toggle switch and a rotary switch. This code replaced the PS2 calls. Please again note that this is from a much earlier version of Xans code:
[code];--------------------------------------------------------------------
; Initialization code for using the RC Receivers. In particular we are trying to find
; the zero trim point for the joysticks. We will not do the one we use for the height as it may not be
; at a zero point location…
;
RCInit:
pwmZero01 = 0
pwmZero02 = 0
;pwmZero03 = 0
pwmZero04 = 0
for RCtemp = 1 to 3
pulsin RCch01,0, pwmInput01
pwmZero01 = pwmZero01 + pwmInput01
;pulsin RCch03,0, pwmInput03 ; y height
pulsin RCch02,0, pwmInput02
pwmZero02 = pwmZero02 + pwmInput02
pulsin RCch04,0, pwmInput04
pwmZero04 = pwmZero04 + pwmInput04
next
pwmZero01 = pwmZero01/3
pwmZero02 = pwmZero02/3
pwmZero04 = pwmZero04/3
if ((pwmZero01 > 1400) and (pwmZero01 < 1600)) and ((pwmZero02 > 1400) and (pwmZero02 < 1600)) and ((pwmZero04 > 1400) and (pwmZero04 < 1600)) then
return
endif
; not valid zero point, Play a sound to let the user know, wait a little and try again.
sound P9,[100\4000, 100\4000, 100\4000]
pause 500
goto RCInit
;--------------------------------------------------------------------
;Read RC pulses and converts them to travellengths
;
RCInput:
pulsin RCch01,0, pwmInput01
pwmInput01 = (pwmInput01 min RCpwmMIN) max RCpwmMAX
if (pwmInput01 > (pwmZero01-10)) and (pwmInput01 < (pwmZero01+10)) then
pwmInput01 = pwmZero01
endif
pulsin RCch03,0, pwmInput03
pwmInput03 = (pwmInput03 min RCpwmMIN) max RCpwmMAX
BodyPosY = (pwmInput03 - 1300)/15
pulsin RCch05,0, pwmInput05
pwmInput05 = (pwmInput05 min RCpwmMIN) max RCpwmMAX
pulsin RCch02,0, pwmInput02
pwmInput02 = (pwmInput02 min RCpwmMIN) max RCpwmMAX
if (pwmInput02 > (pwmZero02-10)) and (pwmInput02 < (pwmZero02+10)) then
pwmInput02 = pwmZero02
endif
;BodyRotX = -(pwmInput02 - 1500)/15
pulsin RCch04,0, pwmInput04
pwmInput04 = (pwmInput04 min RCpwmMIN) max RCpwmMAX
if (pwmInput04 > (pwmZero04-10)) and (pwmInput04 < (pwmZero04+10)) then
pwmInput04 = pwmZero04
endif
pulsin RCch06,0, pwmInput06
pwmInput06 = (pwmInput06 min RCpwmMIN) max RCpwmMAX
' Depending on the position of Switch 5 we will other Move or just play with moving and rotating the body
if pwmInput05 < 1500 then
' Standard walk
TravelLengthX = -(pwmInput01 - pwmZero01)/4
TravelLengthZ =-(pwmInput02 - pwmZero02)/4
TravelRotationY = (pwmInput04 - pwmZero04)/14
' Choose gait mode by this switch.
if pwmInput06 < 1375 then
GaitMode = GAITMODE_RIP
elseif pwmInput06 < 1625
GaitMode = GAITMODE_TRI
else
GaitMode = GAITMODE_QUAD
endif
else
if pwmInput06 < 1500 then
BodyPosX = -(pwmInput01 - pwmZero01)/4
BodyPosZ = -(pwmInput02 - pwmZero02)/8
else
BodyRotX = (pwmInput02 - pwmZero02)/27
BodyRotY = (pwmInput04 - pwmZero04)/20
BodyRotZ = (pwmInput01 - pwmZero01)/27
endif
endif
'serout s_out, i9600, "BodyPosY=", sdec BodyPosY," TLZ=", sdec TravelLengthZ," TLX=", sdec TravelLengthX," TRY=", sdec TravelRotationY, 13]
'serout s_out, i9600, "BodyPosY=", sdec BodyPosY," BodyRotZ=", sdec BodyRotZ," BodyRotX=", sdec BodyRotX," TRY=", sdec TravelRotationY, 13]
'serout s_out, i9600, "CH01=", sdec pwmInput01," CH02=", sdec pwmInput02," CH03=", sdec pwmInput03," CH04=", sdec pwmInput04, " CH05=", sdec pwmInput05, " CH06=", sdec pwmInput06,13]
return [/code]
As I said this code is from an early version but you might be able to extract enough to get yourself up and running.
Later I tried to solve the delays and wrote an asembly language function to read in the RC on multiple channels in one pass, which was a lot quicker. You can find details of this up on several threads including: lynxmotion.net/phpbb/viewtopic.php?t=4751&start=18
I always meant to go back and modify my Hex to for the DIY RC receiver, but I was instead using it on my rover. Now I will wait until I finish my transforming the DIY remote to being XBee based and put on XBEE on my hex as well as the brat and rover.
Good Luck
Kurt
Hi,
I don’t like posting complete code on the forums. But since Harry mentioned this file in his article and Xan gave me permision to post it, I’m doing it.
This is the whole program based upon Xan’s v1.3. Credit to Xan:
Don’t use the GP part if you’ve not installed the GP firmware on the SSC32.
[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 GP player mode with the 3-state switch
;Use the two-state switch to toogle between the different gaits, movement or GPsequences
;In GPMode start GP player by moving the right stick upwards
;
;KNOWN BUGS:
; - None at the moment 
;
;====================================================================
;[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 P11 ;Output pin for (SSC32 RX) on BotBoard (Yellow)
SSC_IN con P10 ;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)
;--------------------------------------------------------------------
;[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
#ELSE ;Connector to the back
RFCoxaPin con P2 ;Front Right leg Hip Horizontal
RFFemurPin con P1 ;Front Right leg Hip Vertical
RFTibiaPin con P0 ;Front Right leg Knee
RMCoxaPin con P6 ;Middle Right leg Hip Horizontal
RMFemurPin con P5 ;Middle Right leg Hip Vertical
RMTibiaPin con P4 ;Middle Right leg Knee
RRCoxaPin con P10 ;Rear Right leg Hip Horizontal
RRFemurPin con P9 ;Rear Right leg Hip Vertical
RRTibiaPin con P8 ;Rear Right leg Knee
LFCoxaPin con P18 ;Front Left leg Hip Horizontal
LFFemurPin con P17 ;Front Left leg Hip Vertical
LFTibiaPin con P16 ;Front Left leg Knee
LMCoxaPin con P22 ;Middle Left leg Hip Horizontal
LMFemurPin con P21 ;Middle Left leg Hip Vertical
LMTibiaPin con P20 ;Middle Left leg Knee
LRCoxaPin con P26 ;Rear Left leg Hip Horizontal
LRFemurPin con P25 ;Rear Left leg Hip Vertical
LRTibiaPin con P24 ;Rear Left leg Knee
#ENDIF
;--------------------------------------------------------------------
;[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 113;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
;--------------------------------------------------------------------
;[GP player]
GPByte0 var Byte
GPByte1 var Byte
GPByte2 var Byte
GPByte3 var Byte
GPIsPlaying var bit
GPSeqToBePlayed var Byte
GPoccupied var bit
GPLastStep var Byte
GPSpeed var Byte
GPCounter var byte
;--------------------------------------------------------------------
;[RC Remote]
RCInput var word(7)
Buttons var byte
LastButtons var byte
StickMode var nib
Alive var byte
WalkMode var bit ;ch5 Mode switch (Switch E on Futaba T7C H)
MoveMode var bit
GPModeON var bit
TwoStateSW var bit ;ch 7 ,2-state switch
ToogleMovement 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
;GP mode
GPIsPlaying = FALSE
GPoccupied = TRUE
;Gait
GaitType = 0
BalanceMode = 0
LegLiftHeight = 50
GaitStep = 1
GOSUB GaitSelect
;Timer
WTIMERTICSPERMS con 2000; we have 16 clocks per ms and we are incrementing every 8 so divide again by 2
TCRW = 0x30 ;clears TCNT and sets the timer to inc clock cycle / 8
TMRW = 0x80 ;starts the timer counting
lTimerWOverflowCnt = 0
enable TIMERWINT_OVF
;SSC
SSCTime = 150
HexOn = True 'KÃ¥re turned it on…
;====================================================================
;[MAIN]
main:
'Start time
GOSUB GetCurrentTime], lTimerStart
;Read input
GOSUB RCInput1
'Check if we are in GPMode
if (GPModeON OR GPIsPlaying) = FALSE then
'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
serout s_out, i38400, [dec HexOn, " ", sdec bodyposy, " "]
IF(HexOn)THEN
;Wait for previous commands to be completed while walking
IF(ABS(TravelLengthX)>TravelDeadZone | ABS(TravelLengthZ)>TravelDeadZone | ABS(TravelRotationY*2)>TravelDeadZone) THEN
pause (PrevSSCTime - CycleTime -50) MIN 1 ; Min 1 ensures that there alway is a value in the pause command
IF(BalanceMode=0)THEN
SSCTime = NomGaitSpeed + (InputTimeDelay*2)
ELSE
SSCTime = NomGaitSpeed + (InputTimeDelay*2) + 100
ENDIF
ELSE
SSCTime = 200 ;NomGaitSpeed
ENDIF
GOSUB ServoDriver
;Turn the bot off
IF TurnOff THEN
;pause 200
;GOSUB FreeServos
HexOn=FALSE
ENDIF
ENDIF
'We are in GPMode:
else
gosub GPPlayer
pause 60
endif
'serout s_out, i38400, [sdec bodyposy, 13]
goto main
;====================================================================
;[ReadButtons] Reading input buttons from the ABB
ReadButtons:
input P4
input P5
input P6
prev_butA = butA
prev_butB = butB
prev_butC = butC
butA = IN4
butB = IN5
butC = IN6
return
;--------------------------------------------------------------------
;[WriteLEDs] Updates the state of the leds
WriteLEDs:
if ledA = 1 THEN
low p4
ENDIF
if ledB = 1 THEN
low p5
ENDIF
if ledC = 1 THEN
low p6
ENDIF
return
;--------------------------------------------------------------------
;[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)
WalkMode = False
MoveMode = False
GPModeON = False
BalanceMode = False
if RCInput(4) < 1100 then
WalkMode = True
elseif ((RCInput(4) > 1400) & (RCInput(4) < 1600))
MoveMode = True
else
GPModeON = True
endif
if RCInput(6) < 1500 then
TwoStateSW = False
else
TwoStateSW = True
endif
;***********************************************************************************
;*** Walking mode, toogle between the different walking gait with 2-state button ***
;***********************************************************************************
if WalkMode then
if RCInput(5) < 1750 then 'Select BalanceMode with VR-pot at slower speed
BalanceMode = TRUE
else
BalanceMode = FALSE
endif
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 = (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 MoveMode
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 = -(RCInput(0)-1500)/10
BodyPosZ = -(RCInput(1)-1500)/8
BodyRotY = -(RCInput(3)-1500)/20
else
'Body rotate:
BodyRotX = -(RCInput(1)-1500)/20
BodyRotY = -(RCInput(3)-1500)/20
BodyRotZ = (RCInput(0)-1500)/20
endif
;*************************************************************************************
;*** GP sequencer player mode choose between two different seq with 2-state button ***
;*************************************************************************************
elseif GPModeON
'GPoccupied = TRUE
if TwoStateSW then 'Use 2-state switch to toogle between every sequence
if GPIsPlaying = FALSE then
if GPSeqToBePlayed < 2 then
GPSeqToBePlayed = GPSeqToBePlayed +1
else
GPSeqToBePlayed = 0 'Selected GP sequence
endif
for GPCounter = 0 to GPSeqToBePlayed ' number of beeps = Sequence number to be played
pause 150
sound P9,[70\1200]
next
endif
endif
if RCInput(1) < 1400 then 'Start GP player by moving the right stick upwards
GPoccupied = FALSE
sound P9,[30\800,30\1000,30\1200,30\1400]
endif
GPSpeed = 100
endif
BodyPosY = (RCInput(2)-1000)/8 'adjust body height
;Calculate walking time delay
InputTimeDelay = 128 - (ABS((RCInput(1)-1500)/4) MIN ABS(((RCInput(0)-1500)/4))) MIN ABS(((RCInput(3)-1500)/6)) + (128 -(RCInput(5)-1010)/8)
'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]
;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
;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((IKFeetPosX*IKFeetPosX)+(IKFeetPosZ*IKFeetPosZ))))
;IKSW - Length between shoulder and wrist
IKSW = FSQRT(TOFLOAT(((IKFeetPosXZ-CoxaLength)*(IKFeetPosXZ-CoxaLength))+(IKFeetPosY*IKFeetPosY)))
;IKA1 - Angle between SW line and the ground in rad
GOSUB GetBoogTan [IKFeetPosXZ-CoxaLength, IKFeetPosY]
IKA1 = BoogTan
;IKA2 - ?
IKA2 = FACOS((TOFLOAT((FemurLength*FemurLength) - (TibiaLength*TibiaLength)) + (IKSW*IKSW)) / (TOFLOAT(2*Femurlength) * IKSW))
;IKFemurAngle
IKFemurAngle = (TOINT(((IKA1 + IKA2) * 180.0) / 3.141592)*-1)+90
;IKTibiaAngle
IKTibiaAngle = (90-TOINT(((FACOS((TOFLOAT((FemurLength*FemurLength) + (TibiaLength*TibiaLength)) - (IKSW*IKSW)) / TOFLOAT(2*Femurlength*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
;--------------------------------------------------------------------
;GP player
GPPlayer
if (GPIsPlaying = FALSE) & (GPoccupied = FALSE) then
serout SSC_OUT,SSC_BAUTE,“PL0 SQ”, dec GPSeqToBePlayed, " SM" , dec GPSpeed, " IX0 PA0 ONCE",13]
GPIsPlaying = TRUE
GPoccupied = TRUE 'This inhibit the player to play the same sequence several times
else
serout SSC_OUT,SSC_BAUTE,“QPL0”,13]
serin SSC_IN, SSC_BAUTE,[GPByte0,GPByte1,GPByte2,GPByte3]
;debug:
'serout S_OUT, i9600, "SQPlayed: “,dec GPByte0,” IX: “, dec GPByte2,” -> “,dec GPByte1,” Remaining time: ",dec GPByte3, 13]
IF GPByte0 = 255 THEN
GPIsPlaying = FALSE
endif
endif
return
;-----------------------------------------------------------------------------------
[/code]
Hi KÃ¥re,
My that is a lot of code! Thanks for posting it, I’ve been wanting to look at code for the T7C.
Alan KM6VV
P.S. Thanks Xan too!
from my experience with it, the only thing thats really been modified compared to the ps2 code is the input for the remote done with the pulsin command.
I believe Zenta figured out which order to have the channels polled so that the channels can be read the fastest… or it might have been kurte.
Either way, I have some other code also zenta helped me getting individual leg control working.
Thanks zenta!
–Aaron
Yeah, I found the PULSIN commands, and that’s where the action starts. I have an old R/C that was given to me, it might be fun to see what I can do with it. the T7C looks like it costs about $300 USD, if you can find it.
The reads are “interleaved”, much like the sector reads on a disk drive.
Alan KM6VV
Yeah I have a quick application i made to calibrate the receiver, I think zenta had shared me one he had built, although I made my own as this was at the infancy of learning RC stuff… It’s pretty simple now.
All the basic idea is was to sample each of your 7 channels and output them via serial on the bot board as everything was connected via serout and adjust the sliders to make sure all your values are 1500… with the T7C I found this to be not challenging at all, the remote is solid.
The other issue I found was you have to adjust your MAX and MIN values for each channel or the phoenix will walk fast then all of a sudden go really slow. I believe it was something like +500 and -500 from 1000-1500-2000 with the T7C you can go even further for more accuracy in your code although you have to change bits of code that work for you “as I found”
Either way just know your min middle and endpoints when you try and work with the values that pulsin reads if you have any accurate math trying to divide the numbers and get whatever values you want…
If you have any one of the messengers I could share what I have done with that code Zenta has provided me. “send me a pm” I’ll send you the link to an ebay store that has new transmitters also “unless anyone else wants it posted here?”
–Aaron
Hi Aaron,
Thanks for the useful information! I’m not sure the old R/C system I have is very good, but I can take a look sometime.
A little scaling goes a long way! Probably not that much range anyhow. A little filtering could make the positions a little more stable as well.
I’m not in any rush to get a new R/C, but it would be good to know where one could get one.
I found this link, but I don’t know anything about them:
hobbyrc2u.com/html/index.php?main_page=product_info&cPath=23_25&products_id=64
You might as well post the links here!
You can email me at the below address.
Alan KM6VV
I’ll email you sometime… I think what ive seen done in the rc stuff is if the number is larger then 1510 or smaller then 1490 then do something.
Ive found it real easy to get usable values out of RC stuff.
Jim above said there was something that was in the process of getting made to read the pulsin inputs and convert them to some other format that was quicker for the botboard to read…
Ha I suppose you could just use 2 botboards… I saw that done here once…
–Aaron
Hi Aaron,
Yeah, I’m thinking basically characterize the range of joysticks, and scale accordingly. One could even make the code “autoscale” for you. I’ve done that in the past for joysticks. Not hard.
Another board would work. I’m inclined to just make up a little PIC board and then synthesize a PS2 or similar packet for the data. Shouldn’t be hard either.
Alan KM6VV
Well if you can make a circuit board that converts PWM RC signals into something that the botboard can read with a small amount of cpu time vs the pulsin command I am all ears!
–Aaron
Sure!
Well it was Jim’s idea, so let’s see where he takes it and/or if he wants help on it. I’d simply use a PIC chip of the required size, and probably program it in either C or BasicMicro’s MBASIC.
The code would have to receive one request from the 'bot:
shiftout CMD,CLK,FASTLSBPRE,$1\8]
and transmit the data back to this command:
shiftin DAT,CLK,FASTLSBPOST,[DS2Mode\8]
And then there’s the “PS2 setup stuff” to arbitrate. More work!
The “development” work could be done with a BasicMicro PRO chip, and a BB2 easily enough.
With a board that small, I can mill it with my CNC’d Sherline mill.
With a button to “characterize the joysticks”, it could be a neat project.
Alan KM6VV
Well honestly if you have a homebrew circuit that could be had now “do it” I have never had any PIC programming experience but if the circuit to program a pic is something to be built I’d like that.
I’m sure others would do… Any way to really speed up the reading of pulsin channels I’d certainly get on it and modify the RC code to accept it, Ive had my fair share of tweaking with it and learning how it works, although I know about zilch when it comes to the IK portion… I know when I mess with it …it doesn’t work anymore!..
It was tempting to buy another botboard and atom to simply read the pulsin commands. Have at it man If you could code it id build it to try the code.
–Aaron
I’ve built PIC boards to run stepper motors and DC servo motors; and not much is really needed on this board. Just plug in the receiver channels, and output SPI to the 'bot BB2. The board can even get 5v power from the BB2.
The trick would be reading PULSEIN, and sending SEROUT at closely interleaved timing to reduce the latency.
Perhaps Jim will weigh in on this!
Alan KM6VV
well… The idea would be have the pic somehow buffer the reads so when the botboard 2 requested data the pic would instantly reply… and I suppose my definition of instant would be less then 20ms as that’s the width of a single channel…
Kurte Explained this to me once and the exact way this happens has slipped my mind at the moment, although it was something like 140ms to read all 7 channels so the botboard would update for instance, channel 1-7 approximately 7.14 times a second 1000ms / 140ms(7x140ms)
Kurte Chime in on this one if you could.
I think thats right…
So even if the botboard pulled late data it wouldn’t be waiting around for what seems like eternity getting updated data… I honestly don’t think it would make much of a difference getting data that was 140ms old or older as it wouldn’t…
I have compared the orignal ps2 code Xan released with the RC code above and it is very apparent that you can see physical slowdowns in the gait while the phoenix walks around… Now since Xan was doing some pretty cool things with the 1.3 ps2 code like a Lookup table its quite possible that so many cpu cycles are free’ed up in the process that using pulsin might just not cause any gait issues… but I did remember at one point one of the people here said that with the 1.2 ps2 code the atom pro cpu was nearing 100% usage.
And exhale.
–Aaron