;************************************************
;*** Basic Atom with SSC-32 and PS2 DualShock ***
;*************** H3R program #1 *****************
;********** Bot Board Buzzer support ************
;************************************************
;** Programmer: Laurent Gay, lynxrios@yahoo.fr **
;************************************************
; 
;
; for now -> only H3R support
; let's say that the 2 switches holes of the 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
; nothing is connected on pin 3 !
; Middle right leg : Hip Horizontal : pin 4, Hip Vertical : pin 5, Knee : pin 6
; nothing is connected on pin 7 !
; Front right leg : Hip Horizontal : pin 8, Hip Vertical : pin 9, Knee : pin 10
; pins 11,12,13,14,15 are free too
;
; 3 left legs connections :
; rear left leg : Hip Horizontal : pin 16, Hip Vertical : pin 17, Knee : pin 18
; nothing is connected on pin 19 !
; Middle left leg : Hip Horizontal : pin 20, Hip Vertical : pin 21, Knee : pin 22
; nothing is connected on pin 23 !
; Front left leg : Hip Horizontal : pin 24, Hip Vertical : pin 25, Knee : pin 26
; pins 27,28,29,30,31 are free too
;
; Modification for RC control - depends on your RC transmitter settings
;
;	r_ch1,r_ch2 - transition
;	r_ch4 - rotation
;	r_ch3 - height, legupshift, speed
;	
;	r_ch6 = 2
;			r_ch5 = 2 ;do nothing
;			r_ch5 = 1 ;attack
;			r_ch5 = 3 ;standby
;
;	r_ch6 = 1
;			r_ch5 = 2 ;do nothing
;			r_ch5 = 1 ;nbsteps+,- by r_ch3
;			r_ch5 = 3 ;lefupsfift by r_ch3
;
;	r_ch6 = 3
;			r_ch5 = 2 ;do nothing
;			r_ch5 = 1 ;fly
;			r_ch5 = 3 ;set height by r_ch3
;
;************************************************
;
;
;--------------------------------------------------------------------
;-------------Constants
DeadZone con 10

;Legs
Legs con 6

;RC channels
ch1 con p0
ch2 con p1
ch3 con p2
ch4 con p3
ch5 con p8
ch6 con p10 ;p9 is speaker
rcratio con 5

;LED
red con p4
green con p5
yellow con p6


HipH_AngleDef con 64		;90° (90/360*256)	
HipH_AngleMin con 21		;30°
HipH_AngleMax con 107		;150°
HipH_PulseMin con 910		
HipH_PulseMax con 2090		

HipV_AngleDef con 64		;90° (90/360*256)
HipV_AngleMin con 25		;35°
HipV_AngleMax con 103		;145°
HipV_PulseMin con 960	
HipV_PulseMax con 2040	

Knee_AngleDef con 64		;90° (90/360*256)
Knee_AngleMin con 36		;50°
Knee_AngleMax con 107		;150°
Knee_PulseMin con 1107		
Knee_PulseMax con 2090	

; Lexan Leg Dimensions
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.375" = 108mm (4.375 * 25.4)


LegUpShiftMin con 15
LegUpShiftMax con 35

;ACos
ACS ByteTable 64,64,63,63,63,62,62,62,61,61,61,60,60,60,59,59,|   ; data command before
			  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 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"

;--------------------------------------------------------------------				
;-------------Variables

;RC channels value
r_ch1 var word
r_ch2 var word
r_ch3 var word
r_ch4 var word
r_ch5 var word
r_ch6 var word
RCcheck var byte

Index var Byte
Index2 var Byte
Steps var Byte
Steps2 var byte
FlipFlap var Bit
FlagOff var Bit
YellowFlag var Bit
MovesDelai var Byte
Height var Sbyte

XCoord var Sbyte
YCoord var Sbyte
ZCoord var Sbyte
WCoord var Sbyte
Rotate var Sbyte(6)
DCoord var Byte
DAngle var Byte
NbSteps 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 Sword
HipV_Angle var Sword
Knee_Angle var Sword

HipH_Pulse var word(6)
HipV_Pulse var word(6)
Knee_Pulse var word(6)

LegUpShift var byte

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

;-------------Init
pause 500

clear

gosub RC_check ; it is dangerous to move servos without RC signal
	
NbSteps = 4
StepFlag = NbSteps - 1

pause 500

;-------------Init SSC-32 with pulse offsets
serout p15,i115200,["#",RRHH,RRHH2,"po40 #",RRHV,RRHV2,"po44 #",RRK,RRK2,"po-102 #", |
	MRHH,MRHH2,"po-3 #",MRHV,MRHV2,"po46 #",MRK,MRK2,"po15 #", |
	FRHH,FRHH2,"po07 #",FRHV,FRHV2,"po-19 #",FRK,FRK2,"po26 #", |
	RLHH,RLHH2,"po51 #",RLHV,RLHV2,"po21 #",RLK,RLK2,"po-36 #", |
	MLHH,MLHH2,"po39 #",MLHV,MLHV2,"po12 #",MLK,MLK2,"po-34 #", |
	FLHH,FLHH2,"po06 #",FLHV,FLHV2,"po-25 #",FLK,FLK2,"po-15",13]

;SSC-32 -> H3 engine
gosub H3Init
LegUpShift = (LegUpShiftMin + LegUpShiftMax) / 2

;--------------------------------------------------------------------
;-------------Main loop
main
	gosub RCQuery

	if YellowFlag then	;yellow led flashing as a visual feedback
		low Yellow
		YellowFlag = YellowFlag ^ 1
	else
		high Yellow
		YellowFlag = YellowFlag ^ 1
	endif	
	
	if r_ch5 = 3 and r_ch6 = 2 then	;Standby position
			sound 9,[100\4343]
			Gosub ParkPos
			pause 150
			serout p15,i115200,["#",RRHH,RRHH2,"P0#",RRHV,RRHV2,"P0#",RRK,RRK2,"P0#", | ;"turns off" servos
				MRHH,MRHH2,"P0#",MRHV,MRHV2,"P0#",MRK,MRK2,"P0#", |
				FRHH,FRHH2,"P0#",FRHV,FRHV2,"P0#",FRK,FRK2,"P0#", |
				RLHH,RLHH2,"P0#",RLHV,RLHV2,"P0#",RLK,RLK2,"P0#", |
				MLHH,MLHH2,"P0#",MLHV,MLHV2,"P0#",MLK,MLK2,"P0#", |
				FLHH,FLHH2,"P0#",FLHV,FLHV2,"P0#",FLK,FLK2,"P0#",13]
			repeat ; wait for switch
				sleep 500, sleepactive
				gosub RCQuery
			until r_ch5 = 2		
			sound 9,[100\4343]
			gosub H3Init
		
	endif
	
	
	
	if r_ch5 = 1 and r_ch6 = 2 and (MovesDelai = 0) then	;attack
		gosub All1500
		serout p15,i115200,["#",MRHH,MRHH2,"P1800#",MRHV,MRHV2,"P1700#",MRK,MRK2,"P1700#",MLHH,MLHH2,"P1200#", |
			MLHV,MLHV2,"P1300#",MLK,MLK2,"P1300T288",13]
		pause 280
		serout p15,i115200,["#",MRHV,MRHV2,"P1500#",MRK,MRK2,"P1500#",MLHV,MLHV2,"P1500#",MLK,MLK2,"P1500T288",13]
		pause 280
		serout p15,i115200,["#",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 280
		for Index = 1 to 10
			serout p15,i115200,["#",FRHH,FRHH2,"P1700#",FRHV,FRHV2,"P2000#",FRK,FRK2,"P1600#", |
				FLHH,FLHH2,"P1300#",FLHV,FLHV2,"P900#",FLK,FLK2,"P1600T144",13]
			pause 150
			serout p15,i115200,["#",FRHH,FRHH2,"P1800#",FRHV,FRHV2,"P2100#",FRK,FRK2,"P1400#", |
				FLHH,FLHH2,"P1200#",FLHV,FLHV2,"P1000#",FLK,FLK2,"P1400T144",13]
			pause 150
		next
		serout p15,i115200,["#",FRHH,FRHH2,"P1500#",FRHV,FRHV2,"P2100#",FRK,FRK2,"P1500#", |
			FLHH,FLHH2,"P1500#",FLHV,FLHV2,"P900#",FLK,FLK2,"P1500T288",13]
		pause 280
		serout p15,i115200,["#",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 600
		serout p15,i115200,["#",MRHH,MRHH2,"P1500#",MRHV,MRHV2,"P1700#",MRK,MRK2,"P1700#", |
			MLHH,MLHH2,"P1500#",MLHV,MLHV2,"P1300#",MLK,MLK2,"P1300T288",13]
		pause 280
		serout p15,i115200,["#",MRHV,MRHV2,"P1500#",MRK,MRK2,"P1500#",MLHV,MLHV2,"P1500#",MLK,MLK2,"P1500T288",13]
		pause 280
	endif
	
	if r_ch6=3 and r_ch5=1 and (MovesDelai = 0) then	;Fly
		gosub All1500
				;Learning to fly posture (uh ? nutty ?...it CAN fly...)
		for Index = 1 to 4
			serout p15,i115200,["#",MRHV,MRHV2,"P1900#",MRK,MRK2,"P1800#",MLHV,MLHV2,"P1100#",MLK,MLK2,"P1200T288",13]
			serout p15,i115200,["#",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 550
			serout p15,i115200,["#",MRK,MRK2,"P1200#",MLK,MLK2,"P1800T288",13]
			pause 300
			serout p15,i115200,["#",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 550
		next
		
		serout p15,i115200,["#",MRHV,MRHV2,"P1800#",MRK,MRK2,"P1800#",MLHV,MLHV2,"P1200#",MLK,MLK2,"P1200T288",13]
		pause 500
		gosub All1500
	endif
	
	XCoord = r_ch4
	if XCoord > DeadZone then
		XCoord = XCoord - DeadZone
	elseif XCoord < -DeadZone
		XCoord = XCoord + DeadZone
	else
		XCoord = 0 
	endif
	XCoord = XCoord/5
	
	if r_ch6=3 and r_ch5=3 then	;set height
		YCoord = r_ch3
		if YCoord > DeadZone then
			YCoord = YCoord - DeadZone
		elseif YCoord < -DeadZone
			YCoord = YCoord + DeadZone
		else
			YCoord = 0 
		endif
		if Height <> 0 AND YCoord =0 then
			MovesDelai = 8
		endif
		Height = YCoord/5
	else
		Ycoord = 0
	endif
		
	ZCoord = r_ch1
	if ZCoord > DeadZone then
		ZCoord = ZCoord - DeadZone
	elseif ZCoord < -DeadZone
		ZCoord = ZCoord + DeadZone
	else
		ZCoord = 0 
	endif
	
	WCoord = r_ch2
		if WCoord > DeadZone then
		WCoord = WCoord - DeadZone
	elseif WCoord < -DeadZone
		WCoord = WCoord + DeadZone
	else
		WCoord = 0 
	endif
	

	if r_ch6 = 1 and r_ch5 = 1 then ;speed 
		ycoord = r_ch3
		if ycoord > 20 then 
		NbSteps = (NbSteps + 1) max 5
		endif
		if ycoord < -20 then
		NbSteps = (NbSteps - 1) min 3
		endif
		
	pause 500
	Sound 9,[20\(6-NbSteps) * 300]
	MovesDelai = 8
	StepFlag = NbSteps - 1
	endif


	if  r_ch6 = 1 and r_ch5 = 3 then;legupshift	
		ycoord = r_ch3
		if ycoord >20 then
		LegUpShift = (LegUpShift + 1) max LegUpShiftMax	
		endif
		if ycoord < -20 then
		LegUpShift = (LegUpShift - 1) min LegUpShiftMin
		endif	
	
	pause 500
	Sound 9,[20\(LegUpShift * 8 + 400)]
	MovesDelai = 8
	endif
;H3	
	if XCoord or YCoord or ZCoord or WCoord then
		MovesDelai = 8
	endif
	if MovesDelai then	
		MovesDelai = MovesDelai - 1
		DCoord = SQR(WCoord * WCoord + ZCoord * ZCoord) & 0x7FFF ;bug
		TmpCos = WCoord * 127 / DCoord
		gosub ACos
		DAngle = TmpAngle
		if ZCoord > 0 then
			DAngle = 256 - DAngle
		endif
		for Index = 0 to Legs - 1
			XPos2(Index) = -(DCoord * sin(DAngle + (Index * 43 + 21) + 64) / 300) ; 43 => 60 degrees ;cos(x)=sin(x+64)
			ZPos2(Index) = -(DCoord * sin(DAngle + (Index * 43 + 21)) / 300) ; 43 => 60 degrees ;
			XPos(Index) = XPos(Index) - 102	;102 = 32+70 coz je femur + hipv_hiph
		next
		Steps = Steps + 1
		if Steps > NbSteps then
			Steps = 1
			FlipFlap = FlipFlap ^ 1
		endif
		Steps2 = Steps
		If FlipFlap then
			gosub B_D
			if Steps < NbSteps then 
				gosub A_U
			else
				Steps2 = 1
				gosub A_D
			endif	
		else
			gosub A_D	
			if Steps < NbSteps then 
				gosub B_U
			else
				Steps2 = 1
				gosub B_D
			endif
		endif
		for Index = 0 to Legs - 1
			gosub DistAndBA
			gosub SetAngle
			HipH_Angle = HipH_Angle + Rotate(Index)
			HipH_Pulse(Index) =  (HipH_PulseMax - HipH_PulseMin) * (HipH_Angle - HipH_AngleMin) / (HipH_AngleMax - HipH_AngleMin) + HipH_PulseMin
			if HipH_Pulse(Index) > HipH_PulseMax then
				HipH_Pulse(Index) = HipH_PulseMax
			elseif HipH_Pulse(Index) < HipH_PulseMin
				HipH_Pulse(Index) = HipH_PulseMin 	
			endif
			
			HipV_Pulse(Index) = (HipV_PulseMax - HipV_PulseMin) * (HipV_Angle - HipV_AngleMin) 	/ (HipV_AngleMax - HipV_AngleMin) + HipV_PulseMin 
			if HipV_Pulse(Index) > HipV_PulseMax then
				HipV_Pulse(Index) = HipV_PulseMax
			elseif HipV_Pulse(Index) < HipV_PulseMin
				HipV_Pulse(Index) = HipV_PulseMin		
			endif
			
			Knee_Pulse(Index) = (Knee_PulseMax - Knee_PulseMin) * (Knee_Angle - Knee_AngleMin) 	/ (Knee_AngleMax - Knee_AngleMin) + Knee_PulseMin
			if Knee_Pulse(Index) > Knee_PulseMax then
				Knee_Pulse(Index) = Knee_PulseMax	
			elseif Knee_Pulse(Index) < Knee_PulseMin
				Knee_Pulse(Index) = Knee_PulseMin		
			endif
			Knee_Pulse(Index) = 3000 - Knee_Pulse(Index)
		next
		gosub LegUpdatePosition
	else
		pause 150
	endif				
OffEnd	
	
	goto main
	
;--------------------------------------------------------------------
;-------------Sub Update Position
LegUpdatePosition
	serout p15,i115200,["#",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), |
		"T170",13]	
	
	return
	
;-------------Sub H3 Init
H3Init
	
	HipH_Angle = HipH_AngleDef
	HipV_Angle = HipV_AngleDef
	Knee_Angle = Knee_AngleDef
	TmpDistance = Femur_Length * sin(HipV_Angle) - Tibia_Length * sin(Knee_Angle + HipV_Angle) + HipV_HipH * 127;cos(x)=sin(x+64)
	TmpXpos = TmpDistance * sin(Hiph_Angle) / 127
	TmpZPos = TmpDistance * sin(Hiph_Angle + 64) / 127;cos(x)=sin(x+64)
	TmpYPos = - (Femur_Length * sin(HipV_Angle + 64) + Tibia_Length * sin(Knee_Angle - 64 + HipV_Angle));cos(x)=sin(x+64)	
	for Index = 0 to Legs - 1
		XPos(Index) = TmpXPos / 127
		YPos(Index) = TmpYPos / 127
		ZPos(Index) = TmpZPos / 127
		Distance(Index) = TmpDistance / 127
	next
	Gosub InitPos
	serout p15,i115200,["#",RRHH,RRHH2,"P1500#",RRHV,RRHV2,"P1500#",RRK,RRK2,"P1500#", |
				MRHH,MRHH2,"P1500#",MRHV,MRHV2,"P1500#",MRK,MRK2,"P1500#", |
				FRHH,FRHH2,"P1500#",FRHV,FRHV2,"P1500#",FRK,FRK2,"P1500#", |
				RLHH,RLHH2,"P1500#",RLHV,RLHV2,"P1500#",RLK,RLK2,"P1500#", |
				MLHH,MLHH2,"P1500#",MLHV,MLHV2,"P1500#",MLK,MLK2,"P1500#", |
				FLHH,FLHH2,"P1500#",FLHV,FLHV2,"P1500#",FLK,FLK2,"P1500T2500",13]
	pause 2700
	Steps = 1
	MovesDelai = 7
	
	return
	
;------------Sub Park
ParkPos
	serout p15,i115200,["#",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,"P1200T2000",13]
	pause 2200
	serout p15,i115200,["#",RRHH,RRHH2,"P900#",MRHH,MRHH2,"P900#",FRHH,FRHH2,"P900#", |
		RLHH,RLHH2,"P900#",MLHH,MLHH2,"P900#",FLHH,FLHH2,"P900T2000",13]
	pause 1600	
	
	return

;-------------Sub InitPos
InitPos
	serout p15,i115200,["#",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,"P1200T2000",13]
	pause 2200
	serout p15,i115200,["#",RRHH,RRHH2,"P1500#",MRHH,MRHH2,"P1500#",FRHH,FRHH2,"P1500#", |
		RLHH,RLHH2,"P1500#",MLHH,MLHH2,"P1500#",FLHH,FLHH2,"P1500T2000",13]
	pause 1600	
	
	return

;-------------Sub All1500
All1500
;	HeightAdjust = 1
	height = 0
	serout p15,i115200,["#",RRHH,RRHH2,"P1500#",RRHV,RRHV2,"P1500#",RRK,RRK2,"P1500#", |
		MRHH,MRHH2,"P1500#",MRHV,MRHV2,"P1500#",MRK,MRK2,"P1500#", |
		FRHH,FRHH2,"P1500#",FRHV,FRHV2,"P1500#",FRK,FRK2,"P1500#", |
		RLHH,RLHH2,"P1500#",RLHV,RLHV2,"P1500#",RLK,RLK2,"P1500#", |
		MLHH,MLHH2,"P1500#",MLHV,MLHV2,"P1500#",MLK,MLK2,"P1500#", |
		FLHH,FLHH2,"P1500#",FLHV,FLHV2,"P1500#",FLK,FLK2,"P1500T576",13]
	pause 600

	return
	
;-------------Sub RC_check
RC_check

no_signal
	pause 200
	low red
	pause 200
	high red
	pulsin ch1,0,r_ch1
	if r_ch1 < 1450 or r_ch1 > 1550 then
		RCcheck = 0
		goto no_signal
	else 
		RCcheck = RCcheck + 1	;filter possible noise
		if RCcheck < 10 then goto no_signal
	endif	

signal_OK

	low green
	sound 9,[100\4200,100\4300,100\4400]
	high green

return


;-------------Sub RCQuery
RCquery	

	pulsin ch1,0,r_ch1 ;transition
	pulsin ch2,0,r_ch2 ;transition
	pulsin ch5,0,r_ch5 ;attack, standby
	pulsin ch6,0,r_ch6 ;as a shift for ch5 
	pulsin ch3,0,r_ch3 ;height, legupshift, speed
	pulsin ch4,0,r_ch4 ;rotation


;	r_ch1 = 1500 ;pro odladeni
;	r_ch2 = 1500 ;pro odladeni
;	r_ch5 = 1500 ;pro odladeni
;	r_ch3 = 1500 ;pro odladeni
;	r_ch4 = 1500 ;pro odladeni


	r_ch3=(r_ch3-1500)/rcratio
	r_ch1=(r_ch1-1500)/rcratio
	r_ch2=(r_ch2-1500)/rcratio
	r_ch4=(r_ch4-1500)/rcratio
	r_ch5=r_ch5/600
	r_ch6=r_ch6/600
;	Serout	S_OUT, i9600, ["kanal 1 = ", DEC r_ch1,"  |   kanal 2 = ", DEC r_ch2," |  kanal 5 = ", DEC r_ch5, 13]

return
	
;-------------Sub Leg SetAngle
SetAngle
	TmpXPos = XPos(Index) * 127
	TmpZPos = ZPos(Index) * 127
	TmpYPos = (YPos(Index) * 127) max 0
	TmpDistance = ((Distance - HipV_HipH) * 127) min 0
	TmpSEWSEW = (TmpYPos * TmpYPos + TmpDistance * TmpDistance) / 16129 ; 127*127=16129
	TmpSEW = SQR(TmpSEWSEW) & 0x7FFF ;bug
	TmpCos = (Femur_Length * Femur_Length + Tibia_Length * Tibia_Length - TmpSEWSEW)
	TmpCos = TmpCos * 127 / (2 * Femur_Length * Tibia_Length)
	gosub ACos
	If TmpAngle > Knee_AngleMax then
		Knee_Angle = Knee_AngleMax
	elseif TmpAngle < Knee_AngleMin
		Knee_Angle = Knee_AngleMin
	else
		Knee_Angle = TmpAngle
	endif


	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
    if TmpYpos > 0 then
    	TmpAngle = TmpAngle - HipV_Angle
    else
    	TmpAngle = TmpAngle + HipV_Angle
    endif
    If TmpAngle > HipV_AngleMax then
		HipV_Angle = HipV_AngleMax
	elseif TmpAngle < HipV_AngleMin
		HipV_Angle = HipV_AngleMin
	else
		HipV_Angle = TmpAngle
	endif 
	return
	
;-------------Sub Distance and HipH_Angle with XZ
DistAndBA
	Distance = SQR((XPos(Index) * XPos(Index) + ZPos(Index) * ZPos(Index)) / 4) & 0x7FFF ;bug
	Distance = Distance * 2
	TmpCos = ZPos(Index) * 127 / Distance
	Gosub ACos
	HipH_Angle = TmpAngle
	
	If HipH_Angle > HipH_AngleMax then
		HipH_Angle = HipH_AngleMax
	elseif HipH_Angle < HipH_AngleMin
		HipH_Angle = HipH_AngleMin
	else
		return
	endif
	
	XPos(Index) = TmpXPos / 127
	ZPos(Index) = TmpZPos / 127
	
		
	return
	
;-------------Sub Arc Cosinus
ACos
	if TmpCos < 0 then
		TmpAngle = 128 - ACS(-TmpCos)
	else
		TmpAngle = ACS(TmpCos)
	endif
	
	return	





;-------------Tripod 'A' Up
A_U	
	for Index = 0 to Legs - 2 Step 2
		YPos(Index) = -Tibia_Length + LegUpShift + Height
		XPos(Index) = XPos(Index) + ((-XPos2(Index) - XPos(Index))/StepFlag)*Steps + HipV_HipH + Femur_Length
		ZPos(Index) = ZPos(Index) + ((-ZPos2(Index) - ZPos(Index))/StepFlag)*Steps
		Rotate(Index) = Rotate(Index) + ((-XCoord - Rotate(Index))/StepFlag)*Steps
	next
	
	return
	
;-------------Tripod 'A' Down
A_D	
	for Index = 0 to Legs - 2 Step 2
		YPos(Index) = -Tibia_Length + Height
		XPos(Index) = XPos(Index) + ((XPos2(Index) - XPos(Index))/NbSteps)*Steps2 + HipV_HipH + Femur_Length
		ZPos(Index) = ZPos(Index) + ((ZPos2(Index) - ZPos(Index))/NbSteps)*Steps2
		Rotate(Index) = Rotate(Index) + ((XCoord - Rotate(Index))/NbSteps)*Steps2
	next
	
	return
	
;-------------Tripod 'B' Up	
B_U
	for Index = 1 to Legs - 1 Step 2	
		YPos(Index) = -Tibia_Length + LegUpShift + Height
		XPos(Index) = XPos(Index) + ((-XPos2(Index) - XPos(Index))/StepFlag)*Steps + HipV_HipH + Femur_Length
		ZPos(Index) = ZPos(Index) + ((-ZPos2(Index) - ZPos(Index))/StepFlag)*Steps
		Rotate(Index) = Rotate(Index) + ((-XCoord - Rotate(Index))/StepFlag)*Steps
	next
	
	return
	
;-------------Tripod 'B' Down
B_D	
	for Index = 1 to Legs - 1 Step 2	
		YPos(Index) = -Tibia_Length + Height
		XPos(Index) = XPos(Index) + ((XPos2(Index) - XPos(Index))/NbSteps)*Steps2 + HipV_HipH + Femur_Length
		ZPos(Index) = ZPos(Index) + ((ZPos2(Index) - ZPos(Index))/NbSteps)*Steps2
		Rotate(Index) = Rotate(Index) + ((XCoord - Rotate(Index))/NbSteps)*Steps2
	next	
		
	return	
	
;--------------------------------------------------------------------