;************************************************
;*** Basic Atom with SSC-32 and PS2 DualShock ***
;***        H2 program #1 : Tank move         ***
;********** Bot Board Buzzer support ************
;************************************************
;** Programmer: Laurent Gay, lynxrios@yahoo.fr **
;************************************************
;
;
; 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)
;
; Right and Left buttons adjust the speed limit
;
; 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)
YCoord var Sbyte
WCoord var Sbyte
Speed var Byte
MaxSpeed var Byte

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

MaxSpeed = 100

;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
	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
	
	WCoord = DualShock(4) - 128
	if WCoord > DeadZone then
		WCoord = WCoord - DeadZone
	elseif WCoord < -DeadZone
		WCoord = WCoord + DeadZone
	else
		WCoord = 0 
	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 abs(WCoord) > abs(YCoord) then
		Speed = abs(WCoord) * MaxSpeed / 100
	else
		Speed = abs(YCoord) * MaxSpeed / 100
	endif	
	serout SSC32,i38400,["XS",DEC Speed," XR",SDEC -WCoord," XL",SDEC -YCoord,13]
	
	nap 2 ;internal sleep mode, approx 38ms, use value 3,4 or 5 with some slow wireless controller
	
	goto main
	
;--------------------------------------------------------------------