Futaba T7C Phoenix 1.3/2.0 code

Lynxmotion Legged Robots
21

Well as the accuracy question was dropped, I’m kinda going to go off on a limb… I mean if pulsin returned a value of 1500 when the stick is centered and your assembly returns a value of 1500 when centered i think…its close enough for government work as they say.

I honestly would like to get the 1.3 code working with your assembly snippet or some other means so later when I dive into adding the “assembly code” or “some code to read RC inputs” to the 2.0 code I know I wont be running into oddities like this which is probably really unexpected as it seems like these little RC receivers seem to spit out pwm data at different times.

So as for your accuracy question Kurte… not really sure?.. I see 900-2100 values 1500 centered… looks peachy for me.

It would be sweet if this assembly code was included as a function within the basic micro studio!

As for going back to pulsin commands respectfully that will not work, the bot was really bad the gait was noticeably pausing between movements it was just bad. although using the pulsin commands with the 2.0 code might work as there are lookup tables and not complicated math involved so it might work… although seeing how much faster your asm code reads these inputs, I think Id have more luck using it then going backwards.

–Aaron

I’ll try the code you posted and see if it works better/worse/same and let you know

22

Hi Aaron,

I think I know how I could code it up reasonably to handle your case. It would be along the lines I mentioned in the previous post. Yes it would be great if it became a command or the like, but I am not holding my breath.

I had a thought this morning. I really wonder how bad it would be if you did not get updates for every thing every pass through the inputs. if maybe a few of the fields only get updated on every other pass. You might try something like:

bFutabaHack var byte RCInput1: ;-------------------------Begin Crazyness------------------------------------------------ #ifdef USETIMERA disable timeraint #endif PMR5 = 0 PCR5 = 0 mov.b @BFUTABAHACK:16, r0l ; load variable to do different mask on even/odd calls beq _FUTAB1:8 ; mov.b #0x5f, r1l ; Do bits 0,1,2,3,4,6 bra _FUTAB2:8 _FUTAB1: mov.b #0x3e, r1l ; Do bits 1,2,3,4,5 _FUTAB2: xor.b #0xff, r0l ; ; setup for the next pass to use the other mask mov.b r0l, @BFUTABAHACK:16 ; save it away mov.l #250000,er2 _PI7_WAIT_FOR_ALL_LOW: ...

It might work fine enough…

Kurt

23

Just an update, I haven’t had a chance to test this yet! I will soon.

–Aaron

24

Howdy Kurte,

OK So I took a look at what you typed, it appears as if you are just replacing the top part of your assembly code with some “more assembly” is this correct?

but it seems as if your adding something else @BFUTABAHACK and also the bFutabaHack var byte

So I guess should I just add that var byte to my variables list at the top and paste over that new assembly code?

It appears it just overwrites

PMR5 = 0 PCR5 = 0 mov.b #0x7f, r1l mov.l #250000,er2 _PI7_WAIT_FOR_ALL_LOW:

and adds multiple extra lines…

–Aaron

25

Hey everyone I have the Itch again to start poking around with this robotics money pit.

As it stands my goal is to take the phoenix 1.3 code and add individual leg control along with Kurte’s assembly routines so that the AtomPro will read RC control inputs and allow smooth walking without the long pauses between the seven pulsin commands due to the complicated nature of the basic atom reading a PWM input.

For another project of mine I’m trying to do research in and my lack of really cool testing bits, I had picked up one of these http://dangerousprototypes.com.php5-11.dfw1-1.websitetestlink.com/wp-content/media/2010/02/ols-cover.jpg Its 45 bucks and an open source logic analyzer. Kurte had posted a photo of the output of a spektrum receiver, I’m not sure they all output the same method so since I have this logic sniffer on order I’ll hook it up and post the results of my T7C receiver

I understand in today’s technology PWM sent via RC transmitter isn’t exactly the best method for robotics… Xbee’s send serial data at much faster speeds and two way communication is also a great option, as well as bluetooth open source platforms. However I was always thinking it would be kinda interesting to see an inexpensive device that would be developed for converting PWM into serial on some small compact board so you can use xyz rc channel inputs and they would be output via 2 wires to your project.

So once I get this 1.3 code working with the RC I think I better take a stab at the 2.0 code then perhaps use this gyro I have had sitting on my shelf for the better part of a year, although I’m not sure how to code with this thing… I understand how the device works as I had experimented with it by monitoring the voltage values over serial…

Hopefully everyone will get some updates in this thread if anyone is interested in this feel free to say hello.

–Aaron

26

Hi kurte, I tried your response from last year in here a couple posts above and overwrote some code this is what I tried to compile

[code] RCInput1:
;-------------------------Begin Crazyness------------------------------------------------
asm {
#ifdef USETIMERA
disable timeraint
#endif
PMR5 = 0
PCR5 = 0
mov.b @BFUTABAHACK:16, r0l ; load variable to do different mask on even/odd calls
beq _FUTAB1:8 ;
mov.b #0x5f, r1l ; Do bits 0,1,2,3,4,6
bra _FUTAB2:8
_FUTAB1:
mov.b #0x3e, r1l ; Do bits 1,2,3,4,5
_FUTAB2:
xor.b #0xff, r0l ; ; setup for the next pass to use the other mask
mov.b r0l, @BFUTABAHACK:16 ; save it away
mov.l #250000,er2
_PI7_WAIT_FOR_ALL_LOW:

   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
}[/code]

However I get an error message
c:\users\root\documents\phoenix\rc_1.3_test10.asm: Assembler messages:
c:\users\root\documents\phoenix\rc_1.3_test10.asm:2503: Error: unknown opcode
C:\Program Files (x86)\BasicMicro\bin\ld.exe : cannot open c:\users\root\documents\phoenix\rc_1.3_test10.o: No such file or directory

I’d love to learn assembly! however I feel blind in helping.

–Aaron

27

Not sure how to help here. This is obviously not all of what you tried to compile as it errored on line 2503… It also does not have any of the external variables defined such as BPULSETIMEOUT… Need full thing if we are to compile.

Kurt

28

Hey there,

[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------------------------------------------------
asm {

#ifdef USETIMERA
disable timeraint
#endif
PMR5 = 0
PCR5 = 0
mov.b @BFUTABAHACK:16, r0l ; load variable to do different mask on even/odd calls
beq _FUTAB1:8 ;
mov.b #0x5f, r1l ; Do bits 0,1,2,3,4,6
bra _FUTAB2:8
_FUTAB1:
mov.b #0x3e, r1l ; Do bits 1,2,3,4,5
_FUTAB2:
xor.b #0xff, r0l ; ; setup for the next pass to use the other mask
mov.b r0l, @BFUTABAHACK:16 ; save it away
mov.l #250000,er2
_PI7_WAIT_FOR_ALL_LOW:

   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((BoogTan*180.0) / 3.141592)
  gosub GetBoogTan [TotalX, TotalY]
  TotalZbal = TotalZbal + TOINT((BoogTan*180.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)*CosA*CosB - TOFLOAT(TotalZ)*CosB*SinA + TOFLOAT(PosY)*SinB)
  BodyIKPosZ = TotalZ-TOINT(TOFLOAT(TotalX)*CosG*SinA + TOFLOAT(TotalX)*CosA*SinB*SinG +TOFLOAT(TotalZ)*CosA*CosG-TOFLOAT(TotalZ)*SinA*SinB*SinG-TOFLOAT(PosY)*CosB*SinG)
  BodyIKPosY = PosY - TOINT(TOFLOAT(TotalX)*SinA*SinG - TOFLOAT(TotalX)*CosA*CosG*SinB + TOFLOAT(TotalZ)*CosA*SinG + TOFLOAT(TotalZ)*CosG*SinA*SinB + TOFLOAT(PosY)*CosB*CosG)

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 <CR>
  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
asm {
  push.w   r1                        ; save away register we will use
  bclr #7,@TSRW:8                     ; clear the overflow bit in the Timer status word
  mov.w @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     
ENDASMSUB
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]

SO I verified the code I had was what I last had on my Phoenix before adding your snippit, however the interesting thing is, there is no line 2503.

–Aaron

29

Actually there is a line 2503 in the generated ASM file. If you double-clicked on the error message it would have opened up the ASM file and but the cursor at the issue. The problem was that you put a BASIC statement inside of a ASM {} block…

Look for:

asm { #ifdef USETIMERA disable timeraint #endif
Move the ASM block after the #endif

Then if you try to compile it, you will find that it complains about something is not defined… You need to add a data variable, probably up with the other variables…

    bFutabaHack			var	byte

Then it compiles…

30

Thanks I will try this when I get home and see if the edits you have made correct the channel 5 overlap issue!
I can’t wait.

Thanks for your help.

–Aaron

31

There are better ways to attach long files.

Then possibly highlight the lines of interest in snips like in Kurt’s post above as well.

Alan KM6VV

32

Hi Kurte,

I have tried the edits you provided the code does compile however, I can’t really describe it but the phoenix just jerks around and does not respond to transmitter commands,

Anything I can assist with to figure out why the assembly does not function?

Thanks,
–Aaron

33

Try writing a quick and dirty program that uses it and after each pass through, see if any of the values have changed, if so print out the current values. Then you can easily see if moving the joysticks and the like is updating anything and hopefully the right locations…

Kurt

34

Hi Kurt,

I’ll do this when I get home from work.

–Aaron

35

Hey kurt, I had some assembly code you had given me previously to test the outputs, well every output is zero for some reason

[code]
counted var word
counted = 0
awPulsesIn var word(7)
bPulseTimeout var byte
bFutabaHack var byte

top
Gosub ReadInputs:

counted = counted+1

serout s_out,i57600,[dec awPulsesIn(0)," “,dec awPulsesIn(1),” “,dec awPulsesIn(2),” “,dec awPulsesIn(3),” “,dec awPulsesIn(4),” “,dec awPulsesIn(5),” “,dec awPulsesIn(6),” ",dec counted,13]
;pause 100

goto top

ReadInputs:
asm {
PMR5 = 0
PCR5 = 0
mov.b @BFUTABAHACK:16, r0l ; load variable to do different mask on even/odd calls
beq _FUTAB1:8 ;
mov.b #0x5f, r1l ; Do bits 0,1,2,3,4,6
bra _FUTAB2:8
_FUTAB1:
mov.b #0x3e, r1l ; Do bits 1,2,3,4,5
_FUTAB2:
xor.b #0xff, r0l ; ; setup for the next pass to use the other mask
mov.b r0l, @BFUTABAHACK:16 ; save it away
mov.l #250000,er2
_PI7_WAIT_FOR_ALL_LOW:

   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
}
Return[/code]

This is the output

0 0 0 0 0 0 0 1256
0 0 0 0 0 0 0 1257
0 0 0 0 0 0 0 1258
0 0 0 0 0 0 0 1259
0 0 0 0 0 0 0 1260
0 0 0 0 0 0 0 1261

1256-1261 is the number of times the loop has ran.

It might be helpful if it was outputting something, but no.

what’s the plan stan?

–Aaron

36

I am taking a quick look… But for a quick test you may want to add at the start:

input p0
input p1
input p2
input p3
input p4
input p5
input p6

Just to make sure that all of the IO pins are properly set up for input. Also My assumption here is that you have the receiver setup on pins 0-6 of the BAP and the receiver is properly powered.

Kurt

37

I’ll add your info.

You are correct on the 0-6 pin assignments as well properly powered. I like the R617FS receiver for my T7c it shows a nice little green LED when the transmitter is on.

Went ahead and added those edits, did not work.
mov.l #250000,er2 <— is the 250000 micro seconds to wait? … guess here…

38

So far I see that your version of the code is missing some lines. If I go back to page 1 of this thread and look at a file I uploaded they are there…

In particular, the assembly language function:

[code];[RCInput] reads the input data from the RC-Remote and processes the
;data to the parameters.
bFutabaHack var byte
RCInput1:
;-------------------------Begin Crazyness------------------------------------------------
#ifdef USETIMERA
disable timeraint
#endif
asm {
PMR5 = 0
PCR5 = 0
mov.b @BFUTABAHACK:16, r0l ; load variable to do different mask on even/odd calls
beq _FUTAB1:8 ;
mov.b #0x5f, r1l ; Do bits 0,1,2,3,4,6
bra _FUTAB2:8
_FUTAB1:
mov.b #0x3c, r1l ; Do bits 2,3,4,5
_FUTAB2:
xor.b #0xff, r0l ; ; setup for the next pass to use the other mask
mov.b r0l, @BFUTABAHACK:16 ; save it away
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
}
#ifdef USETIMERA
enable timeraint
#endif
[/code]
If you look at the version in your test program, if you look at the label: _PI7_WAIT_FOR_ALL_LOW:
Notice the next line simply branches to the end and returns nothing… Whereas the version I just pasted here checks the status to see if any input changed…

Kurt

39

The Phoenix is a bit shaky now, I think my NIMH batteries are old… need to figure that out, or perhaps use what Xan or Zenta did for their phoenix… Lipo baddass battery!

I think your code edit just might have worked, I need to charge this battery again, but using your code and the test program I wrote the outputs appear to be 100% correct!
So I thank you with your amazing Assembly skills I will let you know the outcome.

Thanks Again,

–Aaron

40

Hey Kurt,

So It appears my battery on my bot is worse then I thought, it doesn’t last 2 minutes. But that was enough time to poke around with it and make sure the code was doing what it was suppose to do.

On the T7C I have a toggle switch which happens to be awPulsesIn(6) “Channel 7” So prior to the assembly code pressing this switch would cause a tone to appear and the gait to change from I believe it was 1 of 7 gaits which audibly output a different tone per which gait you were on. So here’s the problem the code runs I press the toggle switch nothing happens I press it again then the gait switches twice very quickly, It’s almost like a normal push button and needs denounce code in it. One way around this I could simply add some code to make it so two button presses = 1 button press???

Maybe I am going about this all wrong and need to get the 2.0 code working with your assembly routines to avoid the calculations… What are your thoughts on this Kurt? or anyone…

I understand that I’m a unique case for this since not a lot of people use RC, as the Xbee and other serial/Bluetooth/ps2 controller options require much much less cpu power to read inputs. Maybe a 2nd BotBoard strapped on top? although not sure if this is really required… I’m all ears people.

–Aaron

And many Thanks Kurt for your help.