;
; 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 ************
;----------------- 2.44 3DOF-C ------------------
;----------------- Autonomous -------------------
;-------------- 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
;
; Little Gripper support on Pin 29 (Left/Right) and 30 (Open/Close) (only in 'Body move' mode)
;
; 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
;
;************************************************
;
;
;--------------------------------------------------------------------
;-------------Constants
;
;*** SSC-32 card communication on pin 15 ***
SSC32 con p15

;Deck
Deck_PulseMin con 600
Deck_PulseMax con 2400	

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

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

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

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

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

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

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

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

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

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

;ACos
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
Buttons var Byte
Buttons2 var byte
Steps var Byte
FlipFlap var Bit
FlagOff var Bit
MovesDelay var Sbyte

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

Steering var Sbyte
Height var Sbyte
Steering2 var Sbyte
Height2 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
LastLeftStickMode var Byte
KneeShiftOn var Bit

StepCnt var Word
Initialized var Bit

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

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

LittleGripOCPulse = 1500
LittleGripLRPulse = 1500

HeightLimit = 25 - ABS(TibiaAngle / 2)

pause 500

;-------------Init SSC-32 with pulse offsets
serout SSC32,i38400,["#", |
	RRHH,RRHH2,"po-1 #",RRHV,RRHV2,"po-1 #",RRK,RRK2,"po-1 #", |
	MRHH,MRHH2,"po-1 #",MRHV,MRHV2,"po-1 #",MRK,MRK2,"po-1 #", |
	FRHH,FRHH2,"po-1 #",FRHV,FRHV2,"po-1 #",FRK,FRK2,"po-1 #", |
	RLHH,RLHH2,"po-1 #",RLHV,RLHV2,"po0 #",RLK,RLK2,"po-1 #", |
	MLHH,MLHH2,"po-1 #",MLHV,MLHV2,"po-1 #",MLK,MLK2,"po-1 #", |
	FLHH,FLHH2,"po-8 #",FLHV,FLHV2,"po-1 #",FLK,FLK2,"po-1",13]

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

	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)

;--------------------------------------------------------------------

Shunt
	if Buttons.bit4 then	; On/Off
		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 Buttons.bit5 then	; All = 1500
			gosub All1500
		endif

		if Buttons.bit6 and (MovesDelay <= 0) then	; Attack
			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 Buttons.bit7 then	; Learning To Fly
			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 (LockLegs = 0) or (LeftStickMode <> LastLeftStickMode) then gosub FlatBody
	
	if KneeShiftOn then
		KneeShift(6) = KneeShiftPulse
	else
		KneeShift(6) = 0
	endif

	if Buttons.bit0 then	; Horn
		Sound 9,[800\392, 600\370, 200\0, 800\330, 600\294, 200\0, 300\330, 100\0, 300\330]
	endif

	Steering = (Steering Max 120) min -120
	Steering2 = Steering
	Height = (Height Max 25) min -25
	Height2 = Height
	YSpeed = (YSpeed Max 120) min -120
	XSpeed = -50

	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

	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 + (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
			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
		Initialized = 1
		pause 144
		if MovesDelay < -17 then
			MovesDelay = (NbSteps - GaitKind) - Steps + 1
		EndIf	
	endif
	MovesDelay = MovesDelay - 1
OffEnd
	Buttons = 0
	Buttons2 = 0

	goto main

;--------------------------------------------------------------------
;-------------Sub H3 Init
H3Init
	LockLegs = 0
	NbSteps = 4
	GaitSpeed = 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
	Initialized = 0
	; then All1500

;-------------Sub All1500
All1500
	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
	LastLeftStickMode = LeftStickMode

	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) + (Steering2/5 - Rotate(Index))/StepFlag
		YPos(Index) = -Tibia_Length + Height2 + 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) + (-Steering2/5 - Rotate(Index))/StepFlag
		YPos(Index) = -Tibia_Length + Height2 + (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	
	
;--------------------------------------------------------------------