#[Powerpod Tag][V1.01_H3]
#[IDE][Basic Micro IDE V02.2.1.1]
;
; Use Basic Micro ATOM IDE 02.2.1.1
; *** not suitable for IDE 05.3.0.0 (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
#[IDE][Basic Micro IDE V05.3.0.0]
;%CONFIG% BASICATOM28 $0 $0 $0 $0  $0  $0  $0  $0  $0  $0  $0  $0  $0  $0  $0  $0  $0  $0
; 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
#[IDE][Basic Micro PRO IDE V08.0.1.7]
;
; Use Basic Micro ATOM PRO IDE 08.0.1.7
; *** not suitable for IDE 02.2.1.1 (there's a specific version for this IDE)
; *** not suitable for IDE 05.3.0.0 (there's a specific version for this IDE)
; Use Basic Atom PRO 28
#[IDE]

;************************************************
#[Control][PS2]
;*** Basic Atom with SSC-32 and PS2 DualShock ***
#[H3 Legs][3DOF-A]
;----------------- 1.44 3DOF-A ------------------
#[H3 Legs][3DOF-B]
;----------------- 1.44 3DOF-B ------------------
#[H3 Legs][3DOF-C]
;----------------- 1.44 3DOF-C ------------------
#[H3 Legs][3DOF-(Old)]
;--------------- 1.44 3DOF-(Old) ----------------
#[H3 Legs]
;--------------- PS2 Controller -----------------
#[Control][Autonomous]
;************ Basic Atom with SSC-32 ************
#[H3 Legs][3DOF-A]
;----------------- 2.44 3DOF-A ------------------
#[H3 Legs][3DOF-B]
;----------------- 2.44 3DOF-B ------------------
#[H3 Legs][3DOF-C]
;----------------- 2.44 3DOF-C ------------------
#[H3 Legs][3DOF-(Old)]
;--------------- 2.44 3DOF-(Old) ----------------
#[H3 Legs]
;----------------- Autonomous -------------------
#[Control][Serial Port]
;****** Basic Atom with SSC-32, PC control ******
#[H3 Legs][3DOF-A]
;----------------- 3.44 3DOF-A ------------------
#[H3 Legs][3DOF-B]
;----------------- 3.44 3DOF-B ------------------
#[H3 Legs][3DOF-C]
;----------------- 3.44 3DOF-C ------------------
#[H3 Legs][3DOF-(Old)]
;--------------- 3.44 3DOF-(Old) ----------------
#[H3 Legs]
;---- Serial Port Control (Bot Board / PC) ------
#[Control]
#[H3 Body][Round]
;-------------- Round Body (H3-R) ---------------
#[H3 Body][Inline]
;-------------- Inline Body (H3) ----------------
#[H3 Body]
;********** Bot Board Buzzer support ************
;***** Little Gripper / Pan & Tilt support ******
;**************** Deck support ******************
;************************************************
;** Programmer: Laurent Gay, lynxrios@yahoo.fr **
;************************************************
;
; 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)
;
#[Insert 01][Pins Comments]
; 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
#[Insert 01]
;
#[Control][PS2]
; 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
#[Control][Serial Port]
; 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
#[Control][Autonomous]
; Little Gripper support on Pin 29 (Left/Right) and 30 (Open/Close) (only in 'Body move' mode)
#[Control]
;
; 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
#[H3 Body][Round]
; 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
#[H3 Body]
#[Control][PS2]
;
; 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)
#[Control][Serial Port]
;
; Start button toggles between the 'knee angle shift' and 'normal' mode
;
; R3 button : Horn
#[Control]
;
;************************************************
;
;
;--------------------------------------------------------------------
;-------------Constants
#[Control][PS2]
#[PS2 Controller / BB Connections][BotBoard I  (pin 4, 5, 6, 7)]
;PS2 Controller / BotBoard I	(PS2 controller => pin 4, 5, 6, 7)
DAT con P4
CMD con P5
SEL con P6
CLK con P7
;*** SSC-32 card communication on pin 15 ***
SSC32 con p15
#[PS2 Controller / BB Connections][BotBoard II (pin 12, 13, 14, 15)]
;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
#[PS2 Controller / BB Connections]

#[PS2 sticks dead zone][Small]
DeadZone con 14
#[PS2 sticks dead zone][Normal]
DeadZone con 28
#[PS2 sticks dead zone][Large]
DeadZone con 42
#[PS2 sticks dead zone]
PadMode con $79

#[Control][Autonomous]
;
;*** SSC-32 card communication on pin 15 ***
SSC32 con p15

#[Control][Serial Port]
;
DeadZone con 8

;*** SSC-32 card communication on pin 15 ***
SSC32 con p15

#[Control]
;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)
;****************************************************

#[H3 Legs][3DOF-A]
; 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)
#[H3 Legs][3DOF-B]
; 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)
#[H3 Legs][3DOF-C]
; 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)
#[H3 Legs][3DOF-(Old)]
; 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)
#[H3 Legs]

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

;ACos
#[IDE][Basic Micro IDE V02.2.1.1]			
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
#[IDE][Basic Micro IDE V05.3.0.0]			
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
#[IDE][Basic Micro PRO IDE V08.0.1.7]
ArcCos Bytetable	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
#[IDE]		

; Don't use ByteTable instead, it takes too much memory
#[Insert 02][Pins Assign]
RRHH con "0"	;Rear Right leg : Hip Horizontal : pin 00
RRHH2 con "0"
RRHV con "0"	;Rear Right leg : Hip Vertical : pin 01
RRHV2 con "1"
RRK con "0"		;Rear Right leg : Knee : pin 02
RRK2 con "2"

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

FRHH Con "0"	;Front Right leg : Hip Horizontal : pin 08
FRHH2 Con "8"
FRHV Con "0"	;Front Right leg : Hip Vertical : pin 09
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"
#[Insert 02]

;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"

#[H3 Body][Round]
; Additional Tilt servo for 180° Deck
DeckTilt Con "2"
DeckTilt2 Con "8"

#[H3 Body]
;--------------------------------------------------------------------				
;-------------Variables
Index var Byte
#[H3 Body][Inline]
Index2 var Byte
#[H3 Body]
#[Control][PS2]
DualShock var Byte(7)
DS2Mode var Byte
LastButton var Byte(2)
#[Control][Serial Port]
DualShock var Byte(7)
LastButton var Byte(2)
#[Control][Autonomous]
Buttons var Byte
Buttons2 var byte
#[Control]
Steps var Byte
FlipFlap var Bit
FlagOff var Bit
MovesDelay var Sbyte
#[Control][PS2]
HeightAdjust var Bit
#[Control][Serial Port]
HeightAdjust var Bit
#[Control]

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

Steering var Sbyte
#[H3 Body][Inline]
SteeringLimit var Byte
#[H3 Body]
Height var Sbyte
#[Control][Autonomous]
Steering2 var Sbyte
Height2 var Sbyte
#[Control]
LastHeight var Sbyte
HeightLimit var Byte
XSpeed var Sbyte
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
#[H3 Body][Round]
DeckTilt_Pulse var Word
#[H3 Body]

LegUpShift var Byte
GaitKind var Bit
Freeze var Bit
Tripod var Bit
LockLegs var Bit
LeftStickMode var Byte
#[Control][Autonomous]
LastLeftStickMode var Byte
KneeShiftOn var Bit

StepCnt var Word
Initialized var Bit
#[Control][Serial Port]

CheckSum var Byte
#[Control]

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

;-------------Init
clear
#[Control][PS2]

high CLK

LastButton(0) = 255
LastButton(1) = 255
#[Control][Serial Port]

LastButton(0) = 255
LastButton(1) = 255
#[Control]

LittleGripOCPulse = 1500
LittleGripLRPulse = 1500

HeightLimit = 25 - ABS(TibiaAngle / 2)

pause 500

;-------------Init SSC-32 with pulse offsets
#[IDE][Basic Micro IDE V02.2.1.1]
serout SSC32,i38400,["#", |
#[IDE][Basic Micro IDE V05.3.0.0]
serout SSC32,I8N1_38400,["#", |
#[IDE][Basic Micro PRO IDE V08.0.1.7]
serout SSC32,i38400,["#", |
#[IDE]
#[Insert 03][Offsets]
	RRHH,RRHH2,"po00 #",RRHV,RRHV2,"po00 #",RRK,RRK2,"po00 #", |
	MRHH,MRHH2,"po00 #",MRHV,MRHV2,"po00 #",MRK,MRK2,"po00 #", |
	FRHH,FRHH2,"po00 #",FRHV,FRHV2,"po00 #",FRK,FRK2,"po00 #", |
	RLHH,RLHH2,"po00 #",RLHV,RLHV2,"po00 #",RLK,RLK2,"po00 #", |
	MLHH,MLHH2,"po00 #",MLHV,MLHV2,"po00 #",MLK,MLK2,"po00 #", |
	FLHH,FLHH2,"po00 #",FLHV,FLHV2,"po00 #",FLK,FLK2,"po00",13]
#[Insert 03]

;SSC-32 -> H3 engine
gosub H3Init
#[IDE][Basic Micro IDE V02.2.1.1]
Sound 9,[100\4235, 200\4435]
#[IDE][Basic Micro IDE V05.3.0.0]
Sound 9,[100000\4235, 200000\4435]
#[IDE][Basic Micro PRO IDE V08.0.1.7]
Sound 9,[100\4235, 200\4435]
#[IDE]
;--------------------------------------------------------------------
;-------------Main loop
main

#[Control][PS2]
;Ps2Query	
	low SEL
	shiftout CMD,CLK,FASTLSBPRE,[$1\8]
	shiftin DAT,CLK,FASTLSBPOST,[DS2Mode\8]
	high SEL
	pause 1

	low SEL
	shiftout CMD,CLK,FASTLSBPRE,[$1\8,$42\8]	
	shiftin DAT,CLK,FASTLSBPOST,[DualShock(0)\8, DualShock(1)\8, DualShock(2)\8, DualShock(3)\8, |
		DualShock(4)\8, DualShock(5)\8, DualShock(6)\8]
	high SEL
	pause 1	

	if DS2Mode <> PadMode then
		low SEL
		shiftout CMD,CLK,FASTLSBPRE,[$1\8,$43\8,$0\8,$1\8,$0\8] ;CONFIG_MODE_ENTER
		high SEL
		pause 1

		low SEL
		shiftout CMD,CLK,FASTLSBPRE,[$01\8,$44\8,$00\8,$01\8,$03\8,$00\8,$00\8,$00\8,$00\8] ;SET_MODE_AND_LOCK
		high SEL
		pause 1

		;low SEL
		;shiftout CMD,CLK,FASTLSBPRE,[$01\8,$4D\8,$00\8,$00\8,$01\8,$FF\8,$FF\8,$FF\8,$FF\8] ;VIBRATION_ENABLE
		;high SEL
		;pause 1

		low SEL
		shiftout CMD,CLK,FASTLSBPRE,[$01\8,$4F\8,$00\8,$FF\8,$FF\8,$03\8,$00\8,$00\8,$00\8] ;SET_DS2_NATIVE_MODE
		high SEL
		pause 1

		low SEL
		shiftout CMD,CLK,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 SEL
		pause 1

		low SEL
		shiftout CMD,CLK,FASTLSBPRE,[$01\8,$43\8,$00\8,$00\8,$00\8,$00\8,$00\8,$00\8,$00\8] ;CONFIG_MODE_EXIT
		high SEL
		pause 288

		goto main
	endif

	;**********************************************************************
	; R1-L1-R2-L2 presets code
	;
	; to change Body Height (range is -25 (body up) to 25 (body down)) :
	;Height = 10 ; for example
	;
	; to change Leg Up Shift (range is 20 (close to floor) to 70 (about 2.5 inches from floor))
	;LegUpShift = 30 ; for example
	;
	; to change Gait Speed (range is 3 (fast) to 20 (very slow))
	;GaitSpeed = 4 ; for example

	If LockLegs then NoPresets
	if (DualShock(2).bit3 = 0) and LastButton(1).bit3 then	;R1 Button test
		Height = -25		; tall grass
		LegUpShift = 70
		GaitSpeed = 10
	elseif (DualShock(2).bit2 = 0) and LastButton(1).bit2	;L1 Button test
		Height = 5			; tile floor
		LegUpShift = 30
		GaitSpeed = 3
	elseif (DualShock(2).bit1 = 0) and LastButton(1).bit1	;R2 Button test
		Height = 25			; Body low
		LegUpShift = 35
		GaitSpeed = 4
	elseif (DualShock(2).bit0 = 0) and LastButton(1).bit0	;L2 Button test
		Height = 0			; Default preset
		LegUpShift = 35
		GaitSpeed = 4
	else
		goto NoPresets
	endif

	HeightAdjust = 0
	LegUpShift = (LegUpShift max LegUpShiftMax) min LegUpShiftMin 
	GaitKind = LegUpShift > 45
	GaitSpeedTmp = GaitSpeed max 20
	MovesDelay = 8	
NoPresets
	;**********************************************************************
#[Control][Autonomous]
	if Initialized = 0 then shunt

;--------------------------------------------------------------------
	; ***************
	; Buttons :
	; ***************
	; Bit 0 - Horn
	; Bit 1 - ** Unused **
	; Bit 2 - ** Unused **
	; Bit 3 - ** Unused **
	; Bit 4 - On/Off (you can test FlagOff(=1) to check if the servos are Off)
	; Bit 5 - All = 1500
	; Bit 6 - Attack
	; Bit 7 - Learning to fly
	; (for example, 'Buttons.bit6 = 1' will call the Attack mode posture)
	; 'Attack' and 'Learning to Fly' must be called while the robot is not moving
	; so, you'll have to test the variable MovesDelay (=0) to make sure the H3R is not moving,
	; else, it will not call the sub routine
	;
 	; ***************
	; Buttons2 :
	; ***************
	; Bit 0 - Open
	; Bit 1 - Rotate Right
	; Bit 2 - Close
	; Bit 3 - Rotate Left
	; Bit 4 - ** Unused **
	; Bit 5 - ** Unused **
	; Bit 6 - ** Unused **
	; Bit 7 - ** Unused **
	;
	; ***************
	; Steering (Rotation):
	; ***************
	; Range 120 (Right) to -120 (Left)
	; 
	; ***************
	; XSpeed (translation):
	; ***************
	; Range -120 (Forward) to 120 (Backward) (translation)
	;
	; ***************
	; YSpeed (translation):
	; ***************
	; Range 120 (Right) to -120 (Left)
	;
	; ***************
	; Height :
	; ***************
	; Range 25 (Body Down) to -25 (Body Up)
	; 
	; ***************
	; LegUpShift :
	; ***************
	; Range 20 (Close to Floor) to 70 (About 2.5 inches from floor)
	; if LegUpShift > 45 the tripod Gait is modified to allow extra vertical move and the GaitSpeed is limited to 8-4 range
	; if LegUpShift is close to 70 it's recommended to use a 8-5 range for GaitSpeed 
	;	(a too fast gait loop won't allow extra vertical move)
	; If LegUpShift > 45, you'll have to move the Body Higher 
	;	(Setting Height to -15 or -20 for example) to allow extra vertical move
	;
	; ***************
	; GaitSpeed :
	; ***************
	; range 20 (Slow gait loop) to 3 (Fast gait loop)
	; Note, gaitSpeed is limited to 20-4 range if LegUpShift > 45
	;
	; ***************
	; KneeShiftOn	
	; ***************
	; KneeShiftOn = 0 => normal gait
	; KneeShiftOn = 1 => add a shift angle when tripod is Up
	;
	; ***************
	; LockLegs	
	; ***************
	; LockLegs = 0 => normal walk
	; LockLegs = 1 => body moves
	;
	; ***************
	; LeftStickMode	
	; ***************
	; if locklegs = 1 => body moves mode :
	; 	- LeftStickMode = 0 => Variable Steering and Height 
	;		are used for vertical axis rotation (Yaw) and translation (Height)
	;		---------------------------------------------
	; 	- LeftStickMode = 1 => Variable Steering and Height 
	;		are used for roll (right or left) and vertical axis translation (Height)
	;		---------------------------------------------
	; 	- LeftStickMode = 2 => Variable Steering and Height 
	;		are used for vertical axis rotation (Yaw) and pitch (forward or backward)
	;		---------------------------------------------
	; 	- LeftStickMode = 3 => Variable Steering and Height 
	;		are used for roll (right or left) and pitch (forward or backward)

;--------------------------------------------------------------------
#[Control][Serial Port]
;SerialQuery
#[IDE][Basic Micro IDE V02.2.1.1]
	serout S_OUT,i9600,["Rd"]	; 'Ready' to recieve data now
	serin S_OUT,i9600,100,NoData,[str DualShock(0)\7]

	CheckSum = DualShock(1) ^ DualShock(2) ^ DualShock(3) ^ |
		DualShock(4) ^ DualShock(5) ^ DualShock(6)

	;serout S_OUT,i9600,[CheckSum]

if CheckSum <> DualShock(0) then
	Sound 9,[50\2000]
#[IDE][Basic Micro IDE V05.3.0.0]	
	serout S_OUT,I8N1_9600,["Rd"]	; 'Ready' to recieve data now
	serin S_OUT,I8N1_9600,100000,NoData,[str DualShock(0)\7]

	CheckSum = DualShock(1) ^ DualShock(2) ^ DualShock(3) ^ |
		DualShock(4) ^ DualShock(5) ^ DualShock(6)

	;serout S_OUT,I8N1_9600,[CheckSum]

if CheckSum <> DualShock(0) then
	Sound 9,[50000\2000]
#[IDE][Basic Micro PRO IDE V08.0.1.7]
	serout S_OUT,i9600,["Rd"]	; 'Ready' to recieve data now
	serin S_OUT,i9600,200000,NoData,[str DualShock(0)\7]

	CheckSum = DualShock(1) ^ DualShock(2) ^ DualShock(3) ^ |
		DualShock(4) ^ DualShock(5) ^ DualShock(6)

	;serout S_OUT,i9600,[CheckSum]

if CheckSum <> DualShock(0) then
	Sound 9,[50\2000]
#[IDE]
NoData	
	DualShock(1) = $FF
	DualShock(2) = $FF
	for Index = 3 to 6
		DualShock(Index) = $80
	next
endif

	;**********************************************************************
	; R1-L1-R2-L2 presets code
	;
	; to change Body Height (range is -25 (body up) to 25 (body down)) :
	;Height = 10 ; for example
	;
	; to change Leg Up Shift (range is 20 (close to floor) to 70 (about 2.5 inches from floor))
	;LegUpShift = 30 ; for example
	;
	; to change Gait Speed (range is 3 (fast) to 20 (very slow))
	;GaitSpeed = 4 ; for example

	if (DualShock(2).bit3 = 0) and LastButton(1).bit3 then	;R1 Button test
		Height = -25		; tall grass
		LegUpShift = 70
		GaitSpeed = 10
	elseif (DualShock(2).bit2 = 0) and LastButton(1).bit2	;L1 Button test
		Height = 5			; tile floor
		LegUpShift = 30
		GaitSpeed = 3
	elseif (DualShock(2).bit1 = 0) and LastButton(1).bit1	;R2 Button test
		Height = 25			; Body low
		LegUpShift = 35
		GaitSpeed = 4
	elseif (DualShock(2).bit0 = 0) and LastButton(1).bit0	;L2 Button test
		Height = 0			; Default preset
		LegUpShift = 35
		GaitSpeed = 4
	else
		goto NoPresets
	endif

	HeightAdjust = 0
	LegUpShift = (LegUpShift max LegUpShiftMax) min LegUpShiftMin 
	GaitKind = LegUpShift > 45
	GaitSpeedTmp = GaitSpeed max 20
	MovesDelay = 8	
NoPresets
	;**********************************************************************
#[Control]

#[Control][Autonomous]
Shunt
	if Buttons.bit4 then	; On/Off
#[Control][PS2]
	if (DualShock(2).bit4 = 0) and LastButton(1).bit4 then	;Triangle Button test
#[Control][Serial Port]
	if (DualShock(2).bit4 = 0) and LastButton(1).bit4 then	;Triangle Button test
#[Control]
		FlagOff = FlagOff ^ 1
		if FlagOff then
			gosub All1500
			Gosub InitPos
			pause 144
#[IDE][Basic Micro IDE V02.2.1.1]
			for Index = 0 to 31
				serout SSC32,i38400,["#",DEC Index,"P0"]
			next
			serout SSC32,i38400,[13]
#[IDE][Basic Micro IDE V05.3.0.0]
			for Index = 0 to 31
				serout SSC32,I8N1_38400,["#",DEC Index,"P0"]
			next
			serout SSC32,I8N1_38400,[13]
#[IDE][Basic Micro PRO IDE V08.0.1.7]
			for Index = 0 to 31
				serout SSC32,i38400,["#",DEC Index,"P0"]
			next
			serout SSC32,i38400,[13]
#[IDE]
		else
			gosub H3Init
		endif
	endif
	
	if FlagOff then
		pause 144
		goto OffEnd
	endif

	if (MovesDelay <= 0) or LockLegs then
#[Control][Autonomous]
		if Buttons.bit5 then	; All = 1500
#[Control][PS2]
		if (DualShock(2).bit5 = 0) and LastButton(1).bit5 then	;Circle Button test
#[Control][Serial Port]
		if (DualShock(2).bit5 = 0) and LastButton(1).bit5 then	;Circle Button test
#[Control]
			gosub All1500
		endif

#[Control][Autonomous]
		if Buttons.bit6 and (MovesDelay <= 0) then	; Attack
#[Control][PS2]
		if (DualShock(2).bit6 = 0) and LastButton(1).bit6 then	;Cross Button test
#[Control][Serial Port]
		if (DualShock(2).bit6 = 0) and LastButton(1).bit6 then	;Cross Button test
#[Control]
			gosub All1500
			;Attack posture
#[IDE][Basic Micro IDE V02.2.1.1]
			serout SSC32,i38400,["#",MRHH,MRHH2,"P1800#",MRHV,MRHV2,"P1700#",MRK,MRK2,"P1700#", |
#[IDE][Basic Micro IDE V05.3.0.0]		
			serout SSC32,I8N1_38400,["#",MRHH,MRHH2,"P1800#",MRHV,MRHV2,"P1700#",MRK,MRK2,"P1700#", |
#[IDE][Basic Micro PRO IDE V08.0.1.7]
			serout SSC32,i38400,["#",MRHH,MRHH2,"P1800#",MRHV,MRHV2,"P1700#",MRK,MRK2,"P1700#", |
#[IDE]		
				MLHH,MLHH2,"P1200#",MLHV,MLHV2,"P1300#",MLK,MLK2,"P1300T288",13]
			pause 288
#[IDE][Basic Micro IDE V02.2.1.1]		
			serout SSC32,i38400,["#",MRHV,MRHV2,"P1500#",MRK,MRK2,"P1500#",MLHV,MLHV2,"P1500#", |
#[IDE][Basic Micro IDE V05.3.0.0]		
			serout SSC32,I8N1_38400,["#",MRHV,MRHV2,"P1500#",MRK,MRK2,"P1500#",MLHV,MLHV2,"P1500#", |
#[IDE][Basic Micro PRO IDE V08.0.1.7]
			serout SSC32,i38400,["#",MRHV,MRHV2,"P1500#",MRK,MRK2,"P1500#",MLHV,MLHV2,"P1500#", |
#[IDE]		
				MLK,MLK2,"P1500T288",13]
			pause 288
#[IDE][Basic Micro IDE V02.2.1.1]		
			serout SSC32,i38400,["#",RRHV,RRHV2,"P1800#",RRK,RRK2,"P1800#",MRHV,MRHV2,"P1400#", |
#[IDE][Basic Micro IDE V05.3.0.0]		
			serout SSC32,I8N1_38400,["#",RRHV,RRHV2,"P1800#",RRK,RRK2,"P1800#",MRHV,MRHV2,"P1400#", |
#[IDE][Basic Micro PRO IDE V08.0.1.7]
			serout SSC32,i38400,["#",RRHV,RRHV2,"P1800#",RRK,RRK2,"P1800#",MRHV,MRHV2,"P1400#", |
#[IDE]
#[H3 Body][Round]
				MRK,MRK2,"P1400#",FRHH,FRHH2,"P1800#",FRHV,FRHV2,"P2100#",RLHV,RLHV2,"P1200#", |
				RLK,RLK2,"P1200#",MLHV,MLHV2,"P1600#",MLK,MLK2,"P1600#",FLHH,FLHH2,"P1200#", |
				FLHV,FLHV2,"P900T288",13]
			pause 288
#[H3 Body][Inline]
				MRK,MRK2,"P1400#",FRHH,FRHH2,"P2250#",FRHV,FRHV2,"P2100#",RLHV,RLHV2,"P1200#", |
				RLK,RLK2,"P1200#",MLHV,MLHV2,"P1600#",MLK,MLK2,"P1600#",FLHH,FLHH2,"P750#", |
				FLHV,FLHV2,"P900T576",13]
			pause 576
#[H3 Body]			
			for Index = 1 to 10
#[IDE][Basic Micro IDE V02.2.1.1]		
				serout SSC32,i38400,["#",FRHV,FRHV2,"P2000#",FRK,FRK2,"P1600#",FLHV,FLHV2,"P900#", |
#[IDE][Basic Micro IDE V05.3.0.0]			
				serout SSC32,I8N1_38400,["#",FRHV,FRHV2,"P2000#",FRK,FRK2,"P1600#",FLHV,FLHV2,"P900#", |
#[IDE][Basic Micro PRO IDE V08.0.1.7]
				serout SSC32,i38400,["#",FRHV,FRHV2,"P2000#",FRK,FRK2,"P1600#",FLHV,FLHV2,"P900#", |
#[IDE]			
					FLK,FLK2,"P1600T144",13]
				pause 144
#[IDE][Basic Micro IDE V02.2.1.1]			
				serout SSC32,i38400,["#",FRHV,FRHV2,"P2100#",FRK,FRK2,"P1400#",FLHV,FLHV2,"P1000#", |
#[IDE][Basic Micro IDE V05.3.0.0]			
				serout SSC32,I8N1_38400,["#",FRHV,FRHV2,"P2100#",FRK,FRK2,"P1400#",FLHV,FLHV2,"P1000#", |
#[IDE][Basic Micro PRO IDE V08.0.1.7]
				serout SSC32,i38400,["#",FRHV,FRHV2,"P2100#",FRK,FRK2,"P1400#",FLHV,FLHV2,"P1000#", |
#[IDE]			
					FLK,FLK2,"P1400T144",13]
				pause 144
			next
#[IDE][Basic Micro IDE V02.2.1.1]		
			serout SSC32,i38400,["#",FRHH,FRHH2,"P1500#",FRK,FRK2,"P1500#",FLHH,FLHH2,"P1500#", |
#[IDE][Basic Micro IDE V05.3.0.0]		
			serout SSC32,I8N1_38400,["#",FRHH,FRHH2,"P1500#",FRK,FRK2,"P1500#",FLHH,FLHH2,"P1500#", |
#[IDE][Basic Micro PRO IDE V08.0.1.7]
			serout SSC32,i38400,["#",FRHH,FRHH2,"P1500#",FRK,FRK2,"P1500#",FLHH,FLHH2,"P1500#", |
#[IDE]		
#[H3 Body][Round]
				FLHV,FLHV2,"P900#",FLK,FLK2,"P1500T288",13]
			pause 288
#[H3 Body][Inline]
				FLHV,FLHV2,"P900#",FLK,FLK2,"P1500T576",13]
			pause 576
#[H3 Body]
#[IDE][Basic Micro IDE V02.2.1.1]		
			serout SSC32,i38400,["#",RRHV,RRHV2,"P1500#",RRK,RRK2,"P1500#",MRHV,MRHV2,"P1500#", |
#[IDE][Basic Micro IDE V05.3.0.0]		
			serout SSC32,I8N1_38400,["#",RRHV,RRHV2,"P1500#",RRK,RRK2,"P1500#",MRHV,MRHV2,"P1500#", |
#[IDE][Basic Micro PRO IDE V08.0.1.7]
			serout SSC32,i38400,["#",RRHV,RRHV2,"P1500#",RRK,RRK2,"P1500#",MRHV,MRHV2,"P1500#", |
#[IDE]		
				MRK,MRK2,"P1500#",FRHV,FRHV2,"P1500#",MLHV,MLHV2,"P1500#",MLK,MLK2,"P1500#", |
				FLHV,FLHV2,"P1500#",RLHV,RLHV2,"P1500#",RLK,RLK2,"P1500T576",13]
			pause 576
#[IDE][Basic Micro IDE V02.2.1.1]		
			serout SSC32,i38400,["#",MRHH,MRHH2,"P1500#",MRHV,MRHV2,"P1700#",MRK,MRK2,"P1700#", |
#[IDE][Basic Micro IDE V05.3.0.0]		
			serout SSC32,I8N1_38400,["#",MRHH,MRHH2,"P1500#",MRHV,MRHV2,"P1700#",MRK,MRK2,"P1700#", |
#[IDE][Basic Micro PRO IDE V08.0.1.7]
			serout SSC32,i38400,["#",MRHH,MRHH2,"P1500#",MRHV,MRHV2,"P1700#",MRK,MRK2,"P1700#", |
#[IDE]		
				MLHH,MLHH2,"P1500#",MLHV,MLHV2,"P1300#",MLK,MLK2,"P1300T288",13]
			pause 288
#[IDE][Basic Micro IDE V02.2.1.1]		
			serout SSC32,i38400,["#",MRHV,MRHV2,"P1500#",MRK,MRK2,"P1500#",MLHV,MLHV2,"P1500#", |
#[IDE][Basic Micro IDE V05.3.0.0]		
			serout SSC32,I8N1_38400,["#",MRHV,MRHV2,"P1500#",MRK,MRK2,"P1500#",MLHV,MLHV2,"P1500#", |
#[IDE][Basic Micro PRO IDE V08.0.1.7]
			serout SSC32,i38400,["#",MRHV,MRHV2,"P1500#",MRK,MRK2,"P1500#",MLHV,MLHV2,"P1500#", |
#[IDE]		
				MLK,MLK2,"P1500T288",13]
			pause 288
		endif

#[Control][Autonomous]
		if Buttons.bit7 then	; Learning To Fly
#[Control][PS2]
		if (DualShock(2).bit7 = 0) and LastButton(1).bit7 then	;Square Button test
#[Control][Serial Port]
		if (DualShock(2).bit7 = 0) and LastButton(1).bit7 then	;Square Button test
#[Control]
			gosub All1500
			;Learning to fly posture (uh ? nutty ?...it CAN fly...)
			for Index = 1 to 4
#[IDE][Basic Micro IDE V02.2.1.1]		
				serout SSC32,i38400,["#",MRHV,MRHV2,"P1900#",MRK,MRK2,"P1800#",MLHV,MLHV2,"P1100#", |
#[IDE][Basic Micro IDE V05.3.0.0]			
				serout SSC32,I8N1_38400,["#",MRHV,MRHV2,"P1900#",MRK,MRK2,"P1800#",MLHV,MLHV2,"P1100#", |
#[IDE][Basic Micro PRO IDE V08.0.1.7]
				serout SSC32,i38400,["#",MRHV,MRHV2,"P1900#",MRK,MRK2,"P1800#",MLHV,MLHV2,"P1100#", |
#[IDE]			
					MLK,MLK2,"P1200T288",13]
#[IDE][Basic Micro IDE V02.2.1.1]				
				serout SSC32,i38400,["#",RRHV,RRHV2,"P1600#",RRK,RRK2,"P1600#",FRHV,FRHV2,"P1600#", |
#[IDE][Basic Micro IDE V05.3.0.0]			
				serout SSC32,I8N1_38400,["#",RRHV,RRHV2,"P1600#",RRK,RRK2,"P1600#",FRHV,FRHV2,"P1600#", |
#[IDE][Basic Micro PRO IDE V08.0.1.7]
				serout SSC32,i38400,["#",RRHV,RRHV2,"P1600#",RRK,RRK2,"P1600#",FRHV,FRHV2,"P1600#", |
#[IDE]			
					FRK,FRK2,"P1600#",RLHV,RLHV2,"P1400#",RLK,RLK2,"P1400#",FLHV,FLHV2,"P1400#", |
					FLK,FLK2,"P1400T576",13]
				pause 288
#[IDE][Basic Micro IDE V02.2.1.1]			
				serout SSC32,i38400,["#",MRK,MRK2,"P1200#",MLK,MLK2,"P1800T288",13]
#[IDE][Basic Micro IDE V05.3.0.0]			
				serout SSC32,I8N1_38400,["#",MRK,MRK2,"P1200#",MLK,MLK2,"P1800T288",13]
#[IDE][Basic Micro PRO IDE V08.0.1.7]
				serout SSC32,i38400,["#",MRK,MRK2,"P1200#",MLK,MLK2,"P1800T288",13]
#[IDE]			
				pause 288
#[IDE][Basic Micro IDE V02.2.1.1]			
				serout SSC32,i38400,["#",RRHV,RRHV2,"P1400#",RRK,RRK2,"P1400#",MRHV,MRHV2,"P1500#", |
#[IDE][Basic Micro IDE V05.3.0.0]			
				serout SSC32,I8N1_38400,["#",RRHV,RRHV2,"P1400#",RRK,RRK2,"P1400#",MRHV,MRHV2,"P1500#", |
#[IDE][Basic Micro PRO IDE V08.0.1.7]
				serout SSC32,i38400,["#",RRHV,RRHV2,"P1400#",RRK,RRK2,"P1400#",MRHV,MRHV2,"P1500#", |
#[IDE]			
					FRHV,FRHV2,"P1400#",FRK,FRK2,"P1400#",RLHV,RLHV2,"P1600#",RLK,RLK2,"P1600#", |
					MLHV,MLHV2,"P1500#",FLHV,FLHV2,"P1600#",FLK,FLK2,"P1600T576",13]
				pause 576
			next
#[IDE][Basic Micro IDE V02.2.1.1]		
			serout SSC32,i38400,["#",MRHV,MRHV2,"P1800#",MRK,MRK2,"P1800#",MLHV,MLHV2,"P1200#", |
#[IDE][Basic Micro IDE V05.3.0.0]		
			serout SSC32,I8N1_38400,["#",MRHV,MRHV2,"P1800#",MRK,MRK2,"P1800#",MLHV,MLHV2,"P1200#", |
#[IDE][Basic Micro PRO IDE V08.0.1.7]
			serout SSC32,i38400,["#",MRHV,MRHV2,"P1800#",MRK,MRK2,"P1800#",MLHV,MLHV2,"P1200#", |
#[IDE]		
				MLK,MLK2,"P1200T288",13]
			pause 288
			gosub All1500
		endif
	endif

#[Control][Autonomous]
	if (LockLegs = 0) or (LeftStickMode <> LastLeftStickMode) then gosub FlatBody
	
	if KneeShiftOn then
		KneeShift(6) = KneeShiftPulse
	else
		KneeShift(6) = 0
	endif
#[Control][PS2]	
	if (DualShock(1).bit0 = 0) and LastButton(0).bit0 then	;Select Button test
		LockLegs = LockLegs ^ 1
#[IDE][Basic Micro IDE V02.2.1.1]
		Sound 9,[100\(2000 + LockLegs * 1000)]
#[IDE][Basic Micro IDE V05.3.0.0]
		Sound 9,[100000\(2000 + LockLegs * 1000)]
#[IDE][Basic Micro PRO IDE V08.0.1.7]
		Sound 9,[100\(2000 + LockLegs * 1000)]
#[IDE]
		gosub FlatBody
	endif

	if (DualShock(1).bit3 = 0) and LastButton(0).bit3 then	;Start Button test
		KneeShift(6) = KneeShift(6) ^ KneeShiftPulse
#[IDE][Basic Micro IDE V02.2.1.1]
		Sound 9,[100\(800 + KneeShift(6))]
#[IDE][Basic Micro IDE V05.3.0.0]
		Sound 9,[100000\(800 + KneeShift(6))]
#[IDE][Basic Micro PRO IDE V08.0.1.7]
		Sound 9,[100\(800 + KneeShift(6))]
#[IDE]
	endif

	if (DualShock(1).bit1 = 0) and LastButton(0).bit1 then	;L3 Button test
		if LockLegs then
			LeftStickMode = (LeftStickMode + 1) & 3
			gosub FlatBody
		else
			HeightAdjust = HeightAdjust ^ 1
		endif
#[IDE][Basic Micro IDE V02.2.1.1]
		Sound 9,[100\3500]
#[IDE][Basic Micro IDE V05.3.0.0]
		Sound 9,[100000\3500]
#[IDE][Basic Micro PRO IDE V08.0.1.7]
		Sound 9,[100\3500]
#[IDE]
	endif
#[Control][Serial Port]
	if (DualShock(1).bit0 = 0) and LastButton(0).bit0 then	;Select Button test
		LockLegs = LockLegs ^ 1
#[IDE][Basic Micro IDE V02.2.1.1]
		Sound 9,[100\(2000 + LockLegs * 1000)]
#[IDE][Basic Micro IDE V05.3.0.0]
		Sound 9,[100000\(2000 + LockLegs * 1000)]
#[IDE][Basic Micro PRO IDE V08.0.1.7]
		Sound 9,[100\(2000 + LockLegs * 1000)]
#[IDE]
		gosub FlatBody
	endif

	if (DualShock(1).bit3 = 0) and LastButton(0).bit3 then	;Start Button test
		KneeShift(6) = KneeShift(6) ^ KneeShiftPulse
#[IDE][Basic Micro IDE V02.2.1.1]
		Sound 9,[100\(800 + KneeShift(6))]
#[IDE][Basic Micro IDE V05.3.0.0]
		Sound 9,[100000\(800 + KneeShift(6))]
#[IDE][Basic Micro PRO IDE V08.0.1.7]
		Sound 9,[100\(800 + KneeShift(6))]
#[IDE]
	endif

	if (DualShock(1).bit1 = 0) and LastButton(0).bit1 then	;L3 Button test
		if LockLegs then
			LeftStickMode = (LeftStickMode + 1) & 3
			gosub FlatBody
		else
			HeightAdjust = HeightAdjust ^ 1
		endif
#[IDE][Basic Micro IDE V02.2.1.1]
		Sound 9,[100\3500]
#[IDE][Basic Micro IDE V05.3.0.0]
		Sound 9,[100000\3500]
#[IDE][Basic Micro PRO IDE V08.0.1.7]
		Sound 9,[100\3500]
#[IDE]
	endif
#[Control]

#[Control][Autonomous]
	if Buttons.bit0 then	; Horn
#[Control][PS2]
	if (DualShock(1).bit2 = 0) and LastButton(0).bit2 then	;R3 Button test (Horn)
#[Control][Serial Port]
	if (DualShock(1).bit2 = 0) and LastButton(0).bit2 then	;R3 Button test (Horn)
#[Control]
#[IDE][Basic Micro IDE V02.2.1.1]
		Sound 9,[800\392, 600\370, 200\0, 800\330, 600\294, 200\0, 300\330, 100\0, 300\330]
#[IDE][Basic Micro IDE V05.3.0.0]
		Sound 9,[800000\392, 600000\370, 200000\0, 800000\330, 600000\294, 200000\0, |
			300000\330, 100000\0, 300000\330]
#[IDE][Basic Micro PRO IDE V08.0.1.7]
		Sound 9,[800\392, 600\370, 200\0, 800\330, 600\294, 200\0, 300\330, 100\0, 300\330]
#[IDE]
	endif

#[Control][Autonomous]
	Steering = (Steering Max 120) min -120
	Steering2 = Steering
	Height = (Height Max 25) min -25
	Height2 = Height
	YSpeed = (YSpeed Max 120) min -120
	XSpeed = (XSpeed Max 120) min -120

	GaitSpeed = (GaitSpeed max 20) min 3
	StepFlag = (GaitSpeedTmp - 2) max 3
	LegUpShift = (LegUpShift max 70) min 20	
	if LegUpShift > 45 then
		GaitKind = 1
		GaitSpeed = GaitSpeed min 4
	else
		GaitKind = 0
	endif
#[Control][PS2]	
	if DualShock(1).bit4 = 0 then 	;Up Button test
		LegUpShift = (LegUpShift + 1) max LegUpShiftMax	
	elseif DualShock(1).bit6 = 0	;Down Button test
		LegUpShift = (LegUpShift - 1) min LegUpShiftMin	
	else
		goto NoSound
	endif

	GaitKind = LegUpShift > 45
#[IDE][Basic Micro IDE V02.2.1.1]	
	Sound 9,[20\(LegUpShift * 10 + 400 + GaitKind * 200)]
#[IDE][Basic Micro IDE V05.3.0.0]		
	Sound 9,[20000\(LegUpShift * 10 + 400 + GaitKind * 200)]
#[IDE][Basic Micro PRO IDE V08.0.1.7]
	Sound 9,[20\(LegUpShift * 10 + 400 + GaitKind * 200)]
#[IDE]	
	MovesDelay = 8
NoSound

	if (DualShock(1).bit5 = 0) and LastButton(0).bit5 then 	;Right Button test
		GaitSpeedTmp = (GaitSpeedTmp + 1) max 20
	elseif (DualShock(1).bit7 = 0) and LastButton(0).bit7	;Left Button test
		GaitSpeedTmp = (GaitSpeedTmp - 1) min 3
	else
		goto NoSound1
	endif
#[IDE][Basic Micro IDE V02.2.1.1]
	Sound 9,[20\(50-GaitSpeedTmp) * 35 + 400]
#[IDE][Basic Micro IDE V05.3.0.0]
	Sound 9,[20000\(50-GaitSpeedTmp) * 35 + 400]
#[IDE][Basic Micro PRO IDE V08.0.1.7]
	Sound 9,[20\(50-GaitSpeedTmp) * 35 + 400]
#[IDE]
	MovesDelay = 8
NoSound1

	GaitSpeed = GaitSpeedTmp min (3 + GaitKind)

	for Index = 3 to 6
		if DualShock(Index) > (128 + DeadZone) then
			DualShock(Index) = DualShock(Index) - DeadZone/2
		elseif DualShock(Index) < (128 - DeadZone)
			DualShock(Index) = DualShock(Index) + DeadZone/2
		else
			DualShock(Index) = 128
		endif
	next

#[H3 Body][Round]
	YSpeed = DualShock(3) - 128		; Right Stick Horizontal

	XSpeed = DualShock(4) - 128		; Right Stick Vertical
#[H3 Body][Inline]
#[IDE][Basic Micro IDE V02.2.1.1]
	YSpeed = DualShock(3) - 128		; Right Stick Horizontal
	if (YSpeed < 0) then	; **** due to bug in Basic ****
		YSpeed = -((-YSpeed)*3/5)
	else
		YSpeed = YSpeed*3/5
	endif
	
	XSpeed = (DualShock(4) - 128)	; Right Stick Vertical
	if (XSpeed < 0) then	; **** due to bug in Basic ****
		XSpeed = -((-XSpeed)*4/5)
	else
		XSpeed = XSpeed*4/5
	endif	
#[IDE][Basic Micro IDE V05.3.0.0]
	YSpeed = (DualShock(3) - 128)*3/5	; Right Stick Horizontal

	XSpeed = (DualShock(4) - 128)*4/5	; Right Stick Vertical
#[IDE][Basic Micro PRO IDE V08.0.1.7]
	YSpeed = (DualShock(3) - 128)*3/5	; Right Stick Horizontal

	XSpeed = (DualShock(4) - 128)*4/5	; Right Stick Vertical
#[IDE]
#[H3 Body]

	Steering = DualShock(5) - 128		; Left Stick Horizontal

	StepFlag = (GaitSpeedTmp - 2) max 3
	if HeightAdjust or LockLegs then
		Height = LastHeight + (((DualShock(6) - 128) / 5) - LastHeight) / StepFlag; Left Stick Vertical
	endif
#[Control][Serial Port]
	if DualShock(1).bit4 = 0 then 	;Up Button test
		LegUpShift = (LegUpShift + 1) max LegUpShiftMax	
	elseif DualShock(1).bit6 = 0	;Down Button test
		LegUpShift = (LegUpShift - 1) min LegUpShiftMin	
	else
		goto NoSound
	endif

	GaitKind = LegUpShift > 45
#[IDE][Basic Micro IDE V02.2.1.1]	
	Sound 9,[20\(LegUpShift * 10 + 400 + GaitKind * 200)]
#[IDE][Basic Micro IDE V05.3.0.0]		
	Sound 9,[20000\(LegUpShift * 10 + 400 + GaitKind * 200)]
#[IDE][Basic Micro PRO IDE V08.0.1.7]
	Sound 9,[20\(LegUpShift * 10 + 400 + GaitKind * 200)]
#[IDE]	
	MovesDelay = 8
NoSound

	if (DualShock(1).bit5 = 0) and LastButton(0).bit5 then 	;Right Button test
		GaitSpeedTmp = (GaitSpeedTmp + 1) max 20
	elseif (DualShock(1).bit7 = 0) and LastButton(0).bit7	;Left Button test
		GaitSpeedTmp = (GaitSpeedTmp - 1) min 3
	else
		goto NoSound1
	endif
#[IDE][Basic Micro IDE V02.2.1.1]
	Sound 9,[20\(50-GaitSpeedTmp) * 35 + 400]
#[IDE][Basic Micro IDE V05.3.0.0]
	Sound 9,[20000\(50-GaitSpeedTmp) * 35 + 400]
#[IDE][Basic Micro PRO IDE V08.0.1.7]
	Sound 9,[20\(50-GaitSpeedTmp) * 35 + 400]
#[IDE]
	MovesDelay = 8
NoSound1

	GaitSpeed = GaitSpeedTmp min (3 + GaitKind)

	for Index = 3 to 6
		if DualShock(Index) > (128 + DeadZone) then
			DualShock(Index) = DualShock(Index) - DeadZone/2
		elseif DualShock(Index) < (128 - DeadZone)
			DualShock(Index) = DualShock(Index) + DeadZone/2
		else
			DualShock(Index) = 128
		endif
	next

#[H3 Body][Round]
	YSpeed = DualShock(3) - 128		; Right Stick Horizontal

	XSpeed = DualShock(4) - 128		; Right Stick Vertical
#[H3 Body][Inline]
#[IDE][Basic Micro IDE V02.2.1.1]
	YSpeed = DualShock(3) - 128		; Right Stick Horizontal
	if (YSpeed < 0) then	; **** due to bug in Basic ****
		YSpeed = -((-YSpeed)*3/5)
	else
		YSpeed = YSpeed*3/5
	endif
	
	XSpeed = (DualShock(4) - 128)	; Right Stick Vertical
	if (XSpeed < 0) then	; **** due to bug in Basic ****
		XSpeed = -((-XSpeed)*4/5)
	else
		XSpeed = XSpeed*4/5
	endif	
#[IDE][Basic Micro IDE V05.3.0.0]
	YSpeed = (DualShock(3) - 128)*3/5	; Right Stick Horizontal

	XSpeed = (DualShock(4) - 128)*4/5	; Right Stick Vertical
#[IDE][Basic Micro PRO IDE V08.0.1.7]
	YSpeed = (DualShock(3) - 128)*3/5	; Right Stick Horizontal

	XSpeed = (DualShock(4) - 128)*4/5	; Right Stick Vertical
#[IDE]
#[H3 Body]

	Steering = DualShock(5) - 128		; Left Stick Horizontal

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

	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)
#[IDE][Basic Micro IDE V02.2.1.1]
		if XSpeed > 0 then
			TmpCos = XSpeed * 127 / DCoord
		else
			TmpCos = -((-XSpeed) * 127 / DCoord) ; **** due to bug in Basic ****
		endif
#[IDE][Basic Micro IDE V05.3.0.0]
		TmpCos = XSpeed * 127 / DCoord
#[IDE][Basic Micro PRO IDE V08.0.1.7]
		TmpCos = XSpeed * 127 / DCoord
#[IDE]
		gosub ACos
		DAngle = TmpAngle
		if YSpeed > 0 then
			DAngle = 256 - DAngle
		endif

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

		; 180° Deck Code
#[H3 Body][Round]
		if (Dangle < 64 or Dangle > 191) and (DCoord > 0) then
			DeckTilt_Pulse = 1680 - Dcoord
		else
			DeckTilt_Pulse = 1380 + Dcoord
		endif
#[H3 Body]
		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

#[H3 Body][Round]
				HeightShift(0) = Roll/10
#[H3 Body][Inline]	
				HeightShift(0) = Roll/5
#[H3 Body]
				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)

#[H3 Body][Round]			
				HeightShift(1) = HeightShift(0) + HeightShift(2)
				HeightShift(4) = HeightShift(3) + HeightShift(5)
#[H3 Body][Inline]
				HeightShift(1) = (HeightShift(0) + HeightShift(2))/2
				HeightShift(4) = (HeightShift(3) + HeightShift(5))/2	
#[H3 Body]
			endif
#[Control][PS2]
			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
#[Control][Serial Port]
			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
#[Control][Autonomous]
			LittleGripOCPulse = ((LittleGripOCPulse + (Buttons2.bit3 * 50) - (Buttons2.bit2 * 50)) |
				min LGripOC_PulseMin) max LGripOC_PulseMax
			LittleGripLRPulse = ((LittleGripLRPulse + (Buttons2.bit1 * 50) - (Buttons2.bit0 * 50)) |
				min LGripLR_PulseMin) max LGripLR_PulseMax
#[Control]
			GaitSpeedTmp = StepFlag + 2
		else
			StepFlag = NbSteps - Steps + 1
		endif

#[H3 Body][Round]
		for Index = 0 to 5
#[IDE][Basic Micro IDE V02.2.1.1]
			XPos2(Index) = -(DCoord * COS(DAngle + (Index * 43 + 21)) / 300) ; 43 => 60 degrees
#[IDE][Basic Micro IDE V05.3.0.0]
			XPos2(Index) = -(DCoord * COS(DAngle + (Index * 43 + 21)) / 300) ; 43 => 60 degrees
#[IDE][Basic Micro PRO IDE V08.0.1.7]
			XPos2(Index) = -(DCoord * SIN(DAngle + (Index * 43 + 21) + 64) / 300) ; 43 => 60 degrees
#[IDE]
			ZPos2(Index) = -(DCoord * SIN(DAngle + (Index * 43 + 21)) / 300) ; 43 => 60 degrees
		next
#[H3 Body][Inline]
		SteeringLimit = 128 - ((ABS(XSpeed) + ABS(YSpeed))/2)
		Steering = (Steering max SteeringLimit) min -SteeringLimit
		
#[IDE][Basic Micro IDE V02.2.1.1]
		if (Steering < 0) then
			Steering = -((-Steering) / 5)	; **** due to bug in Basic ****
		else
			Steering = Steering / 5
		endif
		
		for Index2 = 0 to 1
			for Index = 0 to 2
				XPos2(Index + Index2*3) = -(DCoord * COS(DAngle + 64 + Index2*128) / 300) - ((Index - 1) * Steering)
#[IDE][Basic Micro IDE V05.3.0.0]
		for Index2 = 0 to 1
			for Index = 0 to 2
				XPos2(Index + Index2*3) = -(DCoord * COS(DAngle + 64 + Index2*128) / 300) - ((Index - 1) * Steering/5)
#[IDE][Basic Micro PRO IDE V08.0.1.7]
		for Index2 = 0 to 1
			for Index = 0 to 2
				XPos2(Index + Index2*3) = -(DCoord * SIN(DAngle + 128 + Index2*128) / 300) - ((Index - 1) * Steering/5)
#[IDE]		
				ZPos2(Index + Index2*3) = -(DCoord * SIN(DAngle + 64 + Index2*128) / 300)
			next
		next
#[H3 Body]

		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
#[IDE][Basic Micro IDE V02.2.1.1]
			if Zpos(Index) > 0 then
				Distance = SQR((XPos(Index) * XPos(Index) + ZPos(Index) * ZPos(Index)) / 4) * 2
			else
				Distance = SQR((XPos(Index) * XPos(Index) |
					+ (-ZPos(Index)) * (-ZPos(Index))) / 4) * 2 ; **** due to bug in Basic ****
			endif
			if ZPos(Index) > 0 then
				TmpCos = ZPos(Index) * 127 / Distance
			else
				TmpCos = -(-ZPos(Index) * 127 / Distance) ; **** due to bug in Basic ****
			endif
#[IDE][Basic Micro IDE V05.3.0.0]
			Distance = SQR((XPos(Index) * XPos(Index) + ZPos(Index) * ZPos(Index)) / 4) * 2
			TmpCos = ZPos(Index) * 127 / Distance
#[IDE][Basic Micro PRO IDE V08.0.1.7]
			Distance = SQR((XPos(Index) * XPos(Index) + ZPos(Index) * ZPos(Index)) / 4) * 2
			TmpCos = ZPos(Index) * 127 / Distance
#[IDE]			

			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
#[IDE][Basic Micro IDE V02.2.1.1]
			TmpYPos = (-((-YPos(Index)) * 127)) max 0 ; **** due to bug in Basic ****
			if ZPos(Index) > 0 then
				TmpZPos = ZPos(Index) * 127
			else
				TmpZPos = -((-ZPos(Index)) * 127) ; **** due to bug in Basic ****
			endif
#[IDE][Basic Micro IDE V05.3.0.0]			
			TmpYPos = (YPos(Index) * 127) max 0
			TmpZPos = ZPos(Index) * 127
#[IDE][Basic Micro PRO IDE V08.0.1.7]
			TmpYPos = (YPos(Index) * 127) max 0
			TmpZPos = ZPos(Index) * 127
#[IDE]			
			TmpDistance = ((Distance - HipV_HipH) * 127) min 0
			
#[IDE][Basic Micro IDE V02.2.1.1]			
			TmpSEWSEW = ((-TmpYPos) * (-TmpYPos) + TmpDistance * TmpDistance) / 16129 ; **** due to bug in Basic ****
#[IDE][Basic Micro IDE V05.3.0.0]			
			TmpSEWSEW = (TmpYPos * TmpYPos + TmpDistance * TmpDistance) / 16129
#[IDE][Basic Micro PRO IDE V08.0.1.7]
			TmpSEWSEW = (TmpYPos * TmpYPos + TmpDistance * TmpDistance) / 16129
#[IDE]			
			TmpSEW = SQR(TmpSEWSEW)

			TmpCos = (Femur_Length * Femur_Length + Tibia_Length * Tibia_Length - TmpSEWSEW)
#[IDE][Basic Micro IDE V02.2.1.1]			
			if TmpCos > 0 then
				TmpCos = TmpCos * 127 / (2 * Femur_Length * Tibia_Length)
			else
				TmpCos = -((-TmpCos) * 127 / (2 * Femur_Length * Tibia_Length)) ; **** due to bug in Basic ****
			endif 
#[IDE][Basic Micro IDE V05.3.0.0]			
			TmpCos = TmpCos * 127 / (2 * Femur_Length * Tibia_Length)
#[IDE][Basic Micro PRO IDE V08.0.1.7]
			TmpCos = TmpCos * 127 / (2 * Femur_Length * Tibia_Length)
#[IDE]			
			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) 
#[IDE][Basic Micro IDE V02.2.1.1]			
			if TmpCos > 0 then
				TmpCos = TmpCos * 127 / (2 * Femur_Length * TmpSEW) 
			else
				TmpCos = -((-TmpCos) * 127 / (2 * Femur_Length * TmpSEW)) ; **** due to bug in Basic ****
			endif
#[IDE][Basic Micro IDE V05.3.0.0]		    
			TmpCos = TmpCos * 127 / (2 * Femur_Length * TmpSEW) 
#[IDE][Basic Micro PRO IDE V08.0.1.7]
			TmpCos = TmpCos * 127 / (2 * Femur_Length * TmpSEW) 
#[IDE]		    
		    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

#[IDE][Basic Micro IDE V02.2.1.1]		
		serout SSC32,i38400,["#",RRHH,RRHH2,"P",DEC HipH_Pulse(0),"#",RRHV,RRHV2,"P",DEC HipV_Pulse(0), |
#[IDE][Basic Micro IDE V05.3.0.0]		
		serout SSC32,I8N1_38400,["#",RRHH,RRHH2,"P",DEC HipH_Pulse(0),"#",RRHV,RRHV2,"P",DEC HipV_Pulse(0), |
#[IDE][Basic Micro PRO IDE V08.0.1.7]
		serout SSC32,i38400,["#",RRHH,RRHH2,"P",DEC HipH_Pulse(0),"#",RRHV,RRHV2,"P",DEC HipV_Pulse(0), |
#[IDE]		
			"#",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), |
#[IDE][Basic Micro IDE V02.2.1.1]
#[H3 Body][Round]
			"#",DeckP,DeckP2,"P",DEC Deck_Pulse,"#",DeckTilt,DeckTilt2,"P",DEC DeckTilt_Pulse, |
#[Control][PS2]
			"#",LGripOC,LGripOC2,"P",DEC LittleGripOCPulse,"#",LGripLR,LGripLR2,"P",DEC LittleGripLRPulse,"T190",13]
#[Control][Autonomous]
			"#",LGripOC,LGripOC2,"P",DEC LittleGripOCPulse,"#",LGripLR,LGripLR2,"P",DEC LittleGripLRPulse,"T190",13]
#[Control][Serial Port]
			"#",LGripOC,LGripOC2,"P",DEC LittleGripOCPulse,"#",LGripLR,LGripLR2,"P",DEC LittleGripLRPulse,"T210",13]
#[Control]
#[H3 Body][Inline]
			"#",DeckP,DeckP2,"P",DEC Deck_Pulse,"#",LGripOC,LGripOC2,"P",DEC LittleGripOCPulse, |
#[Control][PS2]
			"#",LGripLR,LGripLR2,"P",DEC LittleGripLRPulse,"T190",13]
#[Control][Autonomous]
			"#",LGripLR,LGripLR2,"P",DEC LittleGripLRPulse,"T190",13]
#[Control][Serial Port]
			"#",LGripLR,LGripLR2,"P",DEC LittleGripLRPulse,"T210",13]
#[Control]
#[H3 Body]
#[IDE][Basic Micro IDE V05.3.0.0]
#[H3 Body][Round]
			"#",DeckP,DeckP2,"P",DEC Deck_Pulse,"#",DeckTilt,DeckTilt2,"P",DEC DeckTilt_Pulse, |
#[Control][PS2]
			"#",LGripOC,LGripOC2,"P",DEC LittleGripOCPulse,"#",LGripLR,LGripLR2,"P",DEC LittleGripLRPulse,"T180",13]
#[Control][Autonomous]
			"#",LGripOC,LGripOC2,"P",DEC LittleGripOCPulse,"#",LGripLR,LGripLR2,"P",DEC LittleGripLRPulse,"T180",13]
#[Control][Serial Port]
			"#",LGripOC,LGripOC2,"P",DEC LittleGripOCPulse,"#",LGripLR,LGripLR2,"P",DEC LittleGripLRPulse,"T200",13]
#[Control]
#[H3 Body][Inline]
			"#",DeckP,DeckP2,"P",DEC Deck_Pulse,"#",LGripOC,LGripOC2,"P",DEC LittleGripOCPulse, |
#[Control][PS2]
			"#",LGripLR,LGripLR2,"P",DEC LittleGripLRPulse,"T180",13]
#[Control][Autonomous]
			"#",LGripLR,LGripLR2,"P",DEC LittleGripLRPulse,"T180",13]
#[Control][Serial Port]
			"#",LGripLR,LGripLR2,"P",DEC LittleGripLRPulse,"T200",13]
#[Control]
#[H3 Body]
#[IDE][Basic Micro PRO IDE V08.0.1.7]
#[H3 Body][Round]
			"#",DeckP,DeckP2,"P",DEC Deck_Pulse,"#",DeckTilt,DeckTilt2,"P",DEC DeckTilt_Pulse, |
#[Control][PS2]
			"#",LGripOC,LGripOC2,"P",DEC LittleGripOCPulse,"#",LGripLR,LGripLR2,"P",DEC LittleGripLRPulse,"T190",13]
			pause 100
#[Control][Autonomous]
			"#",LGripOC,LGripOC2,"P",DEC LittleGripOCPulse,"#",LGripLR,LGripLR2,"P",DEC LittleGripLRPulse,"T190",13]
			pause 100
#[Control][Serial Port]
			"#",LGripOC,LGripOC2,"P",DEC LittleGripOCPulse,"#",LGripLR,LGripLR2,"P",DEC LittleGripLRPulse,"T210",13]
			pause 120
#[Control]
#[H3 Body][Inline]
			"#",DeckP,DeckP2,"P",DEC Deck_Pulse,"#",LGripOC,LGripOC2,"P",DEC LittleGripOCPulse, |
#[Control][PS2]
			"#",LGripLR,LGripLR2,"P",DEC LittleGripLRPulse,"T190",13]
			pause 100
#[Control][Autonomous]
			"#",LGripLR,LGripLR2,"P",DEC LittleGripLRPulse,"T190",13]
			pause 100
#[Control][Serial Port]
			"#",LGripLR,LGripLR2,"P",DEC LittleGripLRPulse,"T210",13]
			pause 120
#[Control]
#[H3 Body]
#[IDE]
	else
#[Control][Autonomous]
		Initialized = 1
#[Control]
		pause 144
#[Auto Legs Down][Disable]
		if MovesDelay < 1 then
			MovesDelay = 1
#[Auto Legs Down]
#[Control][PS2]
#[Auto Legs Down][1 Sec]
		if MovesDelay < -6 then
			MovesDelay = (NbSteps - GaitKind) - Steps + 1
#[Auto Legs Down][3 Sec]
		if MovesDelay < -17 then
			MovesDelay = (NbSteps - GaitKind) - Steps + 1
#[Auto Legs Down][5 Sec]
		if MovesDelay < -28 then
			MovesDelay = (NbSteps - GaitKind) - Steps + 1
#[Auto Legs Down][10 Sec]
		if MovesDelay < -56 then
			MovesDelay = (NbSteps - GaitKind) - Steps + 1
#[Auto Legs Down]
#[Control][Autonomous]
#[Auto Legs Down][1 Sec]
		if MovesDelay < -6 then
			MovesDelay = (NbSteps - GaitKind) - Steps + 1
#[Auto Legs Down][3 Sec]
		if MovesDelay < -17 then
			MovesDelay = (NbSteps - GaitKind) - Steps + 1
#[Auto Legs Down][5 Sec]
		if MovesDelay < -28 then
			MovesDelay = (NbSteps - GaitKind) - Steps + 1
#[Auto Legs Down][10 Sec]
		if MovesDelay < -56 then
			MovesDelay = (NbSteps - GaitKind) - Steps + 1
#[Auto Legs Down]
#[Control][Serial Port]
#[Auto Legs Down][1 Sec]
		if MovesDelay < -5 then
			MovesDelay = (NbSteps - GaitKind) - Steps + 1
#[Auto Legs Down][3 Sec]
		if MovesDelay < -15 then
			MovesDelay = (NbSteps - GaitKind) - Steps + 1
#[Auto Legs Down][5 Sec]
		if MovesDelay < -25 then
			MovesDelay = (NbSteps - GaitKind) - Steps + 1
#[Auto Legs Down][10 Sec]
		if MovesDelay < -50 then
			MovesDelay = (NbSteps - GaitKind) - Steps + 1
#[Auto Legs Down]
#[Control]
		EndIf	
	endif
	MovesDelay = MovesDelay - 1
OffEnd
#[Control][PS2]	
	LastButton(0) = DualShock(1)
	LastButton(1) = DualShock(2)
#[Control][Serial Port]	
	LastButton(0) = DualShock(1)
	LastButton(1) = DualShock(2)
#[Control][Autonomous]	
	Buttons = 0
	Buttons2 = 0
#[Control]	

	goto main

;--------------------------------------------------------------------
;-------------Sub H3 Init
H3Init
	LockLegs = 0
	NbSteps = 4
#[Control][PS2]
	GaitSpeedTmp = NbSteps
#[Control][Serial Port]
	GaitSpeedTmp = NbSteps
#[Control][Autonomous]
	GaitSpeed = NbSteps
#[Control]
	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
#[Control][Autonomous]
	Initialized = 0
#[Control]
	; then All1500

;-------------Sub All1500
All1500
#[Control][PS2]
	HeightAdjust = 1
#[Control][Serial Port]
	HeightAdjust = 1
#[Control]
	height = 0
	LastHeight = 0

#[IDE][Basic Micro IDE V02.2.1.1]
#[H3 Body][Round]
	for Index = 0 to 27	;preserve servo 28 (Deck Tilt),29 and 30 (Little Gripper),31 (Deck)
#[H3 Body][Inline]
	for Index = 0 to 27	;preserve servo 28 (free),29 and 30 (Little Gripper),31 (Deck)
#[H3 Body]
		serout SSC32,i38400,["#",DEC Index,"P1500"]
	next
	serout SSC32,i38400,["T576",13]
#[IDE][Basic Micro IDE V05.3.0.0]
#[H3 Body][Round]
	for Index = 0 to 27	;preserve servo 28 (Deck Tilt),29 and 30 (Little Gripper),31 (Deck)
#[H3 Body][Inline]
	for Index = 0 to 27	;preserve servo 28 (free),29 and 30 (Little Gripper),31 (Deck)
#[H3 Body]
		serout SSC32,I8N1_38400,["#",DEC Index,"P1500"]
	next
	serout SSC32,I8N1_38400,["T576",13]
#[IDE][Basic Micro PRO IDE V08.0.1.7]
#[H3 Body][Round]
	for Index = 0 to 27	;preserve servo 28 (Deck Tilt),29 and 30 (Little Gripper),31 (Deck)
#[H3 Body][Inline]
	for Index = 0 to 27	;preserve servo 28 (free),29 and 30 (Little Gripper),31 (Deck)
#[H3 Body]
		serout SSC32,i38400,["#",DEC Index,"P1500"]
	next
	serout SSC32,i38400,["T576",13]
#[IDE]

	pause 576

	Steps = NbSteps
	; then Flat Body

;-------------Sub Flat Body
FlatBody
	Roll = 0
	Pitch = 0
	for Index = 0 to 5
		HeightShift(Index) = 0
	next
#[Control][Autonomous]
	LastLeftStickMode = LeftStickMode
#[Control]

	return	

;-------------Sub InitPos
InitPos
#[IDE][Basic Micro IDE V02.2.1.1]
	serout SSC32,i38400,["#",RRHV,RRHV2,"P2100#",RRK,RRK2,"P1800#",MRHV,MRHV2,"P2100#",MRK,MRK2,"P1800#", |
#[IDE][Basic Micro IDE V05.3.0.0]	
	serout SSC32,I8N1_38400,["#",RRHV,RRHV2,"P2100#",RRK,RRK2,"P1800#",MRHV,MRHV2,"P2100#",MRK,MRK2,"P1800#", |
#[IDE][Basic Micro PRO IDE V08.0.1.7]
	serout SSC32,i38400,["#",RRHV,RRHV2,"P2100#",RRK,RRK2,"P1800#",MRHV,MRHV2,"P2100#",MRK,MRK2,"P1800#", |
#[IDE]	
		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
#[IDE][Basic Micro IDE V02.2.1.1]	
	serout SSC32,i38400,["#",RRHH,RRHH2,"P1500#",MRHH,MRHH2,"P1500#",FRHH,FRHH2,"P1500#", |
#[IDE][Basic Micro IDE V05.3.0.0]	
	serout SSC32,I8N1_38400,["#",RRHH,RRHH2,"P1500#",MRHH,MRHH2,"P1500#",FRHH,FRHH2,"P1500#", |
#[IDE][Basic Micro PRO IDE V08.0.1.7]
	serout SSC32,i38400,["#",RRHH,RRHH2,"P1500#",MRHH,MRHH2,"P1500#",FRHH,FRHH2,"P1500#", |
#[IDE]	
		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
#[IDE][Basic Micro IDE V02.2.1.1]
		read -TmpCos,TmpAngle
		TmpAngle = 127 - TmpAngle
	else
		read TmpCos,TmpAngle
#[IDE][Basic Micro IDE V05.3.0.0]
		read -TmpCos,TmpAngle
		TmpAngle = 127 - TmpAngle
	else
		read TmpCos,TmpAngle
#[IDE][Basic Micro PRO IDE V08.0.1.7]
		TmpAngle = 128 - ArcCos(-TmpCos)
	else
		TmpAngle = ArcCos(TmpCos)
#[IDE]
	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
#[Control][PS2]
#[H3 Body][Round]
		Rotate(Index) = Rotate(Index) + (Steering/5 - Rotate(Index))/StepFlag
#[H3 Body][Inline]
#[IDE][Basic Micro IDE V02.2.1.1]	
		Rotate(Index) = Rotate(Index) + (Steering/2 - Rotate(Index))/StepFlag
#[IDE][Basic Micro IDE V05.3.0.0]
		Rotate(Index) = Rotate(Index) + (Steering/10 - Rotate(Index))/StepFlag
#[IDE][Basic Micro PRO IDE V08.0.1.7]
		Rotate(Index) = Rotate(Index) + (Steering/2 - Rotate(Index))/StepFlag
#[IDE]
#[H3 Body]
		YPos(Index) = -Tibia_Length + Height + HeightShift(Index)
#[Control][Serial Port]
#[H3 Body][Round]
		Rotate(Index) = Rotate(Index) + (Steering/5 - Rotate(Index))/StepFlag
#[H3 Body][Inline]	
#[IDE][Basic Micro IDE V02.2.1.1]	
		Rotate(Index) = Rotate(Index) + (Steering/2 - Rotate(Index))/StepFlag
#[IDE][Basic Micro IDE V05.3.0.0]
		Rotate(Index) = Rotate(Index) + (Steering/10 - Rotate(Index))/StepFlag
#[IDE][Basic Micro PRO IDE V08.0.1.7]
		Rotate(Index) = Rotate(Index) + (Steering/2 - Rotate(Index))/StepFlag
#[IDE]
#[H3 Body]
		YPos(Index) = -Tibia_Length + Height + HeightShift(Index)
#[Control][Autonomous]
#[H3 Body][Round]
		Rotate(Index) = Rotate(Index) + (Steering2/5 - Rotate(Index))/StepFlag
#[H3 Body][Inline]	
#[IDE][Basic Micro IDE V02.2.1.1]	
		Rotate(Index) = Rotate(Index) + (Steering2/2 - Rotate(Index))/StepFlag
#[IDE][Basic Micro IDE V05.3.0.0]
		Rotate(Index) = Rotate(Index) + (Steering2/10 - Rotate(Index))/StepFlag
#[IDE][Basic Micro PRO IDE V08.0.1.7]
		Rotate(Index) = Rotate(Index) + (Steering2/2 - Rotate(Index))/StepFlag
#[IDE]
#[H3 Body]
		YPos(Index) = -Tibia_Length + Height2 + HeightShift(Index)
#[Control]
		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
#[Control][PS2]
#[H3 Body][Round]
		Rotate(Index) = Rotate(Index) + (-Steering/5 - Rotate(Index))/StepFlag
#[H3 Body][Inline]
#[IDE][Basic Micro IDE V02.2.1.1]	
		Rotate(Index) = Rotate(Index) + (-Steering/2 - Rotate(Index))/StepFlag
#[IDE][Basic Micro IDE V05.3.0.0]
		Rotate(Index) = Rotate(Index) + (-Steering/10 - Rotate(Index))/StepFlag
#[IDE][Basic Micro PRO IDE V08.0.1.7]
		Rotate(Index) = Rotate(Index) + (-Steering/2 - Rotate(Index))/StepFlag
#[IDE]	
#[H3 Body]
		YPos(Index) = -Tibia_Length + Height + (LegUpShift * Freeze)
#[Control][Serial Port]
#[H3 Body][Round]
		Rotate(Index) = Rotate(Index) + (-Steering/5 - Rotate(Index))/StepFlag
#[H3 Body][Inline]
#[IDE][Basic Micro IDE V02.2.1.1]	
		Rotate(Index) = Rotate(Index) + (-Steering/2 - Rotate(Index))/StepFlag
#[IDE][Basic Micro IDE V05.3.0.0]
		Rotate(Index) = Rotate(Index) + (-Steering/10 - Rotate(Index))/StepFlag
#[IDE][Basic Micro PRO IDE V08.0.1.7]
		Rotate(Index) = Rotate(Index) + (-Steering/2 - Rotate(Index))/StepFlag
#[IDE]	
#[H3 Body]
		YPos(Index) = -Tibia_Length + Height + (LegUpShift * Freeze)
#[Control][Autonomous]
#[H3 Body][Round]
		Rotate(Index) = Rotate(Index) + (-Steering2/5 - Rotate(Index))/StepFlag
#[H3 Body][Inline]
#[IDE][Basic Micro IDE V02.2.1.1]	
		Rotate(Index) = Rotate(Index) + (-Steering2/2 - Rotate(Index))/StepFlag
#[IDE][Basic Micro IDE V05.3.0.0]
		Rotate(Index) = Rotate(Index) + (-Steering2/10 - Rotate(Index))/StepFlag
#[IDE][Basic Micro PRO IDE V08.0.1.7]
		Rotate(Index) = Rotate(Index) + (-Steering2/2 - Rotate(Index))/StepFlag
#[IDE]
#[H3 Body]
		YPos(Index) = -Tibia_Length + Height2 + (LegUpShift * Freeze)
#[Control]
		XPos(Index) = XPos(Index) + (-XPos2(Index) - (XPos(Index) - (HipV_HipH + Femur_Length)))/StepFlag
		ZPos(Index) = ZPos(Index) + (-ZPos2(Index) - ZPos(Index))/StepFlag
	next

	return	
	
;--------------------------------------------------------------------