;************************************************
;*** Basic Atom with SSC-32 and PS2 DualShock ***
;* H2 program #2 : Tank move + 1 joystick move  *
;********** 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)
;
; 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

;--------------------------------------------------------------------				
;-------------Variables
index var Byte
DualShock var Byte(7)
LastButton var Byte
XR var SWord
XL var SWord
MoveModeH2 var Bit
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

;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,["LH1000 LM1900 LL2000",13] 			;Vertical Left
serout SSC32,i38400,["RH2000 RM1100 RL1000",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).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
	
;--------------------------------------------------------------------