MSR-H01 Hexapod, using ABB2 with ATOM 28 Pro, SSC32

Post code examples here

MSR-H01 Hexapod, using ABB2 with ATOM 28 Pro, SSC32

Postby innerbreed » Sat Sep 19, 2009 9:20 pm

Here you will find Code to control your MSR-H01 hexapod when using BotBoardII with BasicAtom Pro28, and SSC-32!
This is a modified code for the Lynxmotion Phoenix hexapod originally programmed by Jeroen Janssen (aka Xan).
Image
Image
This code is still in development for my current project but if things take a big change i will throw in a few updates.


The analog sticks not only controls the step length but also the speed of the gait.
Improved gait engine/speed/body rotations X-Z-Y/SSC communication/SSC V2 registers.
A universal gait engine which makes it possible to simply add new gait types by setting properties.
The analog sticks not only controls the step length but also the speed of the gait.
Selective Balance gesture control on all gaits
A internal timer measures the calculation time of each step. This makes it possible to get a perfect timing between the BB2 and the SSC.
The offsets of the servos are moved to the SSC V2 register.

8 different gaits: Thanks to Zenta
Gaits are: *due to updates some of these gaits have been removed from code.
- 6 steps ripple
- 12 steps ripple
- 9 steps quadripple
- 4 steps tripod
- 6 steps tripod
- 8 steps tripod
- 12 steps wave
- 18 steps wave

Improved body rotations:
Thanks to Zenta
The improved body calculations by Zenta are implemented
Internal timer to improve SSC communication:
A internal timer measures the calculation time of each step. This makes it possible to get a perfect timing between the BB2 and the SSC. This makes the gaits very stable.
Turn on/off the bot with the remote:
Press the start button on the PS2 remote to turn on and off the bot.
Servo offsets moved to SSC V2 registers:
The offsets of the servos are moved to the SSC V2 register. This way there is no need to change the offsets in case of a new version.
CODE VERSION 1.0 Still under development
Code: Select all
;Project: MSR-H01 Hexapod using Lynxmotion Phoenix code 1.3
;Description: MSR-H01 Hexapod, controlled by ABB2 with ATOM 28 Pro, SSC32 V2, PS2 Wireless control

;Programmer: Jeroen Janssen (aka Xan)
;Developer: Jonny Poole (aka Innerbreed)

;PS2 CONTROLS:
;   - Start         Turn on/off the bot
;   - select      Switch gaits
;   - Left Stick   Walk/Strafe
;   - Right Stick   Rotate
;   - D-Pad up      Body up manual control
;   - D-Pad down   Body down manual control
;   - Triangle      Move body to (walk pos)
;   - Circle      Move body to (crouch pos)
;   - Square        Switch Balance mode on or off
;   - Cross         Attack Mode Sequence
;   - L1 & L. Stick   Shift body X/Z
;   - L1 & R. Stick Shift body Y and rotate body Y
;   - L2 & Sticks   Rotate body X/Y/Z
;   - L3            *non-defined*
;   - R1 & L. Stick   Moves Head X/Z
;   - R2 & L. Stick Double gait travel length
;   - R3            *non-defined*
;   
;====================================================================
;[CONSTANDS]
TRUE       con 1
FALSE       con 0

BUTTON_DOWN con 0
BUTTON_UP    con 1
;--------------------------------------------------------------------
;[SERIAL CONNECTIONS]
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
;--------------------------------------------------------------------
;[PS2 Controller]
PS2DAT       con P12      ;PS2 Controller DAT (Brown)
PS2CMD       con P13      ;PS2 controller CMD (Orange)
PS2SEL       con P14      ;PS2 Controller SEL (Blue)
PS2CLK       con P15      ;PS2 Controller CLK (White)
PadMode    con $79
;--------------------------------------------------------------------
;[PIN NUMBERS]

  RRCoxaPin    con P12   ;Rear Right leg Hip Horizontal
  RRFemurPin    con P13   ;Rear Right leg Hip Vertical
  RRTibiaPin     con P14   ;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 P0   ;Front Right leg Hip Horizontal
  RFFemurPin    con P1   ;Front Right leg Hip Vertical
  RFTibiaPin     con P2   ;Front Right leg Knee

  LRCoxaPin    con P28   ;Rear Left leg Hip Horizontal
  LRFemurPin    con P29   ;Rear Left leg Hip Vertical
  LRTibiaPin     con P30   ;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 P16   ;Front Left leg Hip Horizontal
  LFFemurPin    con P17   ;Front Left leg Hip Vertical
  LFTibiaPin     con P18   ;Front Left leg Knee
 
  HeadPan       con P19
  HeadTilt      con P3
;--------------------------------------------------------------------
;[MIN/MAX ANGLES]
RRCoxa_MIN   con -45      ;Mechanical limits of the Right Rear Leg
RRCoxa_MAX   con 45
RRFemur_MIN   con -80
RRFemur_MAX   con 80
RRTibia_MIN   con -70
RRTibia_MAX   con 70

RMCoxa_MIN   con -45      ;Mechanical limits of the Right Middle Leg
RMCoxa_MAX   con 45
RMFemur_MIN   con -80
RMFemur_MAX   con 80
RMTibia_MIN   con -70
RMTibia_MAX   con 70

RFCoxa_MIN   con -10      ;Mechanical limits of the Right Front Leg
RFCoxa_MAX   con 40
RFFemur_MIN   con -80
RFFemur_MAX   con 80
RFTibia_MIN   con -70
RFTibia_MAX   con 70

LRCoxa_MIN   con -45      ;Mechanical limits of the Left Rear Leg
LRCoxa_MAX   con 45
LRFemur_MIN   con -80
LRFemur_MAX   con 80
LRTibia_MIN   con -70
LRTibia_MAX   con 70

LMCoxa_MIN   con -45      ;Mechanical limits of the Left Middle Leg
LMCoxa_MAX   con 45
LMFemur_MIN   con -80
LMFemur_MAX   con 80
LMTibia_MIN   con -70
LMTibia_MAX   con 70

LFCoxa_MIN   con -10      ;Mechanical limits of the Left Front Leg
LFCoxa_MAX   con 40
LFFemur_MIN   con -80
LFFemur_MAX   con 80
LFTibia_MIN   con -70
LFTibia_MAX   con 70

HeadPan_MAX con 20
HeadPan_MIN con -20

HeadTilt_MAX con 20
HeadTilt_MIN con -30
;--------------------------------------------------------------------
;[BODY DIMENSIONS] [mm]
CoxaLength  con 20      ;Length of the Coxa [mm]
FemurLength con 80      ;Length of the Femur [mm]
TibiaLength con 130      ;Lenght of the Tibia [mm]

Panlength   con 10
Tiltlength  con 40

CoxaAngle con 30          ;Default Coxa setup angle / Was 60

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

RMOffsetX con -60      ;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 -40      ;Distance X from center of the body to the Right Rear coxa
RROffsetZ con 80      ;Distance Z from center of the body to the Right Rear coxa

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

LMOffsetX con 60      ;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 40      ;Distance X from center of the body to the Left Rear coxa
LROffsetZ con 80      ;Distance Z from center of the body to the Left Rear coxa

PanOffsetX    con 0     ;Distance X from center of the body to the head
PanOffsetZ    con 110   ;Distance Z from center of the body to the head

TiltOffsetX   con 130   ;Distance X from center of the body to the head
TiltffsetZ    con 0     ;Distance Z from center of the body to the head

;--------------------------------------------------------------------
;[REMOTE]
TravelDeadZone   con 9   ;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

HeadPanAngle   var sword   ;Actual Angle of the Head pan
HeadTiltAngle   var sword   ;Actual Angle of the Head tilt
;--------------------------------------------------------------------
;[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
BodyPosXZ       var sword
;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
;--------------------------------------------------------------------
;[Ps2 Controller]
DualShock    var Byte(7)
LastButton    var Byte(2)
DS2Mode    var Byte
PS2Index   var byte
BodyYShift   var sbyte
;--------------------------------------------------------------------
;[TIMING]
lTimerWOverflowCnt   var long    ;used in WTimer overflow. Will keep a 16 bit overflow so we have a 32 bit timer
lCurrentTime      var long   
lTimerStart         var long   ;Start time of the calculation cycles
lTimerEnd         var long    ;End time of the calculation cycles
CycleTime         var byte   ;Total Cycle time

SSCTime           var word   ;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
;--------------------------------------------------------------------
;[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
GaitSpeed      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
TravelLengthXZ  Var sword   ;Current Travel length X/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 [mm]
RFPosX = 78      ;Start positions of the Right Front leg
RFPosY = 0
RFPosZ = -45

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

RRPosX = 78      ;Start positions of the Right Rear leg
RRPosY = 0
RRPosZ = 45

LFPosX = 78      ;Start positions of the Left Front leg
LFPosY = 0
LFPosZ = -45

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

LRPosX = 78      ;Start positions of the Left Rear leg
LRPosY = 0
LRPosZ = 45

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

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

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

;Timer
WTIMERTICSPERMS con 2000; we have 16 clocks per ms and we are incrementing every 8 so divide again by 2
TCRW = 0x30          ;clears TCNT and sets the timer to inc clock cycle / 8
TMRW = 0x80          ;starts the timer counting
lTimerWOverflowCnt = 0
enable TIMERWINT_OVF

;PS2 controller
high PS2CLK
LastButton(0) = 255
LastButton(1) = 255
BodyYShift = 0

;SSC
SSCTime = 150
HexOn = False
;====================================================================
;[MAIN]   
main:
  'Start time
  GOSUB GetCurrentTime[], lTimerStart
     
  '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+BodyYShift+RFGaitPosY, RFOffsetX, RFOffsetZ, RFGaitRotY]
  GOSUB LegIK [RFPosX-BodyPosX+BodyIKPosX-RFGaitPosX, RFPosY+BodyPosY+BodyYShift-BodyIKPosY+RFGaitPosY, RFPosZ+BodyPosZ-BodyIKPosZ+RFGaitPosZ]   
  RFCoxaAngle  = IKCoxaAngle + CoxaAngle ;Angle for the basic setup for the front leg   
  RFFemurAngle = IKFemurAngle
  RFTibiaAngle = IKTibiaAngle
   
  ;Right Middle leg
  GOSUB BodyIK [-RMPosX+BodyPosX+RMGaitPosX, RMPosZ+BodyPosZ+RMGaitPosZ,RMPosY+BodyPosY+BodyYShift+RMGaitPosY, RMOffsetX, RMOffsetZ, RMGaitRotY]
  GOSUB LegIK [RMPosX-BodyPosX+BodyIKPosX-RMGaitPosX, RMPosY+BodyPosY+BodyYShift-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+BodyYShift+RRGaitPosY, RROffsetX, RROffsetZ, RRGaitRotY]
  GOSUB LegIK [RRPosX-BodyPosX+BodyIKPosX-RRGaitPosX, RRPosY+BodyPosY+BodyYShift-BodyIKPosY+RRGaitPosY, RRPosZ+BodyPosZ-BodyIKPosZ+RRGaitPosZ]
  RRCoxaAngle  = IKCoxaAngle - CoxaAngle ;Angle for the basic setup for the front leg   
  RRFemurAngle = IKFemurAngle
  RRTibiaAngle = IKTibiaAngle

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

  ;Left Middle leg
  GOSUB BodyIK [LMPosX-BodyPosX+LMGaitPosX, LMPosZ+BodyPosZ+LMGaitPosZ,LMPosY+BodyPosY+BodyYShift+LMGaitPosY, LMOffsetX, LMOffsetZ, LMGaitRotY]
  GOSUB LegIK [LMPosX+BodyPosX-BodyIKPosX+LMGaitPosX, LMPosY+BodyPosY+BodyYShift-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+BodyYShift+LRGaitPosY, LROffsetX, LROffsetZ, LRGaitRotY]
  GOSUB LegIK [LRPosX+BodyPosX-BodyIKPosX+LRGaitPosX, LRPosY+BodyPosY+BodyYShift-BodyIKPosY+LRGaitPosY, LRPosZ+BodyPosZ-BodyIKPosZ+LRGaitPosZ]
  LRCoxaAngle  = IKCoxaAngle - CoxaAngle ;Angle for the basic setup for the front leg   
  LRFemurAngle = IKFemurAngle
  LRTibiaAngle = IKTibiaAngle
 
  GOSUB HeadTracking
  GOSUB CheckAngles

  LedC = IKSolutionWarning
  LedA = IKSolutionError

  ;Read input
  GOSUB Ps2Input
  ;GOSUB ReadButtons   ;I/O used by the PS2 remote
  ;GOSUB WriteLeds   ;I/O used by the PS2 remote

  ;Get endtime and calculate wait time
  GOSUB GetCurrentTime[], lTimerEnd   
  CycleTime = (lTimerEnd-lTimerStart)/WTIMERTICSPERMS
   
  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 = GaitSpeed + (InputTimeDelay*2)
     ELSE
        SSCTime = GaitSpeed + (InputTimeDelay*2) + 100
     ENDIF
    
   ELSE
     SSCTime = 200 ;GaitSpeed
     ENDIF
        
   GOSUB ServoDriver
  ELSE
    ;Turn off
   GOSUB FreeServos 
  ENDIF   
goto main
;-------------------------------------------------------------------
;[Head Tracking]
  HeadTracking:

  ;Return to the middle position
;  HeadPanAngle=0
;IF (ABS(TravelLengthX)>TravelDeadZone | ABS(TravelLengthZ)>TravelDeadZone | ABS(TravelRotationY*2)>TravelDeadZone) THEN
    ;Calculate walking direction X and Z
;    TravelLengthXZ = SQR((TravelLengthX * TravelLengthX) + TravelLengthZ * TravelLengthZ)
;    HeadPanAngle = TOINT(FACOS(TOFLOAT(TravelLengthZ) / TOFLOAT(TravelLengthXZ)) * 180.0 / 3.141592)-180
    ;Add sign depending on the direction of X
;    HeadPanAngle = HeadPanAngle * (TravelLengthX/ABS(TravelLengthX))
;  ENDIF
  ;Calculate body angle depending on rotation
;  IF ABS(TravelRotationY*2)>TravelDeadZone & ABS(TravelRotationY*3) > ABS(HeadPanAngle) THEN
;    HeadPanAngle = -TravelRotationY*3 ; Rotation max = 16*6 to get max range of 90 deg.   
;  ENDIF
 
  ;Return to the middle position
     HeadPanAngle=0
IF (ABS(BodyRotY)>TravelDeadZone | ABS(BodyRotY)>TravelDeadZone | ABS(TravelRotationY*2)>TravelDeadZone) THEN
    ;Calculate direction Z and X
    HeadPanAngle = SQR((BodyRotY * BodyRotY) + BodyYShift * BodyYShift)
     
    ;Add sign depending on the direction of X
    HeadPanAngle = HeadPanAngle * (BodyRotY/ABS(BodyRotY))
  ENDIF
  ;Calculate body angle depending on rotation
  IF ABS(TravelRotationY*2)>TravelDeadZone & ABS(TravelRotationY*3) > ABS(HeadPanAngle) THEN
    HeadPanAngle = -TravelRotationY*3 ; Rotation max = 16*6 to get max range of 90 deg.   
  ENDIF
;----------------
   ;Return to the middle position
;   HeadTiltAngle=0
;IF (ABS(TravelLengthZ)>TravelDeadZone | ABS(TravelLengthX)>TravelDeadZone | ABS(TravelRotationY*2)>TravelDeadZone) THEN
    ;Calculate walking direction Z and X
;    TravelLengthXZ = SQR((TravelLengthZ * TravelLengthZ) + TravelLengthX * TravelLengthX)
;    HeadTiltAngle = TOINT(FACOS(TOFLOAT(TravelLengthX) / TOFLOAT(TravelLengthXZ)) * 180.0 / 3.141592)-180
    ;Add sign depending on the direction of Z
;    HeadTiltAngle = HeadTiltAngle * (TravelLengthZ/ABS(TravelLengthZ))
;  ENDIF
  ;Calculate body angle depending on rotation
;  IF ABS(TravelRotationY*2)>TravelDeadZone & ABS(TravelRotationY*3) > ABS(HeadTiltAngle) THEN
;    HeadTiltAngle = -TravelRotationY*3 ; Rotation max = 16*6 to get max range of 90 deg.   
;  ENDIF

;Return to the middle position
   HeadTiltAngle=0
IF (ABS(BodyYShift)>TravelDeadZone | ABS(BodyRotY)>TravelDeadZone | ABS(TravelRotationY*2)>TravelDeadZone) THEN
    ;Calculate direction Z and X
    HeadTiltAngle = SQR((BodyRotY * BodyRotY) + BodyYShift * BodyYShift)
     
    ;Add sign depending on the direction of Z
    HeadTiltAngle = HeadTiltAngle * (BodyYShift/ABS(BodyYShift))
  ENDIF
  ;Calculate body angle depending on rotation
  IF ABS(TravelRotationY*2)>TravelDeadZone & ABS(TravelRotationY*3) > ABS(HeadTiltAngle) THEN
    HeadTiltAngle = -TravelRotationY*3 ; Rotation max = 16*6 to get max range of 90 deg.   
  ENDIF

return
 
;====================================================================
;[ReadButtons] Reading input buttons from the ABB
ReadButtons:
  input P4
  input P5
  input P6
   
  prev_butA = butA
  prev_butB = butB
  prev_butC = butC
   
  butA = IN4
  butB = IN5
  butC = IN6
return
;--------------------------------------------------------------------
;[WriteLEDs] Updates the state of the leds
WriteLEDs:
  if ledA = 1 THEN
   low p4
  ENDIF
  if ledB = 1 THEN
   low p5
  ENDIF
  if ledC = 1 THEN
   low p6
  ENDIF
return
;--------------------------------------------------------------------
;[PS2Input] reads the input data from the Wiiremote and processes the
;data to the parameters.
Ps2Input:
   
  low PS2SEL
  shiftout PS2CMD,PS2CLK,FASTLSBPRE,[$1\8]
  shiftin PS2DAT,PS2CLK,FASTLSBPOST,[DS2Mode\8]
  high PS2SEL
  pause 1

  low PS2SEL
  shiftout PS2CMD,PS2CLK,FASTLSBPRE,[$1\8,$42\8]   
  shiftin PS2DAT,PS2CLK,FASTLSBPOST,[DualShock(0)\8, DualShock(1)\8, DualShock(2)\8, DualShock(3)\8, |
     DualShock(4)\8, DualShock(5)\8, DualShock(6)\8]
  high PS2SEL
  pause 1   

  if DS2Mode <> PadMode THEN
   low PS2SEL
   shiftout PS2CMD,PS2CLK,FASTLSBPRE,[$1\8,$43\8,$0\8,$1\8,$0\8] ;CONFIG_MODE_ENTER
   high PS2SEL
   pause 1

   low PS2SEL
   shiftout PS2CMD,PS2CLK,FASTLSBPRE,[$01\8,$44\8,$00\8,$01\8,$03\8,$00\8,$00\8,$00\8,$00\8] ;SET_MODE_AND_LOCK
   high PS2SEL
   pause 1

   low PS2SEL
   shiftout PS2CMD,PS2CLK,FASTLSBPRE,[$01\8,$4F\8,$00\8,$FF\8,$FF\8,$03\8,$00\8,$00\8,$00\8] ;SET_DS2_NATIVE_MODE
   high PS2SEL
   pause 1

   low PS2SEL
   shiftout PS2CMD,PS2CLK,FASTLSBPRE,[$01\8,$43\8,$00\8,$00\8,$5A\8,$5A\8,$5A\8,$5A\8,$5A\8] ;CONFIG_MODE_EXIT_DS2_NATIVE
   high PS2SEL
   pause 1

   low PS2SEL
   shiftout PS2CMD,PS2CLK,FASTLSBPRE,[$01\8,$43\8,$00\8,$00\8,$00\8,$00\8,$00\8,$00\8,$00\8] ;CONFIG_MODE_EXIT
   high PS2SEL
   pause 100
      
   sound P9,[100\4000, 100\4500, 100\5000]
   return
  ENDIF
;------------------------------------------------------------
  IF (DualShock(1).bit3 = 0) and LastButton(0).bit3 THEN   ;Start Button test
   IF(HexOn) THEN
     'Turn off
     Sound P9,[100\5000]
     BodyPosX = 0
     BodyPosY = 0
     BodyPosZ = 0
     BodyRotX = 0
     BodyRotY = 0
     BodyRotZ = 0
     TravelLengthX = 0
     TravelLengthZ = 0
     TravelRotationY = 0

     SSCTime = 600
     GOSUB ServoDriver
     HexOn = False
   ELSE
     'Turn on
     Sound P9,[60\4000]
     SSCTime = 200
     HexOn = True   
   ENDIF
  ENDIF   
;-------------------
  IF HexOn THEN
   IF (DualShock(1).bit0 = 0) and LastButton(0).bit0 THEN ;Select Button test
     IF TravelLengthX=0 & TravelLengthZ=0 & TravelRotationY=0 THEN
      
     ;Switch to next Gait type
        IF GaitType<5 THEN
        Sound P9,[50\4000]
        GaitType = GaitType+1
          ELSE
        Sound P9,[50\4000, 50\4500]
        GaitType = 0
         ENDIF
 
        GOSUB GaitSelect
       ENDIF
     ENDIF
;-------------------   
   
   IF (DualShock(1).bit4 = 0) and LastButton(0).bit4 THEN   ;Up Button test
     BodyPosY = BodyPosY+10  ;adjust pos hight
   ENDIF
;-------------------      
   IF (DualShock(1).bit6 = 0) and LastButton(0).bit6 THEN   ;Down Button test
     BodyPosY = BodyPosY-10  ;adjust pos hight
   ENDIF
;-------------------   
   IF (DualShock(2).bit4 = 0) and LastButton(1).bit4 THEN   ;Triangle Button test
     BodyPosY = 120 ;walk pos
   ENDIF
;-------------------
   IF (DualShock(2).bit5 = 0) and LastButton(1).bit5 THEN   ;Circle Button test
     BodyPosY = 0   ;crouch pos
   ENDIF   
;-------------------

   IF (DualShock(2).bit6 = 0) THEN   ;Cross Button test       
       GOSUB ATTACK
    ENDIF
;-------------------   
   IF (DualShock(2).bit7 = 0) and LastButton(1).bit7 THEN   ;Square Button test
     IF BalanceMode = 0 THEN
      BalanceMode = 1
      sound P9,[100\4000, 50\8000]
     ELSE
      BalanceMode = 0
      sound P9,[250\3000]
     ENDIF
   ENDIF         
;-------------------

;   if (DualShock(1).bit1 = 0) then   ;L3 Button test

;    ENDIF
;-------------------
;   if (DualShock(1).bit2 = 0) then  ;R3 Button test   

;    ENDIF
;-------------------
    BodyYShift = 0
   IF (DualShock(2).bit3 = 0) THEN   ;R1 Button test
     HeadPanAngle = (Dualshock(3) - 128)/2  ;X-axis Right stick horz
     HeadTiltAngle = (Dualshock(4) - 128)/2 ;Z-axis Right stick vert
   ENDIF
;-------------------   
   BodyYShift = 0
   IF (DualShock(2).bit2 = 0) THEN   ;L1 Button test

     BodyPosX = (Dualshock(5) - 128)/2      ;X-axis Left stick horz
     BodyPosZ = -(Dualshock(6) - 128)/3     ;Z-axis Left stick vert
     BodyRotY = (Dualshock(3) - 128)/6                       
     BodyYShift = (-(Dualshock(4) - 128)/2)MIN-(BodyPosY-10)
;--------      
   ELSEIF (DualShock(2).bit0 = 0)    ;L2 Button test

     BodyRotX = (Dualshock(6) - 128)/8
     BodyRotY = (Dualshock(3) - 128)/6
     BodyRotZ = (Dualshock(5) - 128)/8
     BodyYShift = (-(Dualshock(4) - 128)/2)MIN-(BodyPosY-10)
            
   ELSE ;Walk
     'BodyPosX = 0
     'BodyPosY = 0
     'BodyPosZ = 0
     'BodyRotX = 0
     'BodyRotY = 0
     'BodyRotZ = 0

;-------------------      
     IF (DualShock(2).bit1 = 0) THEN ;R2 Button test
      TravelLengthX = -(Dualshock(5) - 128)
      TravelLengthZ = (Dualshock(6) - 128)
     ELSE
      TravelLengthX = -(Dualshock(5) - 128)/2
      TravelLengthZ = (Dualshock(6) - 128)/2   
     ENDIF
       TravelRotationY = -(Dualshock(3) - 128)/4
   ENDIF
   
   ;Calculate walking time delay
   InputTimeDelay = 128 - (ABS((Dualshock(5) - 128)) MIN ABS((Dualshock(6) - 128))) MIN ABS((Dualshock(3) - 128))
   
  ENDIF
 
  LastButton(0) = DualShock(1)
  LastButton(1) = DualShock(2)
return   
;--------------------------------------------------------------------
;[GAIT 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
   GaitSpeed = 200
  ENDIF 
   
  IF (GaitType = 1) THEN ;Quadripple 6 steps
   LRGaitLegNr = 3 
   RFGaitLegNr = 3
   LMGaitLegNr = 5
     RRGaitLegNr = 5
   LFGaitLegNr = 1
   RMGaitLegNr = 1
     sound P9,[50\1000]
   NrLiftedPos = 2
   HalfLiftHeigth = FALSE   
   TLDivFactor = 6    
   StepsInGait = 6   
    GaitSpeed = 200
  ENDIF   
 
  IF (GaitType = 2) THEN ;Tripod 4 steps
   LRGaitLegNr = 3   
   RFGaitLegNr = 1
   LMGaitLegNr = 1
   RRGaitLegNr = 1
   LFGaitLegNr = 3
   RMGaitLegNr = 3
     sound P9,[50\2000]
   NrLiftedPos = 1   
   TLDivFactor = 2    
   StepsInGait = 4      
    GaitSpeed = 200
  ENDIF
 
  IF (GaitType = 3) THEN ;Wave 12 steps
   LRGaitLegNr = 7
   RFGaitLegNr = 1
   LMGaitLegNr = 9
   RRGaitLegNr = 5
   LFGaitLegNr = 11
   RMGaitLegNr = 3
     sound P9,[50\3000]    
   NrLiftedPos = 1
   HalfLiftHeigth = FALSE   
   TLDivFactor = 10    
   StepsInGait = 12      
    GaitSpeed = 200
  ENDIF   
   
    IF (GaitType = 4) THEN ;Ripple 22 steps
   LRGaitLegNr = 3 
   RFGaitLegNr = 3
   LMGaitLegNr = 5
     RRGaitLegNr = 5
   LFGaitLegNr = 1
   RMGaitLegNr = 1
       sound P9,[50\4000]
   NrLiftedPos = 2
   HalfLiftHeigth = FALSE   
   TLDivFactor = 6    
   StepsInGait = 6   
    GaitSpeed = 200
  ENDIF   
 
    IF (GaitType = 5) THEN ;TriRipple Gait 6 steps
   LRGaitLegNr = 1
   RFGaitLegNr = 5
   LMGaitLegNr = 3    
   RRGaitLegNr = 1    
   LFGaitLegNr = 5    
   RMGaitLegNr = 3
     sound P9,[50\5000]           
   NrLiftedPos = 1   
   TLDivFactor = 3    
   StepsInGait = 6   
   GaitSpeed = 200
  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((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

  HeadPanAngle = (HeadPanAngle min HeadPan_MIN) max HeadPan_MAX
  HeadTiltAngle = (HeadTiltAngle min HeadTilt_MIN) max HeadTilt_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]

  Serout SSC_OUT,SSC_BAUTE,["#",dec HeadPan,"P",dec (-HeadPanAngle+90)*10000/1059+650]
  Serout SSC_OUT,SSC_BAUTE,["#",dec HeadTilt,"P",dec (-HeadTiltAngle+90)*10000/1059+650]
  ;Send <CR>
  serout SSC_OUT,SSC_BAUTE,["T",dec SSCTime,13]
 
  PrevSSCTime = SSCTime
return
;--------------------------------------------------------------------
ATTACK: ;Attack mode      
      IF TravelLengthX=0 & TravelLengthZ=0 & TravelRotationY=0 THEN
      ;Attack posture
      serout SSC_OUT,SSC_BAUTE,["#",DEC RMCoxaPin,"P1800#",DEC RMFemurPin,"P1700#",DEC RMTibiaPin,"P1700#",DEC LMCoxaPin,"P1200#", |
             DEC LMFemurPin,"P1300#",DEC LMTibiaPin,"P1300T288",13]
          pause 500
      serout SSC_OUT,SSC_BAUTE,["#",DEC RMFemurPin,"P1500#",DEC RMTibiaPin,"P1500#",DEC LMFemurPin,"P1500#",DEC LMTibiaPin,"P1500T288",13]
          pause 500
      serout SSC_OUT,SSC_BAUTE,["#",DEC RRFemurPin,"P1800#",DEC RRTibiaPin,"P1800#",DEC RMFemurPin,"P1400#",DEC RMTibiaPin,"P1400#", |
             DEC RFCoxaPin,"P1800#",DEC RFFemurPin,"P2100#",DEC LRFemurPin,"P1200#",DEC LRTibiaPin,"P1200#", |
             DEC LMFemurPin,"P1600#",DEC LMTibiaPin,"P1600#",DEC LFCoxaPin,"P1300#",DEC LFFemurPin,"P900T288",13]
          pause 500
   for Index = 1 to 10
      serout SSC_OUT,SSC_BAUTE,["#",DEC RFCoxaPin,"P1800#",DEC RFFemurPin,"P2000#",DEC RFTibiaPin,"P1600#", |
            DEC LFCoxaPin,"P1300#",DEC LFFemurPin,"P900#",DEC LFTibiaPin,"P1600T144",13]
         pause 150
      serout SSC_OUT,SSC_BAUTE,["#",DEC RFCoxaPin,"P1800#",DEC RFFemurPin,"P2100#",DEC RFTibiaPin,"P1300#", |
            DEC LFCoxaPin,"P1300#",DEC LFFemurPin,"P1000#",DEC LFTibiaPin,"P1300T144",13]
         pause 150
   next
      serout SSC_OUT,SSC_BAUTE,["#",DEC RFCoxaPin,"P1500#",DEC RFFemurPin,"P2100#",DEC RFTibiaPin,"P1500#", |
             DEC LFCoxaPin,"P1500#",DEC LFFemurPin,"P900#",DEC LFTibiaPin,"P1500T288",13]
          pause 400
      serout SSC_OUT,SSC_BAUTE,["#",DEC RRFemurPin,"P1500#",DEC RRTibiaPin,"P1500#",DEC RMFemurPin,"P1500#",DEC RMTibiaPin,"P1500#",DEC RFFemurPin,"P1500#", |
             DEC LMFemurPin,"P1500#",DEC LMTibiaPin,"P1500#",DEC LFFemurPin,"P1500#",DEC LRFemurPin,"P1500#",DEC LRTibiaPin,"P1500T576",13]
          pause 400
      serout SSC_OUT,SSC_BAUTE,["#",DEC RMCoxaPin,"P1500#",DEC RMFemurPin,"P1700#",DEC RMTibiaPin,"P1700#", |
             DEC LMCoxaPin,"P1500#",DEC LMFemurPin,"P1300#",DEC LMTibiaPin,"P1300T288",13]
          pause 400
      serout SSC_OUT,SSC_BAUTE,["#",DEC RMFemurPin,"P1500#",DEC RMTibiaPin,"P1500#",DEC LMFemurPin,"P1500#",DEC LMTibiaPin,"P1500T288",13]
          pause 400
    ENDIF

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
;--------------------------------------------------------------------
Last edited by innerbreed on Mon Oct 12, 2009 9:29 pm, edited 1 time in total.
innerbreed
 
Posts: 154
Joined: Mon Jun 29, 2009 4:43 pm
Location: Cambridge UK

Re: MSR-H01 Hexapod, using ABB2 with ATOM 28 Pro, SSC32

Postby Matt Denton » Mon Sep 21, 2009 7:12 am

Hi Jonny, You have been busy!! :)

Thanks for sharing your port for the MSR-H01, I would love to see a video of it on the move if you have time.
Matt Denton
AKA: Winchy_Matt

micromagic systems ltd
Matt Denton
Site Admin
 
Posts: 1622
Joined: Tue May 20, 2008 9:15 pm
Location: Winchester UK

Re: MSR-H01 Hexapod, using ABB2 with ATOM 28 Pro, SSC32

Postby innerbreed » Mon Sep 21, 2009 4:47 pm

managed to grab the camera..
But once im completely happy with it ill record a better video.
here is what we have:
Image
innerbreed
 
Posts: 154
Joined: Mon Jun 29, 2009 4:43 pm
Location: Cambridge UK

Re: MSR-H01 Hexapod, using ABB2 with ATOM 28 Pro, SSC32

Postby innerbreed » Sat Oct 10, 2009 9:28 am

here is an attack mode for the MSR-H01.

Once i have completed the whole code i will update the first code posted at the start.
ATTACT MODE:
Code: Select all
   IF (DualShock(2).bit6 = 0) THEN   ;Cross Button test       
      IF TravelLengthX=0 & TravelLengthZ=0 & TravelRotationY=0 THEN
      ;Attack posture
      serout SSC_OUT,SSC_BAUTE,["#",DEC RMCoxaPin,"P1800#",DEC RMFemurPin,"P1700#",DEC RMTibiaPin,"P1700#",DEC LMCoxaPin,"P1200#", |
             DEC LMFemurPin,"P1300#",DEC LMTibiaPin,"P1300T288",13]
          pause 500
      serout SSC_OUT,SSC_BAUTE,["#",DEC RMFemurPin,"P1500#",DEC RMTibiaPin,"P1500#",DEC LMFemurPin,"P1500#",DEC LMTibiaPin,"P1500T288",13]
          pause 500
      serout SSC_OUT,SSC_BAUTE,["#",DEC RRFemurPin,"P1800#",DEC RRTibiaPin,"P1800#",DEC RMFemurPin,"P1400#",DEC RMTibiaPin,"P1400#", |
             DEC RFCoxaPin,"P1800#",DEC RFFemurPin,"P2100#",DEC LRFemurPin,"P1200#",DEC LRTibiaPin,"P1200#", |
             DEC LMFemurPin,"P1600#",DEC LMTibiaPin,"P1600#",DEC LFCoxaPin,"P1300#",DEC LFFemurPin,"P900T288",13]
          pause 500
   for Index = 1 to 10
      serout SSC_OUT,SSC_BAUTE,["#",DEC RFCoxaPin,"P1800#",DEC RFFemurPin,"P2000#",DEC RFTibiaPin,"P1600#", |
            DEC LFCoxaPin,"P1300#",DEC LFFemurPin,"P900#",DEC LFTibiaPin,"P1600T144",13]
         pause 150
      serout SSC_OUT,SSC_BAUTE,["#",DEC RFCoxaPin,"P1800#",DEC RFFemurPin,"P2100#",DEC RFTibiaPin,"P1300#", |
            DEC LFCoxaPin,"P1300#",DEC LFFemurPin,"P1000#",DEC LFTibiaPin,"P1300T144",13]
         pause 150
   next
      serout SSC_OUT,SSC_BAUTE,["#",DEC RFCoxaPin,"P1500#",DEC RFFemurPin,"P2100#",DEC RFTibiaPin,"P1500#", |
             DEC LFCoxaPin,"P1500#",DEC LFFemurPin,"P900#",DEC LFTibiaPin,"P1500T288",13]
          pause 400
      serout SSC_OUT,SSC_BAUTE,["#",DEC RRFemurPin,"P1500#",DEC RRTibiaPin,"P1500#",DEC RMFemurPin,"P1500#",DEC RMTibiaPin,"P1500#",DEC RFFemurPin,"P1500#", |
             DEC LMFemurPin,"P1500#",DEC LMTibiaPin,"P1500#",DEC LFFemurPin,"P1500#",DEC LRFemurPin,"P1500#",DEC LRTibiaPin,"P1500T576",13]
          pause 400
      serout SSC_OUT,SSC_BAUTE,["#",DEC RMCoxaPin,"P1500#",DEC RMFemurPin,"P1700#",DEC RMTibiaPin,"P1700#", |
             DEC LMCoxaPin,"P1500#",DEC LMFemurPin,"P1300#",DEC LMTibiaPin,"P1300T288",13]
          pause 400
      serout SSC_OUT,SSC_BAUTE,["#",DEC RMFemurPin,"P1500#",DEC RMTibiaPin,"P1500#",DEC LMFemurPin,"P1500#",DEC LMTibiaPin,"P1500T288",13]
          pause 400
    ENDIF
  ENDIF


also im working on a caterpillar gait were the two back legs move forward first followed by the middle, than followed by the front. kinda like a wave but with both sides running symmetrical in sequence.

here is a quick look at what the code looks like:

Code: Select all
    IF (GaitType = 9) THEN ;TriWave Gait 6 steps (caterpillar)
   LRGaitLegNr = 1
   RFGaitLegNr = 3
   LMGaitLegNr = 2    
   RRGaitLegNr = 1    
   LFGaitLegNr = 3    
   RMGaitLegNr = 2
           
   NrLiftedPos = 1   
   TLDivFactor = 3    
   StepsInGait = 4   
   NomGaitSpeed = 200
  ENDIF


And also a 25step Ripple gait will been added.

Code: Select all
    IF (GaitType = 8) THEN ;Ripple 25 steps
   LRGaitLegNr = 1 
   RFGaitLegNr = 5
   LMGaitLegNr = 9
   RRGaitLegNr = 13
   LFGaitLegNr = 17
   RMGaitLegNr = 22
    
   NrLiftedPos = 3
   HalfLiftHeigth = TRUE   
   TLDivFactor = 15 
   StepsInGait = 25       
    NomGaitSpeed = 200
  ENDIF


how this all works:

A universal gait engine which makes is possible to simply add new gait types by setting properties.
I’ll explain the settings with a 6 steps ripple gait for example.


Image
StepsInGait = 6 Total number of steps in the gait
NrLiftedPos = 1 Number of positions in the air (blue position)
TLDivFactor = 4 Number of movements on the ground (red arrows)
NomGaitSpeed = 150 Nominal speed of a single step

LRGaitLegNr = 1
RFGaitLegNr = 2
LMGaitLegNr = 3
RRGaitLegNr = 4
LFGaitLegNr = 5
RMGaitLegNr = 6 Defining sequence of the legs.

for a more detailed look at what this all means please follow the like to Xans page:
http://www.lynxmotion.com/images/html/proj102.htm
innerbreed
 
Posts: 154
Joined: Mon Jun 29, 2009 4:43 pm
Location: Cambridge UK

Re: MSR-H01 Hexapod, using ABB2 with ATOM 28 Pro, SSC32

Postby innerbreed » Mon Oct 12, 2009 9:42 pm

First post code update: It is currently Mon Oct 12, 2009 22:30 pm

Changes are as follows:
Mechanical limits:
    The Right Front Legs and also Head Pan to stop conflics while walking.

PS2 controls added:
    L3 = *non-defined*
    R1 & L. Stick = Moves Head X/Z
    R3 = *non-defined*

HeadTracking (still in development)
    Use R1 & L. Stick now also Moves Head X/Z
    Full P&T while running Body IK moves (right stick only)

Gait:
    TriRipple Gait 6 steps (caterpillar)
innerbreed
 
Posts: 154
Joined: Mon Jun 29, 2009 4:43 pm
Location: Cambridge UK

Re: MSR-H01 Hexapod, using ABB2 with ATOM 28 Pro, SSC32

Postby dikos » Wed Nov 16, 2011 9:37 pm

Hello, I have added the Phoenix robot head and I would like to use the above code with the PS2 remote.
I did Copy Paste the Basic Micro Stydio but did not manage to upload.
I BasicAtom Pro28, and SSC-32.
How should I do? :oops:

Thank you very much.
Dimos
dikos
 
Posts: 3
Joined: Wed Oct 26, 2011 7:45 pm

Re: MSR-H01 Hexapod, using ABB2 with ATOM 28 Pro, SSC32

Postby innerbreed » Thu Nov 17, 2011 8:25 am

have you have followed the tutorials. i need more info. what version of BMS do you have. i dont know if you are unable to upload due to your set up or connection. or other problems.

do you have a Phoenix hex or a MSR hex?
innerbreed
 
Posts: 154
Joined: Mon Jun 29, 2009 4:43 pm
Location: Cambridge UK

Re: MSR-H01 Hexapod, using ABB2 with ATOM 28 Pro, SSC32

Postby dikos » Thu Nov 17, 2011 1:27 pm

Hi innerbreed,i have Basic Micro Studio 2.0.0.14, Phoenix Hexapod, PS2 controller, SSC-32 V2, and Bot Board II / BASIC Atom Pro.
i have follow this tutorial http://www.lynxmotion.com/images/html/build159.htm but i don't know how to use your code.

i don't know how to run your code at my Phoenix Hexapod, what i must do ?
dikos
 
Posts: 3
Joined: Wed Oct 26, 2011 7:45 pm

Re: MSR-H01 Hexapod, using ABB2 with ATOM 28 Pro, SSC32

Postby innerbreed » Thu Nov 17, 2011 6:41 pm

ok i understand now. you have a phoenix hexapod but you have added a pan/tilt servo for a head?

Well the code i posted is exactly the same as the Phoenix v1.3 code
http://www.lynxmotion.com/images/files/phoen1.3.bas
with the additional servos added.

I used BASIC Atom Pro IDE v8.0.1.8 but is should run on Studio. i haven tested it in studio for the latest (after 2.x.x.x) 'so that might be worth thinking about.

Make sure you have used the pin assignments in my code to make sure you have them correct for the robot. unfortunately we don't all use the same pins so always work looking at for future problems.

Also make sure the SSC is set for i38.4 and not 115.2
Also you will need to configure the code for your robots legs.
innerbreed
 
Posts: 154
Joined: Mon Jun 29, 2009 4:43 pm
Location: Cambridge UK

Re: MSR-H01 Hexapod, using ABB2 with ATOM 28 Pro, SSC32

Postby dikos » Thu Nov 17, 2011 8:30 pm

Thank you innerbreed.I understand it's difficult for me to do the changes at pin and i don't now how to do that.
dikos
 
Posts: 3
Joined: Wed Oct 26, 2011 7:45 pm

Next

Return to Code Examples

Who is online

Users browsing this forum: No registered users and 1 guest

cron