;DEFINE CONSTANTS

;PS2 Controller
DAT 	con	P0
CMD 	con P1
ATT	 	con P2
CLK 	con P3

;SSC-32 Data Transmit Pin, HPS RF Transmitter Pins
OUT			con	P8
PDN			con	P9
CTS			con	P10
CS2			con	P11
CS1			con	P12

DeadZone	con 28
PadMode 	con $79

;Deck
Deck_PulseMin con 600
Deck_PulseMax con 2400	

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

;DEFINE SERVO MOTOR PINS
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"

DeckP 		con "3"	;Deck Panning servo : pin 31
DeckP2 		con "1"
DeckTilt 	con "2"; Additional Tilt servo for 180° Deck
DeckTilt2 	con "8"

;DECLARE VARIABLES
;1-bit variables
FlipFlap 		var	Bit
FlagOff 		var Bit
HeightAdjust 	var Bit
GaitKind 		var Bit
Freeze 			var Bit
Tripod 			var Bit
LockLegs 		var Bit
;Unsigned byte variables
Index 			var Byte
DualShock 		var Byte(7)
DS2Mode 		var Byte
LastButton 		var Byte(2)
Steps 			var Byte
KneeShift 		var Byte(7)
HeightLimit 	var Byte
DCoord 			var Byte
DAngle 			var Byte
NbSteps 		var Byte
GaitSpeed 		var Byte
GaitSpeedTmp 	var Byte
StepFlag 		var Byte
LegUpShift 		var Byte
LeftStickMode 	var Byte
;Signed byte variables
MovesDelay 		var Sbyte
HeightShift 	var Sbyte(6)
Steering 		var Sbyte
Height 			var Sbyte
LastHeight 		var Sbyte
XSpeed 			var Sbyte
YSpeed 			var Sbyte
Roll 			var Sbyte
Pitch 			var Sbyte
Rotate 			var Sbyte(6)
HipH_Angle 		var Sbyte
TmpAngle 		var Sbyte
;Unsigned word variables
TmpSEW 			var Word
HipH_Pulse 		var Word(6)
HipV_Pulse 		var Word(6)
Knee_Pulse 		var Word(6)
Deck_Pulse 		var Word
DeckTilt_Pulse 	var Word
;Signed word variables
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
HipV_Angle 		var Sword
Knee_Angle 		var Sword
;Unsigned double word variables
TmpXPos 		var Long
TmpYPos 		var Long
TmpZPos 		var Long
TmpDistance 	var Long
TmpSEWSEW 		var Long
TmpCos 			var Long

;PROGRAM START
clear
high CLK
LastButton(0) = 255
LastButton(1) = 255
HeightLimit = 25 - ABS(TibiaAngle / 2)
pause 500
;OUTPUT PULSE OFFSET DATA
serout OUT,i38400,["#", |
	RRHH,RRHH2,"po0 #",RRHV,RRHV2,"po0 #",RRK,RRK2,"po0 #", |
	MRHH,MRHH2,"po-100 #",MRHV,MRHV2,"po0 #",MRK,MRK2,"po0 #", |
	FRHH,FRHH2,"po0 #",FRHV,FRHV2,"po0 #",FRK,FRK2,"po0 #", |
	RLHH,RLHH2,"po0 #",RLHV,RLHV2,"po0 #",RLK,RLK2,"po0 #", |
	MLHH,MLHH2,"po0 #",MLHV,MLHV2,"po0 #",MLK,MLK2,"po0 #", |
	FLHH,FLHH2,"po0 #",FLHV,FLHV2,"po0 #",FLK,FLK2,"po0",13]
;SSC-32 TO H3 engine
gosub H3Init
;MAIN LOOP//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
main
	low ATT
	shiftout CMD,CLK,FASTLSBPRE,[$1\8]
	shiftin DAT,CLK,FASTLSBPOST,[DS2Mode\8]
	high ATT
	pause 1

	low ATT
	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 ATT
	pause 1	
	if (DS2Mode <> PadMode) then
		;CONFIG_MODE_ENTER
		low ATT
		shiftout CMD,CLK,FASTLSBPRE,[$1\8,$43\8,$0\8,$1\8,$0\8] 
		high ATT
		pause 1
		;SET_MODE_AND_LOCK
		low ATT
		shiftout CMD,CLK,FASTLSBPRE,[$01\8,$44\8,$00\8,$01\8,$03\8,$00\8,$00\8,$00\8,$00\8] 
		high ATT
		pause 1
		;SET_DS2_NATIVE_MODE
        low ATT
		shiftout CMD,CLK,FASTLSBPRE,[$01\8,$4F\8,$00\8,$FF\8,$FF\8,$03\8,$00\8,$00\8,$00\8] 
		high ATT
		pause 1
		;CONFIG_MODE_EXIT_DS2_NATIVE
		low ATT
		shiftout CMD,CLK,FASTLSBPRE,[$01\8,$43\8,$00\8,$00\8,$5A\8,$5A\8,$5A\8,$5A\8,$5A\8] 
		high ATT
		pause 1
		;CONFIG_MODE_EXIT
		low ATT
		shiftout CMD,CLK,FASTLSBPRE,[$01\8,$43\8,$00\8,$00\8,$00\8,$00\8,$00\8,$00\8,$00\8] 
		high ATT
		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
	;R1 BUTTON TEST
	if (DualShock(2).bit3 = 0) and LastButton(1).bit3 then	
		;Default preset
		Height = 0			
		LegUpShift = 35
		GaitSpeed = 4
	;L1 BUTTON TEST	
	elseif (DualShock(2).bit2 = 0) and LastButton(1).bit2	
		;Tile floor
		Height = 5			
		LegUpShift = 30
		GaitSpeed = 3
	;R2 BUTTON TEST	
	
		
	;L2 BUTTON TEST	
		
		
	else
		goto NoPresets
	endif

	HeightAdjust = 0
	LegUpShift = (LegUpShift max LegUpShiftMax) min LegUpShiftMin 
	GaitKind = LegUpShift > 45
	GaitSpeedTmp = GaitSpeed max 20
	MovesDelay = 8	
NoPresets
	;TRIANGLE BUTTON TEST:ON/OFF SERVOMOTORS
	
	

	if (MovesDelay <= 0) or LockLegs then
		;CIRCLE BUTTON TEST: Reset
		if (DualShock(2).bit5 = 0) and LastButton(1).bit5 then
			gosub All1500
		endif
		;CROSS BUTTON TEST
		
			
		;SQUARE BUTTON TEST	
		
	endif
	;SELECT BUTTON TEST
	
	;START BUTTON TEST
	if (DualShock(1).bit3 = 0) and LastButton(0).bit3 then
		FlagOff = FlagOff ^ 1
		if FlagOff then
			gosub All1500
			gosub InitPos
			pause 144
			for Index = 0 to 31
				serout OUT,i38400,["#",DEC Index,"P0"]
			next
			serout OUT,i38400,[13]
		else
			gosub H3Init
		endif
	endif
	if FlagOff then
		pause 144
		goto OffEnd
	endif
	;L3 BUTTON TEST: Locks ride height
	if (DualShock(1).bit1 = 0) and LastButton(0).bit1 then	
		if LockLegs then
			LeftStickMode = (LeftStickMode + 1) & 3
			gosub FlatBody
		else
			HeightAdjust = HeightAdjust ^ 1
		endif
	endif
	;R3 BUTTON TEST
	
	;UP BUTTON TEST: Increase ride height
	if DualShock(1).bit4 = 0 then 	
		LegUpShift = (LegUpShift + 1) max LegUpShiftMax	
	;DOWN BUTTON TEST: Decrease ride height	
	elseif DualShock(1).bit6 = 0	
		LegUpShift = (LegUpShift - 1) min LegUpShiftMin	
	else
		goto NoSound
	endif
	GaitKind = LegUpShift > 45
	MovesDelay = 8
NoSound
	;RIGHT BUTTON TEST: increase speed
	if (DualShock(1).bit5 = 0) and LastButton(0).bit5 then 	
		GaitSpeedTmp = (GaitSpeedTmp - 1) min 3
	;LEFT BUTTON TEST: decrease speed	
	elseif (DualShock(1).bit7 = 0) and LastButton(0).bit7	
		GaitSpeedTmp = (GaitSpeedTmp + 1) max 20
	else
		goto NoSound1
	endif
	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
	;RIGHT STICK HORIZONTAL
	YSpeed = DualShock(3) - 128		
	;RIGHT STICK VERTICAL
	XSpeed = DualShock(4) - 128		
	;LEFT STICK HORIZONTAL
	Steering = DualShock(5) - 128
			
	StepFlag = (GaitSpeedTmp - 2) max 3
	if HeightAdjust or LockLegs then
		;LEFT STICK VERTICAL
		Height = LastHeight + (((DualShock(6) - 128) / 5) - LastHeight) / StepFlag
	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
			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 OUT,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, |
			"T190",13]
	else
		pause 144
		if MovesDelay < -17 then
			MovesDelay = (NbSteps - GaitKind) - Steps + 1
		EndIf	
	endif
	MovesDelay = MovesDelay - 1
OffEnd
	LastButton(0) = DualShock(1)
	LastButton(1) = DualShock(2)

	goto main

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

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

	gosub InitPos
	MovesDelay = 8
	; then All1500

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

	for Index = 0 to 27	;preserve servo 28 (Deck Tilt),29 and 30 (Little Gripper),31 (Deck)
		serout OUT,i38400,["#",DEC Index,"P1500"]
	next
	serout OUT,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 OUT,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 OUT,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	
	
;--------------------------------------------------------------------