Lynxmotion Tech Support

stmaxvalue var word ; maximum raw value for the steering
stminvalue var word ; minimum raw value for the steering
strange var word ; steering range, positive integer
stcentershift var word  ; center point for steering channel if only positive intergers (raw values) are used
thrmaxvalue var word ; maximum raw value for the throttle
thrminvalue var word ; minimum raw value for the throttle
thrrange var word ; the range of the throttle, positive integer
thrcentershift var word ; center point for throttle channel if only positive integers (raw Values) are used
testnum var word ; testnumber for the calibration procedure = or > the calibratetime value
thr var sword ;raw input value for throttle
st var sword ;raw input value for steering
aux var sword ; raw input value for auxilary
xspeed var sword ; recommend keeping as a sword, uses this value repeatedly for filtering, mod only if space is an issue
steering var sword ; recommend keeping as a sword, uses this value repeatedly for filtering, mod only if space is an issue
rcbutton var sword ; third channel if you make your own filter then this can be a signed number, for now its a 1 or 0


rcstaticdeadband con 10 ;the deadband around center (divide by 128 for a percentage, i.e. 10 deadband is 8.33_%)
thrin con P1 ;pin number for the throttle channel
stin con P2 ;pin number for the steering channel
auxin con p0 ;pin number for the Aux in channel
spk con p9 ; botboard II has the speaker on pin 9 - only used for the sound to let you know its calibrating
calibratetime con 128 ; number of tests before saving/stopping calibration


stmaxvalue = 0 ;if the bot drifts left/right/
stminvalue = 2000 ;forward/back then change the 2000's to 8000's
thrmaxvalue = 0
thrminvalue = 2000

xspeed = 0 ; this is the forward speed, Why was X chosen for speed? shouldn't it be Y?
steering = 0 ; Steering rate


;########################################
;Calibration procedure, will beep for a couple seconds
;whilst you move all channels to their extremes
;a few times auto generates max and min values for each channel

for testnum = 1 to calibratetime ;count from 1 to the value of calibrate time, the current value is stored in testnum
high spk ; tie the speaker pin to positive  - makes a click sound
   pulsin thrin,1,thr ; read the minor pulse length of the throttle signal from the receiver (the short part of the pulse)
   pulsin stin,1,st ; same for steering, the 1 here makes this process run 8 times faster than if it were a 0
   
      if thr > thrmaxvalue then ; if the current maximum value for throttle is greater than the highest stored one, then
         thrmaxvalue = thr ; store the current (highest) value in place of the (lower) max value
         elseif thr < thrminvalue ; if the current throttle value is lower than the lowest ever seen then
         thrminvalue = thr ; store the new lowest value in its (the last low value) place
      endif

      if st > stmaxvalue then ; if the current maximum value for steering is greater than the highest stored one, then
         stmaxvalue = st ; store the current (highest) value in place of the (lower) max value
         elseif st < stminvalue ; if the current throttle value is lower than the lowest ever seen then
         stminvalue = st ; store the new lowest value in its (the last low value) place
      endif
      


   low spk ; ground the speaker - makes a click sound
pause 10 ; added a pause so the calibration procedure gives you enough time to react and move the controls
next


thrrange = thrmaxvalue - thrminvalue ; set the throttles range equal to the difference between its max and min values
thrcentershift = thrrange / 2 ; set the center point of the throttle halfway between the two extremes
strange = stmaxvalue - stminvalue ; set the steerings range equal to the difference between its max and min values
stcentershift = strange / 2 ; set the center point of the steering halfway between the two extremes

top

; the guts


;Throttle
; returns about -118 to +118
Pulsin thrin,1,thr ; read the current value of the throttle channel

xspeed = thr - thrminvalue - thrcentershift ; set xspeed equal to a positive or negative value proportional to its extremes
xspeed = xspeed / 3 ; divide xspeed by 3 so it fits within the +/- 120 specs for the hexapod


;hard to explain this one, read it a few times, if the value is bouncing around center, then this will filter it out and set it to 0 instead
if xspeed > rcstaticdeadband then
   xspeed = xspeed
   elseif xspeed < 0 - rcstaticdeadband
   xspeed = xspeed
   else
   xspeed = 0
endif

;#################################################
;Steering - tweaked for -19 to +19 range according to the comments in the orignal
;hex code, but in actual use seems to work better set for giving a +120 to -120 range

Pulsin stin,1,st

steering = st - stminvalue - stcentershift ; set steering equal to a positive or negative value proportional to its extremes
steering = steering / 3 ;19 here gives a +/-19 range and 3 gives a +/- 118 range, the later seems to work better.


;hard to explain this one, read it a few times, if the value is bouncing around center, then this will filter it out and set it to 0 instead
if steering > rcstaticdeadband then
   steering = steering
   elseif steering < 0 - rcstaticdeadband
   steering = steering
   else
   steering = 0
endif

;#################################################


goto top

; Use Basic Micro ATOM IDE 05.3.0.0
; *** not suitable for IDE 02.2.1.1 (there's a specific version for this IDE)
; *** not suitable for PRO IDE 08.0.1.7 (there's a specific version for this IDE)
; Use Basic Atom 28

;************************************************
;*** Basic Atom with SSC-32 and PS2 DualShock ***
;----------------- 1.43 3DOF-C ------------------
;--------------- PS2 Controller -----------------
;-------------- Round Body (H3-R) ---------------
;********** Bot Board Buzzer support ************
;***** Little Gripper / Pan & Tilt support ******
;**************** Deck support ******************
;************************************************
;** Programmer: Laurent Gay, lynxrios-at######### **
;************************************************
;
; let's say that the 2 switches holes of the H3/H3R is the 'rear' of the robot
;
; *** using 3 'right' legs and 3 'left' legs ***
; put the SSC-32 card with the PC serial port looking to 'Front' (opposite to H3R switches)
;
; 3 right legs connections :
; Rear Right leg : Hip Horizontal : pin 0, Hip Vertical : pin 1, Knee : pin 2
; Middle Right leg : Hip Horizontal : pin 4, Hip Vertical : pin 5, Knee : pin 6
; Front Right leg : Hip Horizontal : pin 8, Hip Vertical : pin 9, Knee : pin 10
;
; 3 left legs connections :
; Rear Left leg : Hip Horizontal : pin 16, Hip Vertical : pin 17, Knee : pin 18
; Middle Left leg : Hip Horizontal : pin 20, Hip Vertical : pin 21, Knee : pin 22
; Front Left leg : Hip Horizontal : pin 24, Hip Vertical : pin 25, Knee : pin 26
;
; Default mode :
; Move the robot (2 axis translation) with the right joystick
; Turn with the left joystick X axis
; move Up/Down body with the left joystick Y axis
; lock/unlock the height with the left joystick push button (L3)
;
; Body move mode :
; Push 'select' button to activate this mode (push 'select' again to return to default mode)
; move the body horizontally (2 axis translation) with the right joystick
; the left joystick push button (L3) has a new function now, it selects between 4 control modes for the left joystick:
;  1)- (by default) move the body vertically + rotate around the vertical axis
;  2)- (push L3 to activate) move the body vertically + roll
;  3)- (push L3 again to activate) pitch + rotate around the vertical axis
;  4)- (push L3 again to activate) pitch + roll the body
;   pushing L3 again will return to mode 1 (cycling)
;
; go to standby position with the 'Triangle' button, press again to go to walk position
; Attack posture with the 'Cross' button (*** only possible if legs are not moving ***)
; All = 1500 posture with the 'Circle' button (*** only possible if legs are not moving ***)
; Learning to fly posture with the 'Square' button (uh ? nutty ?...it CAN fly...) (*** only possible if legs are not moving ***)
; Change the legs 'up' position with the digital Up/Down buttons
;
; Change the gait speed with digital right/left buttons
;
; Presets buttons (only with 'Default' mode):
; - R1 => Tall grass
; - L1 => Tile floor
; - R2 => Body low
; - L2 => Default preset
;
; Little Gripper support on Pin 29 (Left/Right) and 30 (Open/Close) (only in 'Body move' mode)
; - R1 => Open
; - L1 => Close
; - R2 => Rotate Right
; - L2 => Rotate Left
;
; Deck support on Pin 31 (panning servo)
; allow to handle a deck (for camera, sensors, lights)
; the panning servo will follow the direction it walk
; Additional tilt servo support for 180° Deck on pin 28 (Round body only)
; When using a 180° deck + 2 GP2D12 back to back on a tilt servo
; the tilt servo will incline the 'usable' GP2D12 (looking the walking direction) to floor
; when walking at low speed, the angle increase (looking near)
; when walking at full speed, the angle is close to horizontal (looking far)
; when going opposite direction, the angle is mirrored to vertical,
;   making the opposite GP2D12 looking near or far
;
; Start button toggles between the 'knee angle shift' and 'normal' mode
;
; R3 button (right joystick push button) : Horn
;
; you may have to push the Analog Button on a MadCatz Wireless controller (if in sleep mode)
;
;************************************************
;
;
;--------------------------------------------------------------------
;-------------Constants
;PS2 Controller / BotBoard II   (PS2 controller => pin 12, 13, 14, 15)
DAT con P12
CMD con P13
SEL con P14
CLK con P15
;*** SSC-32 card communication on pin 8 ***
SSC32 con p8

DeadZone con 14
PadMode con $79

;Deck
Deck_PulseMin con 600
Deck_PulseMax con 2400   

;Little Gripper
LGripLR_PulseMin con 750
LGripLR_PulseMax con 2250
LGripOC_PulseMin con 750
LGripOC_PulseMax con 2250

;Legs
HipH_AngleMin con 21      ;30°
HipH_AngleMax con 107      ;150°
HipH_PulseMin con 910      
HipH_PulseMax con 2090      

HipV_AngleMin con 25      ;35°
HipV_AngleMax con 103      ;145°
HipV_PulseMin con 960   
HipV_PulseMax con 2040   

Knee_AngleMin con 36      ;50°
Knee_AngleMax con 107      ;150°
Knee_PulseMin con 1107      
Knee_PulseMax con 2090

;****************************************************
; 3DOF-A Leg Dimensions      (TibiaAngle constant = 0)
;HipV_HipH    con 38    ;1.50" = 38mm (1.50 * 25.4)
;Femur_Length con 57    ;2.25" = 57mm (2.25 * 25.4)
;Tibia_Length con 124   ;4.875" = 124mm (4.875 * 25.4)

; 3DOF-B Leg Dimensions      (TibiaAngle constant = 0)
;HipV_HipH    con 29    ;1.14" = 29mm (1.14 * 25.4)
;Femur_Length con 57    ;2.25" = 57mm (2.25 * 25.4)
;Tibia_Length con 108   ;4.25" = 108mm (4.25 * 25.4)

; 3DOF-C Leg Dimensions      (TibiaAngle constant = 20)
;HipV_HipH    con 29    ;1.14" = 29mm (1.14 * 25.4)
;Femur_Length con 57    ;2.25" = 57mm (2.25 * 25.4)
;Tibia_Length con 141   ;5.55" = 141mm (5.55 * 25.4)

; 3DOF-(Old) Leg Dimensions   (TibiaAngle constant = 0)
;HipV_HipH    con 32    ;1.25" = 32mm (1.25 * 25.4)
;Femur_Length con 70    ;2.75" = 70mm (2.75 * 25.4)
;Tibia_Length con 108   ;4.25" = 108mm (4.25 * 25.4)
;****************************************************

; 3DOF-C Leg Dimensions      (TibiaAngle constant = 20)
HipV_HipH    con 29    ;1.14" = 29mm (1.14 * 25.4)
Femur_Length con 57    ;2.25" = 57mm (2.25 * 25.4)
Tibia_Length con 141   ;5.55" = 141mm (5.55 * 25.4)

LegUpShiftMin con 30
LegUpShiftMax con 70
KneeShiftPulse con 200       ;range is 0 to 255 (activated by pushing start button, affect the legs when up)
TibiaAngle con 20         ;range is -20 (interior) to 20 (exterior), 0 is vertical, this is 'all time' tibia angle

;ACos
data    64,64,63,63,63,62,62,62,61,61,61,60,60,60,59,59,|
      59,59,58,58,58,57,57,57,56,56,56,55,55,55,54,54,|
      54,53,53,53,52,52,52,51,51,51,50,50,50,49,49,49,|
      48,48,48,47,47,46,46,46,45,45,45,44,44,44,43,43,|
      42,42,42,41,41,41,40,40,39,39,39,38,38,37,37,37,|
      36,36,35,35,35,34,34,33,33,32,32,31,31,31,30,30,|
      29,29,28,28,27,27,26,25,25,24,24,23,23,22,21,21,|
      20,19,19,18,17,16,15,15,14,13,11,10,09,07,05,00

; Don't use ByteTable instead, it takes too much memory
RRHH con "0"   ;Rear Right leg : Hip Horizontal : pin 0
RRHH2 con "0"
RRHV con "0"   ;Rear Right leg : Hip Vertical : pin 1
RRHV2 con "1"
RRK con "0"      ;Rear Right leg : Knee : pin 2
RRK2 con "2"

MRHH con "0"   ;Middle Right leg : Hip Horizontal : pin 4
MRHH2 con "4"
MRHV con "0"   ;Middle Right leg : Hip Vertical : pin 5
MRHV2 con "5"
MRK con "0"      ;Middle Right leg : Knee : pin 6
MRK2 con "6"

FRHH con "0"   ;Front Right leg : Hip Horizontal : pin 8
FRHH2 con "8"
FRHV con "0"   ;Front Right leg : Hip Vertical : pin 9
FRHV2 con "9"
FRK con "1"      ;Front Right leg : Knee : pin 10
FRK2 con "0"

RLHH con "1"   ;Rear Left leg : Hip Horizontal : pin 16
RLHH2 con "6"
RLHV con "1"   ;Rear Left leg : Hip Vertical : pin 17
RLHV2 con "7"
RLK con "1"      ;Rear Left leg : Knee : pin 18
RLK2 con "8"

MLHH con "2"   ;Middle Left leg : Hip Horizontal : pin 20
MLHH2 con "0"
MLHV con "2"   ;Middle Left leg : Hip Vertical : pin 21
MLHV2 con "1"
MLK con "2"      ;Middle Left leg : Knee : pin 22
MLK2 con "2"

FLHH con "2"   ;Front Left leg : Hip Horizontal : pin 24
FLHH2 con "4"
FLHV con "2"   ;Front Left leg : Hip Vertical : pin 25
FLHV2 con "5"
FLK con "2"      ;Front Left leg : Knee : pin 26
FLK2 con "6"

;Little Gripper Pin
LGripLR Con "2"   ;Little Gripper Left/Right : pin 29
LGripLR2 Con "9"
LGripOC Con "3"   ;Little Gripper Open/close : pin 30
LGripOC2 Con "0"

; Deck Pin
DeckP Con "3"   ;Deck Panning servo : pin 31
DeckP2 Con "1"

; Additional Tilt servo for 180° Deck
DeckTilt Con "2"
DeckTilt2 Con "8"

;--------------------------------------------------------------------            
;-------------Variables
Index var Byte
DualShock var Byte(7)
DS2Mode var Byte
LastButton var Byte(2)
Steps var Byte
FlipFlap var Bit
FlagOff var Bit
MovesDelay var Sbyte
HeightAdjust var Bit

HeightShift var Sbyte(6)
KneeShift var Byte(7)

Height var Sbyte
LastHeight var Sbyte
HeightLimit var Byte
YSpeed var Sbyte
Roll var Sbyte
Pitch var Sbyte
Rotate var Sbyte(6)
DCoord var Byte
DAngle var Byte
NbSteps var Byte
GaitSpeed var Byte
GaitSpeedTmp var Byte
StepFlag var Byte

TmpXPos var Long
TmpYPos var Long
TmpZPos var Long
TmpDistance var Long
TmpSEW var Word
TmpSEWSEW var Long
TmpCos var Long
TmpAngle var Sbyte

XPos var Sword(6)
YPos var Sword(6)
ZPos var Sword(6)
XPos2 var Sword(6)
YPos2 var Sword(6)
ZPos2 var Sword(6)
Distance var Sword

HipH_Angle var Sbyte
HipV_Angle var Sword
Knee_Angle var Sword

HipH_Pulse var Word(6)
HipV_Pulse var Word(6)
Knee_Pulse var Word(6)

LittleGripOCPulse var Word
LittleGripLRPulse var Word
Deck_Pulse var Word
DeckTilt_Pulse var Word

LegUpShift var Byte
GaitKind var Bit
Freeze var Bit
Tripod var Bit
LockLegs var Bit
LeftStickMode var Byte


stmaxvalue var word
stminvalue var word
strange var word
stcentershift var word
aumaxvalue var word
auminvalue var word
aurange var word
aucentershift var word
thrmaxvalue var word
thrminvalue var word
thrrange var word
thrcentershift var word
testnum var word
thr var sword
st var sword
au var sword
Xspeed var sword
steering var sword
rcbutton var sword

rcstaticdeadband con 10
thrin con P1
stin con P2
auin con p0
spk con p9
calibratetime con 127


;--------------------------------------------------------------------   
;***************
;*** Program ***
;***************

;-------------Init
clear
auminvalue = 2000
aumaxvalue = 0
stmaxvalue = 0
stminvalue = 2000
thrmaxvalue = 0
thrminvalue = 2000
xspeed = 0
steering = 0
rcbutton = 1

calibrate
pause 3000
for testnum = 1 to 127
high spk
   pulsin thrin,1,thr
   pulsin stin,1,st
   
      if thr > thrmaxvalue then
         thrmaxvalue = thr
         elseif thr < thrminvalue
         thrminvalue = thr
      endif

      if st > stmaxvalue then
         stmaxvalue = st
         elseif st < stminvalue
         stminvalue = st
      endif

      if au > aumaxvalue then
         aumaxvalue = au
         elseif st < auminvalue
         auminvalue = au
      endif
      


   low spk
pause 10
next

thrrange = thrmaxvalue - thrminvalue
thrcentershift = thrrange / 2
strange = stmaxvalue - stminvalue
stcentershift = strange / 2
aurange = aumaxvalue - auminvalue
aucentershift = aurange / 2


Sound 9,[100000\8235, 200000\2435]

programstart

high CLK

LastButton(0) = 255
LastButton(1) = 255

LittleGripOCPulse = 1500
LittleGripLRPulse = 1500

HeightLimit = 25 - ABS(TibiaAngle / 2)

pause 500

;-------------Init SSC-32 with pulse offsets
serout SSC32,I8N1_38400,["#", |
   RRHH,RRHH2,"po-40 #",RRHV,RRHV2,"po7 #",RRK,RRK2,"po-7 #", |
   MRHH,MRHH2,"po-31 #",MRHV,MRHV2,"po-48 #",MRK,MRK2,"po-12 #", |
   FRHH,FRHH2,"po-2 #",FRHV,FRHV2,"po-12 #",FRK,FRK2,"po5 #", |
   RLHH,RLHH2,"po-26 #",RLHV,RLHV2,"po-33 #",RLK,RLK2,"po-22 #", |
   MLHH,MLHH2,"po-40 #",MLHV,MLHV2,"po45 #",MLK,MLK2,"po10 #", |
   FLHH,FLHH2,"po5 #",FLHV,FLHV2,"po16 #",FLK,FLK2,"po-10",13]

;SSC-32 -> H3 engine
gosub H3Init
Sound 9,[100000\4235, 200000\4435]
;--------------------------------------------------------------------
;-------------Main loop
main



Height = 5      ; tall grass
LegUpShift = 30
GaitSpeed = 3
HeightAdjust = 0
   LegUpShift = (LegUpShift max LegUpShiftMax) min LegUpShiftMin
   GaitKind = LegUpShift > 45
   GaitSpeedTmp = GaitSpeed max 20
   MovesDelay = 8   
NoPresets
;################################################
;NOTE: X AND Y ARE FLIPPED, THEY ARE WRONG IN ORIGINAL CODE
;Throttle
; returns about -118 to +118
Pulsin thrin,1,thr

xspeed = thr - thrminvalue - thrcentershift
xspeed = xspeed / 3

if xspeed > rcstaticdeadband then
   xspeed = xspeed
   elseif xspeed < 0 - rcstaticdeadband
   xspeed = xspeed
   else
   xspeed = 0
endif

;#################################################
;Steering - tweaked for +/-118 range

Pulsin stin,1,st

steering = st - stminvalue - stcentershift
steering = steering / 3

if steering > rcstaticdeadband then
   steering = steering
   elseif steering < 0 - rcstaticdeadband
   steering = steering
   else
   steering = 0
endif

if rcbutton = 0 then   ;rcbutton tesT
   gosub hold
endif


;@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@

GaitSpeed = GaitSpeedTmp min (3 + GaitKind)

   StepFlag = (GaitSpeedTmp - 2) max 3
   if HeightAdjust or LockLegs then
      Height = LastHeight + (((DualShock(6) - 128) / 5) - LastHeight) / StepFlag; Left Stick Vertical
   endif

   Height = (Height Max HeightLimit) min - HeightLimit

;H3   
   if Steering or (Height <> LastHeight) or YSpeed or XSpeed or LockLegs then
      MovesDelay = 8
   endif

   if MovesDelay > 0 then
      LastHeight = Height
      DCoord = SQR(XSpeed * XSpeed + YSpeed * YSpeed)
      TmpCos = XSpeed * 127 / DCoord
      gosub ACos
      DAngle = TmpAngle
      if YSpeed > 0 then
         DAngle = 256 - DAngle
      endif

      DCoord = DCoord max (128 - ABS(Height) - ABS(TibiaAngle * 2))

      ; 180° Deck Code
      if (Dangle < 64 or Dangle > 191) and (DCoord > 0) then
         DeckTilt_Pulse = 1680 - Dcoord
      else
         DeckTilt_Pulse = 1380 + Dcoord
      endif
      Deck_Pulse =  ((Deck_PulseMax - Deck_PulseMin) * ((DAngle + 64 ) & $7F)) / 127 + Deck_PulseMin

      ; 360° Deck Code
      ;Deck_Pulse = ((Deck_PulseMax - Deck_PulseMin) * DAngle) / 255 + Deck_PulseMin

      Steps = Steps + 1
      if Steps > NbSteps then
         Steps = 1
         FlipFlap = FlipFlap ^ 1
         NbSteps = GaitSpeed
      endif

      if LockLegs then
         if LeftStickMode > 0 then ;Body roll and pitch
            if (leftStickMode & 1) then
               Roll = Roll + (Steering - Roll) / StepFlag
               Steering = 0
            else
               Roll = 0
            endif

            if (leftStickMode & 2) then
               Pitch = Pitch + (Height - Pitch) / StepFlag
               Height = 0
            else
               Pitch = 0
            endif

            HeightShift(0) = Roll/10
            HeightShift(2) = HeightShift(0)
            HeightShift(3) = -HeightShift(0)
            HeightShift(5) = HeightShift(3)

            HeightShift(0) = ((HeightShift(0) + Pitch) max HeightLimit) min -HeightLimit
            HeightShift(3) = -HeightShift(0)
            HeightShift(2) = ((HeightShift(2) - Pitch) max HeightLimit) min -HeightLimit
            HeightShift(5) = -HeightShift(2)

            HeightShift(1) = HeightShift(0) + HeightShift(2)
            HeightShift(4) = HeightShift(3) + HeightShift(5)
         endif
         LittleGripOCPulse = ((LittleGripOCPulse + (DualShock(2).bit3 * 50) - (DualShock(2).bit2 * 50)) |
            min LGripOC_PulseMin) max LGripOC_PulseMax
         LittleGripLRPulse = ((LittleGripLRPulse + (DualShock(2).bit1 * 50) - (DualShock(2).bit0 * 50)) |
            min LGripLR_PulseMin) max LGripLR_PulseMax
         GaitSpeedTmp = StepFlag + 2
      else
         StepFlag = NbSteps - Steps + 1
      endif

      for Index = 0 to 5
         XPos2(Index) = -(DCoord * COS(DAngle + (Index * 43 + 21)) / 300) ; 43 => 60 degrees
         ZPos2(Index) = -(DCoord * SIN(DAngle + (Index * 43 + 21)) / 300) ; 43 => 60 degrees
      next

      Tripod = FlipFlap
      gosub AorB_Down

      Freeze = Steps < (NbSteps - GaitKind)
      Tripod = FlipFlap ^ 1
      gosub AorB_Up   

      for Index = 0 to 5

         ;Distance and HipH_Angle with XZ
         Distance = SQR((XPos(Index) * XPos(Index) + ZPos(Index) * ZPos(Index)) / 4) * 2
         TmpCos = ZPos(Index) * 127 / Distance

         Distance = Distance + TibiaAngle ; Tibia angle for special legs (3DOF-C)

         Gosub ACos
         HipH_Angle = (TmpAngle max HipH_AngleMax) min HipH_AngleMin
         ;----------

         ;Set Angle
         TmpXPos = XPos(Index) * 127
         TmpYPos = (YPos(Index) * 127) max 0
         TmpZPos = ZPos(Index) * 127
         TmpDistance = ((Distance - HipV_HipH) * 127) min 0
         
         TmpSEWSEW = (TmpYPos * TmpYPos + TmpDistance * TmpDistance) / 16129
         TmpSEW = SQR(TmpSEWSEW)

         TmpCos = (Femur_Length * Femur_Length + Tibia_Length * Tibia_Length - TmpSEWSEW)
         TmpCos = TmpCos * 127 / (2 * Femur_Length * Tibia_Length)
         gosub ACos
         Knee_Angle = (TmpAngle max Knee_AngleMax) min Knee_AngleMin

         TmpCos = -TmpYPos / TmpSew
         gosub ACos
         HipV_Angle = TmpAngle

         TmpCos = (Femur_Length * Femur_Length - Tibia_Length * Tibia_Length + TmpSEWSEW)
         TmpCos = TmpCos * 127 / (2 * Femur_Length * TmpSEW)
          gosub ACos
          HipV_Angle = ((TmpAngle + HipV_Angle) max HipV_AngleMax) min HipV_AngleMin
          ;----------

         HipH_Angle = HipH_Angle + Rotate(Index)
         
         HipH_Pulse(Index) =  (((HipH_PulseMax - HipH_PulseMin) * (HipH_Angle - HipH_AngleMin) |
            / (HipH_AngleMax - HipH_AngleMin) + HipH_PulseMin) max HipH_PulseMax) min HipH_PulseMin

         HipV_Pulse(Index) = (((HipV_PulseMax - HipV_PulseMin) * (HipV_Angle - HipV_AngleMin) |
            / (HipV_AngleMax - HipV_AngleMin) + HipV_PulseMin) max HipV_PulseMax) min HipV_PulseMin

         Knee_Pulse(Index) = (((Knee_PulseMax - Knee_PulseMin) * (Knee_Angle - Knee_AngleMin) |
            / (Knee_AngleMax - Knee_AngleMin) + Knee_PulseMin) max Knee_PulseMax) min Knee_PulseMin
         Knee_Pulse(Index) = 3000 - Knee_Pulse(Index) - KneeShift(Index) * 2
      next

      serout SSC32,I8N1_38400,["#",RRHH,RRHH2,"P",DEC HipH_Pulse(0),"#",RRHV,RRHV2,"P",DEC HipV_Pulse(0), |
         "#",RRK,RRK2,"P",DEC Knee_Pulse(0),"#",FRHH,FRHH2,"P",DEC HipH_Pulse(2), |
         "#",FRHV,FRHV2,"P",DEC HipV_Pulse(2),"#",FRK,FRK2,"P",DEC Knee_Pulse(2), |
         "#",MLHH,MLHH2,"P",DEC HipH_Pulse(4),"#",MLHV,MLHV2,"P",DEC 3000 - HipV_Pulse(4), |
         "#",MLK,MLK2,"P",DEC 3000 - Knee_Pulse(4),"#",MRHH,MRHH2,"P",DEC HipH_Pulse(1), |
         "#",MRHV,MRHV2,"P",DEC HipV_Pulse(1),"#",MRK,MRK2,"P",DEC Knee_Pulse(1), |   
         "#",RLHH,RLHH2,"P",DEC HipH_Pulse(5),"#",RLHV,RLHV2,"P",DEC 3000 - HipV_Pulse(5), |
         "#",RLK,RLK2,"P",DEC 3000 - Knee_Pulse(5),"#",FLHH,FLHH2,"P",DEC HipH_Pulse(3), |   
         "#",FLHV,FLHV2,"P",DEC 3000 - HipV_Pulse(3),"#",FLK,FLK2,"P",DEC 3000 - Knee_Pulse(3), |
         "#",DeckP,DeckP2,"P",DEC Deck_Pulse,"#",DeckTilt,DeckTilt2,"P",DEC DeckTilt_Pulse, |
         "#",LGripOC,LGripOC2,"P",DEC LittleGripOCPulse,"#",LGripLR,LGripLR2,"P",DEC LittleGripLRPulse,"T180",13]
   else
      pause 144
      if MovesDelay < -17 then
         MovesDelay = (NbSteps - GaitKind) - Steps + 1
      EndIf   
   endif
   MovesDelay = MovesDelay - 1
OffEnd
   LastButton(0) = DualShock(1)
   LastButton(1) = DualShock(2)

   goto main

;--------------------------------------------------------------------
;-------------Sub H3 Init
H3Init
   LockLegs = 0
   NbSteps = 4
   GaitSpeedTmp = NbSteps
   LegUpShift = 35

   for Index = 0 to 5
      XPos(Index) = HipV_HipH + Femur_Length
      YPos(Index) = - Tibia_Length
      ZPos(Index) = 0
   next

   Gosub InitPos
   MovesDelay = 8
   ; then All1500

;-------------Sub All1500
All1500
   HeightAdjust = 1
   height = 0
   LastHeight = 0

   for Index = 0 to 27   ;preserve servo 28 (Deck Tilt),29 and 30 (Little Gripper),31 (Deck)
      serout SSC32,I8N1_38400,["#",DEC Index,"P1500"]
   next
   serout SSC32,I8N1_38400,["T576",13]

   pause 576

   Steps = NbSteps
   
   ; then Flat Body

;-------------Sub Flat Body
FlatBody
   Roll = 0
   Pitch = 0
   for Index = 0 to 5
      HeightShift(Index) = 0
   next

   return   

;-------------Sub InitPos
InitPos
   serout SSC32,I8N1_38400,["#",RRHV,RRHV2,"P2100#",RRK,RRK2,"P1800#",MRHV,MRHV2,"P2100#",MRK,MRK2,"P1800#", |
      FRHV,FRHV2,"P2100#",FRK,FRK2,"P1800#",RLHV,RLHV2,"P900#",RLK,RLK2,"P1200#",MLHV,MLHV2,"P900#", |
      MLK,MLK2,"P1200#",FLHV,FLHV2,"P900#",FLK,FLK2,"P1200T1152",13]
   pause 576
   serout SSC32,I8N1_38400,["#",RRHH,RRHH2,"P1500#",MRHH,MRHH2,"P1500#",FRHH,FRHH2,"P1500#", |
      RLHH,RLHH2,"P1500#",MLHH,MLHH2,"P1500#",FLHH,FLHH2,"P1500T576",13]
   pause 576   

   return

;-------------Sub Arc Cosinus
ACos
   TmpCos = (TmpCos max 127) min -127
   if TmpCos < 0 then
      read -TmpCos,TmpAngle
      TmpAngle = 127 - TmpAngle
   else
      read TmpCos,TmpAngle
   endif
   
   return

;-------------Tripod 'A' (tripod = 0) or 'B' (Tripod = 1) Down
AorB_Down   
   for Index = 0 + Tripod to 4 + Tripod Step 2
      KneeShift(Index) = 0
      Rotate(Index) = Rotate(Index) + (Steering/5 - Rotate(Index))/StepFlag
      YPos(Index) = -Tibia_Length + Height + HeightShift(Index)
      XPos(Index) = XPos(Index) + (XPos2(Index) - (XPos(Index) - (HipV_HipH + Femur_Length)))/StepFlag
      ZPos(Index) = ZPos(Index) + (ZPos2(Index) - ZPos(Index))/StepFlag
   next

   return

;-------------Tripod 'A' (tripod = 0) or 'B' (Tripod = 1) Up
AorB_Up
   if LockLegs then AorB_Down   ;Goto sub AorB_Down and return directly from it to main loop

   if (Steps = NbSteps) and GaitKind then
      StepFlag = NbSteps
      goto AorB_Down   ;Goto sub AorB_Down and return directly from it to main loop
   else
       StepFlag = StepFlag - GaitKind - 1
   endif   

   for Index = 0 + Tripod to 4 + Tripod Step 2
      if (Steps > 1) and (Steps < (NbSteps - GaitKind - 1)) then
         KneeShift(Index) = KneeShift(6)
      else
         KneeShift(Index) = 0
      endif
      Rotate(Index) = Rotate(Index) + (-Steering/5 - Rotate(Index))/StepFlag
      YPos(Index) = -Tibia_Length + Height + (LegUpShift * Freeze)
      XPos(Index) = XPos(Index) + (-XPos2(Index) - (XPos(Index) - (HipV_HipH + Femur_Length)))/StepFlag
      ZPos(Index) = ZPos(Index) + (-ZPos2(Index) - ZPos(Index))/StepFlag
   next

   return   
   
;--------------------------------------------------------------------
;-------------Sub All1500
hold
   HeightAdjust = 1
   height = 0
   LastHeight = 0

   for Index = 0 to 27   ;preserve servo 28 (Deck Tilt),29 and 30 (Little Gripper),31 (Deck)
      serout SSC32,I8N1_38400,["#",DEC Index,"P1500"]
   next
   serout SSC32,I8N1_38400,["T576",13]

   pause 576

   Steps = NbSteps
   return
   ; then Flat Body