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

;************************************************
;*** Basic Atom with SSC-32 and PS2 DualShock ***
;----------------- 1.44 3DOF-C ------------------
;--------------- PS2 Controller -----------------
;-------------- Round Body (H3-R) ---------------
;********** 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)
;
; 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 28
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 0			;range is -20 (interior) to 20 (exterior), 0 is vertical, this is 'all time' tibia angle

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

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

Steering var Sbyte
Height var Sbyte
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
DeckTilt_Pulse var Word

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

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

;-------------Init
clear

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,i38400,["#", |
	RRHH,RRHH2,"po-36 #",RRHV,RRHV2,"po-26 #",RRK,RRK2,"po8 #", |
	MRHH,MRHH2,"po40 #",MRHV,MRHV2,"po34 #",MRK,MRK2,"po2 #", |
	FRHH,FRHH2,"po72 #",FRHV,FRHV2,"po56 #",FRK,FRK2,"po-80 #", |
	RLHH,RLHH2,"po-56 #",RLHV,RLHV2,"po54 #",RLK,RLK2,"po-40 #", |
	MLHH,MLHH2,"po-76 #",MLHV,MLHV2,"po26 #",MLK,MLK2,"po-30 #", |
	FLHH,FLHH2,"po38 #",FLHV,FLHV2,"po10 #",FLK,FLK2,"po-62",13]

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

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

	if (DualShock(2).bit4 = 0) and LastButton(1).bit4 then	;Triangle Button test
		FlagOff = FlagOff ^ 1
		if FlagOff then
			gosub All1500
			Gosub InitPos
			pause 144
			for Index = 0 to 31
				serout SSC32,i38400,["#",DEC Index,"P0"]
			next
			serout SSC32,i38400,[13]
		else
			gosub H3Init
		endif
	endif
	
	if FlagOff then
		pause 144
		goto OffEnd
	endif

	if (MovesDelay <= 0) or LockLegs then
		if (DualShock(2).bit5 = 0) and LastButton(1).bit5 then	;Circle Button test
			gosub All1500
		endif

		if (DualShock(2).bit6 = 0) and LastButton(1).bit6 then	;Cross Button test
			gosub All1500
			;Attack posture
			serout SSC32,i38400,["#",MRHH,MRHH2,"P1800#",MRHV,MRHV2,"P1700#",MRK,MRK2,"P1700#", |
				MLHH,MLHH2,"P1200#",MLHV,MLHV2,"P1300#",MLK,MLK2,"P1300T288",13]
			pause 288
			serout SSC32,i38400,["#",MRHV,MRHV2,"P1500#",MRK,MRK2,"P1500#",MLHV,MLHV2,"P1500#", |
				MLK,MLK2,"P1500T288",13]
			pause 288
			serout SSC32,i38400,["#",RRHV,RRHV2,"P1800#",RRK,RRK2,"P1800#",MRHV,MRHV2,"P1400#", |
				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
			for Index = 1 to 10
				serout SSC32,i38400,["#",FRHV,FRHV2,"P2000#",FRK,FRK2,"P1600#",FLHV,FLHV2,"P900#", |
					FLK,FLK2,"P1600T144",13]
				pause 144
				serout SSC32,i38400,["#",FRHV,FRHV2,"P2100#",FRK,FRK2,"P1400#",FLHV,FLHV2,"P1000#", |
					FLK,FLK2,"P1400T144",13]
				pause 144
			next
			serout SSC32,i38400,["#",FRHH,FRHH2,"P1500#",FRK,FRK2,"P1500#",FLHH,FLHH2,"P1500#", |
				FLHV,FLHV2,"P900#",FLK,FLK2,"P1500T288",13]
			pause 288
			serout SSC32,i38400,["#",RRHV,RRHV2,"P1500#",RRK,RRK2,"P1500#",MRHV,MRHV2,"P1500#", |
				MRK,MRK2,"P1500#",FRHV,FRHV2,"P1500#",MLHV,MLHV2,"P1500#",MLK,MLK2,"P1500#", |
				FLHV,FLHV2,"P1500#",RLHV,RLHV2,"P1500#",RLK,RLK2,"P1500T576",13]
			pause 576
			serout SSC32,i38400,["#",MRHH,MRHH2,"P1500#",MRHV,MRHV2,"P1700#",MRK,MRK2,"P1700#", |
				MLHH,MLHH2,"P1500#",MLHV,MLHV2,"P1300#",MLK,MLK2,"P1300T288",13]
			pause 288
			serout SSC32,i38400,["#",MRHV,MRHV2,"P1500#",MRK,MRK2,"P1500#",MLHV,MLHV2,"P1500#", |
				MLK,MLK2,"P1500T288",13]
			pause 288
		endif

		if (DualShock(2).bit7 = 0) and LastButton(1).bit7 then	;Square Button test
			gosub All1500
			;Learning to fly posture (uh ? nutty ?...it CAN fly...)
			for Index = 1 to 4
				serout SSC32,i38400,["#",MRHV,MRHV2,"P1900#",MRK,MRK2,"P1800#",MLHV,MLHV2,"P1100#", |
					MLK,MLK2,"P1200T288",13]
				serout SSC32,i38400,["#",RRHV,RRHV2,"P1600#",RRK,RRK2,"P1600#",FRHV,FRHV2,"P1600#", |
					FRK,FRK2,"P1600#",RLHV,RLHV2,"P1400#",RLK,RLK2,"P1400#",FLHV,FLHV2,"P1400#", |
					FLK,FLK2,"P1400T576",13]
				pause 288
				serout SSC32,i38400,["#",MRK,MRK2,"P1200#",MLK,MLK2,"P1800T288",13]
				pause 288
				serout SSC32,i38400,["#",RRHV,RRHV2,"P1400#",RRK,RRK2,"P1400#",MRHV,MRHV2,"P1500#", |
					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
			serout SSC32,i38400,["#",MRHV,MRHV2,"P1800#",MRK,MRK2,"P1800#",MLHV,MLHV2,"P1200#", |
				MLK,MLK2,"P1200T288",13]
			pause 288
			gosub All1500
		endif
	endif

	if (DualShock(1).bit0 = 0) and LastButton(0).bit0 then	;Select Button test
		LockLegs = LockLegs ^ 1
		Sound 9,[100\(2000 + LockLegs * 1000)]
		gosub FlatBody
	endif

	if (DualShock(1).bit3 = 0) and LastButton(0).bit3 then	;Start Button test
		KneeShift(6) = KneeShift(6) ^ KneeShiftPulse
		Sound 9,[100\(800 + KneeShift(6))]
	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
		Sound 9,[100\3500]
	endif

	if (DualShock(1).bit2 = 0) and LastButton(0).bit2 then	;R3 Button test (Horn)
		Sound 9,[800\392, 600\370, 200\0, 800\330, 600\294, 200\0, 300\330, 100\0, 300\330]
	endif

	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
	Sound 9,[20\(LegUpShift * 10 + 400 + GaitKind * 200)]
	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
	Sound 9,[20\(50-GaitSpeedTmp) * 35 + 400]
	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

	YSpeed = DualShock(3) - 128		; Right Stick Horizontal

	XSpeed = DualShock(4) - 128		; Right Stick Vertical

	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

	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 * SIN(DAngle + (Index * 43 + 21) + 64) / 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,i38400,["#",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,"T190",13]
			pause 100
	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,i38400,["#",DEC Index,"P1500"]
	next
	serout SSC32,i38400,["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,i38400,["#",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,i38400,["#",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
		TmpAngle = 128 - ArcCos(-TmpCos)
	else
		TmpAngle = ArcCos(TmpCos)
	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	
	
;--------------------------------------------------------------------