;************************************************
;*** Basic Atom with SSC-32 and PS2 DualShock ***
;* H2 program #3 : Tank move + 1 joystick move  *
;*********** H2 legs height control *************
;********** Bot Board Buzzer support ************
;************************************************
;** Programmer: Laurent Gay, lynxrios@yahoo.fr **
;************************************************
;
; Control Mode 1 :
; Control the right legs with the right joystick (SSC-32 XR command)
; Control the left legs with the left joystick (SSC-32 XL command)
; the global speed is mixed (SSC-32 XS command)
;
; Push the R3 button (Right joystick push button) to swap between control modes
;
; Control Mode 2 :
; Control All legs with the right joystick (SSC-32 XR-XL command mixed)
; the global speed is mixed too (SSC-32 XS command)
; 
; Right and Left buttons adjust the speed limit (Mode 1 & 2)
;
; Control The H2 legs height with the Up/Down buttons 
;
; you may have to push the Analog Button on a MadCatz Wireless controller (if in sleep mode)
;
;************************************************
;
;
;--------------------------------------------------------------------
;-------------Constants
;PS2 Controller / BotBoard I
DAT con P4
CMD con P5
SEL con P6
CLK con P7
SSC32 con p15
;PS2 Controller / BotBoard II
;DAT con P12
;CMD con P13
;SEL con P14
;CLK con P15
;SSC32 con p8

DeadZone con 28	; must be >= 28
PadMode con $79 ; Dualshock2 mode, use $73 with a Dualshock1
;H2
H2_RH con 2000 	;assume LH = (3000 - H2_RH) = 1000 !
H2_RM con 1100	;assume LM = (3000 - H2_RM) = 1900 !
H2_RL con 1000	;assume LL = (3000 - H2_RL) = 2000 !

;--------------------------------------------------------------------				
;-------------Variables
index var Byte
DualShock var Byte(7)
LastButton var Byte
XR var SWord
XL var SWord
MoveModeH2 var Bit
H2_Height var Sword
Speed var Byte
MaxSpeed var Byte

YCoord var Sbyte
ZCoord var Sbyte
WCoord var Sbyte

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

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

clear
high CLK

again
	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 100
	
	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 1

	low SEL
	shiftout CMD,CLK,FASTLSBPRE,[$1\8]
	shiftin DAT,CLK,FASTLSBPOST,[DualShock(0)\8]
	high SEL
	pause 1
	
	;serout S_OUT,i57600,["PadMode : ", HEX2 DualShock(0), 13]
	Sound 9,[100\4435]
	if DualShock(0) <> PadMode then again
	
MoveModeH2 = 0
MaxSpeed = 100
LastButton(0) = 255
H2_Height = 0

;SSC-32 -> H2 engine
pause 500
serout SSC32,i38400,[13]									;clear the SSC-32 buffers
;Servo Offset Command
;Replace the section between the quotes with the values as described in the tutorial.
;serout SSC32,i38400,["#0PO0 #1PO0 #2PO0 #3PO0 #4PO0 #5PO0 #16PO0 #17PO0 #18PO0 #19PO0 #20PO0 #21PO0",13]
	
serout SSC32,i38400,["XS0 XSTOP",13]						;Stop the sequencer if running
serout SSC32,i38400,["LF1700 RF1300 LR1300 RR1700",13] 	;Horizontal
serout SSC32,i38400,["LH", DEC 3000 - H2_RH, " LM", DEC 3000 - H2_RM, " LL", DEC 3000 - H2_RL,13] ;Vertical Left
serout SSC32,i38400,["RH", DEC H2_RH," RM", DEC H2_RM, " RL", DEC H2_RL,13]						;Vertical Right
serout SSC32,i38400,["VS2500 HT500",13]					;Vertical Speed and Horizontal Time
serout SSC32,i38400,["XS0",13]							;Set Global Speed to 0

;--------------------------------------------------------------------
;-------------Main loop
main
;DS2
	low SEL
	shiftout CMD,CLK,FASTLSBPRE,[$1\8,$42\8]
	for index = 0 to 6
		shiftin DAT,CLK,FASTLSBPOST,[DualShock(index)\8]
	next
	high SEL
	
	YCoord = DualShock(6) - 128
	if YCoord > DeadZone then
		YCoord = YCoord - DeadZone
	elseif YCoord < -DeadZone
		YCoord = YCoord + DeadZone
	else
		YCoord = 0 
	endif
	
	ZCoord = DualShock(3) - 128
	if ZCoord > DeadZone then
		ZCoord = ZCoord - DeadZone
	elseif ZCoord < -DeadZone
		ZCoord = ZCoord + DeadZone
	else
		ZCoord = 0 
	endif
	
	WCoord = DualShock(4) - 128
	if WCoord > DeadZone then
		WCoord = WCoord - DeadZone
	elseif WCoord < -DeadZone
		WCoord = WCoord + DeadZone
	else
		WCoord = 0 
	endif
	
	if (DualShock(1).bit2 = 0) and LastButton.bit2 then ;R3 Button test
		MoveModeH2 = MoveModeH2 ^ 1
		Sound 9,[100\1318]
	endif
	
	if DualShock(1).bit4 = 0 then ;Up Button test
		H2_Height = H2_Height - 20
		if H2_Height < 0 then
			H2_Height = 0
		endif
	elseif DualShock(1).bit6 = 0  ;Down Button test
		H2_Height = H2_Height + 20
		if (H2_Height + H2_RM) > (H2_RH - 100) then
			H2_Height = H2_RH - H2_RM - 100
		endif
	else
		goto NoUpDownMove
	endif
	serout SSC32,i38400,["LH", DEC 3000 - H2_RH, " LM", DEC 3000 - H2_RM - H2_Height, |
		" LL", DEC 3000 - H2_RL - H2_Height,13]
	serout SSC32,i38400,["RH", DEC H2_RH," RM", DEC H2_RM + H2_Height, " RL", DEC H2_RL + H2_Height,13]
	Sound 9,[30\(H2_Height * 2 + 100)]	
NoUpDownMove
	if (DualShock(1).bit5 = 0) and (MaxSpeed <= 190) then	;Right Button test
		MaxSpeed = MaxSpeed + 10
	elseif (DualShock(1).bit7 = 0) and (MaxSpeed >= 20)		;Left Button test
		MaxSpeed = MaxSpeed - 10
	else
		goto NoSound1
	endif
	Sound 9,[100\(MaxSpeed * 10 + 100)]
NoSound1		
	if MoveModeH2 then 	;One Joystick Mode 
		XR = -ZCoord - WCoord
		XL = ZCoord - WCoord
		
		if XR > 100 then
			XR = 100
		elseif XR < -100
			XR = -100
		endif
		
		if XL > 100 then
			XL = 100
		elseif XL < -100
			XL = -100
		endif
		
		if abs(WCoord) > abs(ZCoord) then
			Speed = abs(WCoord) * MaxSpeed / 100
		else
			Speed = abs(ZCoord) * MaxSpeed / 100
		endif
	else				;Tank Mode
		if abs(WCoord) > abs(YCoord) then
			Speed = abs(WCoord) * MaxSpeed / 100
		else
			Speed = abs(YCoord) * MaxSpeed / 100
		endif	
		XR = -WCoord
		XL = -YCoord
	endif
	
	serout SSC32,i38400,["XS",DEC Speed," XR",SDEC XR," XL",SDEC XL,13]
	
	LastButton = DualShock(1)
	
	nap 2 ;internal sleep mode, approx 38ms, use value 3,4 or 5 with some slow wireless controller
	
	goto main
	
;--------------------------------------------------------------------