;************************************************
;*** Basic Atom with SSC-32 and PS2 DualShock ***
;******** or MadCatz wireless controller ********
;* H2 program #5 : Tank move + 1 joystick move  *
;***** H2 legs low, middle and high control *****
;******* Sharp GP2D12 IR Sensor Support *********
;************** L5-L6 Arm Support ***************
;********** Bot Board Buzzer support ************
;************************************************
;** Programmer: Laurent Gay, lynxrios@yahoo.fr **
;************************************************
;
; H2 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 mode
;
; H2 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 'high' position with the R1/R2 buttons
; Control The H2 legs 'low' position with the L1/L2 buttons (H2 body height)
; Control The H2 legs 'low' position with the Up/Down buttons (H2 body height, 10 times faster than L2/R2)
;	and push High position to max (when Up button pressed) if needed
; change the legs trajectories Triangle button -> triangle, Circle button -> triangle like,
;	Cross button -> Square like, Square button -> square
;
; Auto avoid obstacles with the Sharp GP2D12 IR Sensor (lock forward) with sound and pad vibration proportional!
; Disable/enable the sensor with the Start button
;
; Swap between H2 and Arm Control with Select button
;
; Initialize Arm position with the Cross button
; Disable/enable Arm servos with the Triangle button
; Invert the X axis with the L3 button (Left joystick push button)
; Close/Open grip with L1/L2 buttons
; Turn wrist (L6 only) Right/Left with R1/R2 buttons
; move X-Y with left joystick
; move Z and Wrist angle with right joystick
;
; 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
AxisSpeed con 15
PadMode con $79 ; Dualshock2 mode, use $73 with a Dualshock1

;Sharp GP2D12 IR sensor
GP2D12_Threshold con 250
GP2D12_SoundThreshold con 112 
 
;H2
H2_RH con 2000 	;assume LH = (3000 - H2_RH) = 1000 !
H2_RM con 1333	;assume LM = (3000 - H2_RM) = 1667 !
H2_RL con 1000	;assume LL = (3000 - H2_RL) = 2000 !

;Arm
BasePin con 8
Base_PulseDef con 1500
Base_PulseMin con 1300		
Base_PulseMax con 1700		

ShoulderPin con 9
Shoulder_AngleDef con 110	;155° (155/360*256)	
Shoulder_AngleMin con 0		;0°
Shoulder_AngleMax con 110	;155°
Shoulder_PulseMin con 700	
Shoulder_PulseMax con 2050	

ElbowPin con 24
Elbow_AngleDef con 110		;155° (155/360*256)	
Elbow_AngleMin con 0		;0°
Elbow_AngleMax con 110		;155°
Elbow_PulseMin con 640		
Elbow_PulseMax con 2150	

WristPin con 25
Wrist_AngleShift con -64	;Wrist-Forearm Shift
Wrist_AngleDef con 46		;65° (65/360*256)	
Wrist_AngleMin con 0		;0°
Wrist_AngleMax con 128		;180°
Wrist_PulseMin con 600		
Wrist_PulseMax con 2370		

GripPin con 26
Grip_PulseDef con 1500		
Grip_PulseMin con 1000		
Grip_PulseMax con 1800
		
WristRotPin con 27
WristRot_PulseDef con 1500	
WristRot_PulseMin con 750	
WristRot_PulseMax con 2250

;L6
;Arm_Length con 121			;4.75" = 121mm (4.75 * 25.4)
;Forearm_Length con 121		;4.75" = 121mm (4.75 * 25.4)
;Hand_Length con 146		;5.75" = 146mm (5.75 * 25.4)

;L5
Arm_Length con 95			;3.75" = 95mm (3.75 * 25.4)
Forearm_Length con 95		;3.75" = 95mm (3.75 * 25.4)
Hand_Length con 127			;5.00" = 146mm (5.00 * 25.4)

;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,08,06,04,02		;Cheating on values (vertical lag issue)
					;20,19,19,18,17,16,15,15,14,13,11,10,09,07,05,00	;Real values
					
;--------------------------------------------------------------------				
;-------------Variables
index var Byte
DualShock var Byte(7)
LastButton var Byte(2)
XR var SWord
XL var SWord
CmdMode var Bit			;1 = H2, 0 = Arm
MoveModeH2 var Bit		;1 = One Joystick, 0 = Tank
ParkArm var Bit
NeedServosOn var Bit
GP2D12Enable var Bit
ArmDirection var Bit	;1 = Left to right, 0 = right to left
ArmOnOff var Bit
LockForward var Bit
LargeMotor var Byte
Speed var Byte
MaxSpeed var Byte
MiddleMode var Nib		;0 = triangle, 1 = nearly triangle, 2 = nearly square, 3 = square
H2_High var Word
H2_Middle var Word
H2_Low var word
GP2D12 var Word
Time var Word

XCoord var Sbyte
YCoord var Sbyte
ZCoord var Sbyte
WCoord var Sbyte

TmpYPos var Long
TmpDistance var Long
TmpSEW var Word
TmpSEWSEW var Long
TmpCos var Long
TmpAngle var SWord

Distance var Sword
YPos var Sword

Wrist_TableAngle var SWord

Shoulder_Angle var Sword
Elbow_Angle var Sword
Wrist_Angle var Sword

Base_Pulse var word
Shoulder_Pulse var word
Elbow_Pulse var word
Wrist_Pulse var word
Grip_Pulse var word
WristRot_Pulse var word

;--------------------------------------------------------------------	
;***************
;*** 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,$4D\8,$00\8,$00\8,$01\8,$FF\8,$FF\8,$FF\8,$FF\8] ;VIBRATION_ENABLE
	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
	
CmdMode = 1			;1 = H2, 0 = Arm
MoveModeH2 = 0		;1 = One Joystick, 0 = Tank
GP2D12Enable = 1
ArmDirection = 1	;1 = Left to right, 0 = Right to left
ArmOnOff = 1
LockForward = 0
LargeMotor = 0
MiddleMode = 1
MaxSpeed = 100

LastButton(0) = 255
LastButton(1) = 255

;Arm engine
ParkArm = 0
NeedServosOn = 1
gosub ArmInit

;SSC-32 -> H2 engine
pause 500
gosub H2Init

;--------------------------------------------------------------------
;-------------Main loop
main
;DS2
	low SEL
	shiftout CMD,CLK,FASTLSBPRE,[$1\8,$42\8]
	for index = 0 to 2
		shiftin DAT,CLK,FASTLSBPOST,[DualShock(index)\8]
	next
	high SEL
	pause 1
	low SEL
	shiftout CMD,CLK,FASTLSBPRE,[$1\8,$42\8,$0\8,$0\8,LargeMotor\8]
	for index = 3 to 6
		shiftin DAT,CLK,FASTLSBPOST,[DualShock(index)\8]
	next
	high SEL
	pause 1
	
	XCoord = DualShock(5) - 128
	if XCoord > DeadZone then
		XCoord = XCoord - DeadZone
	elseif XCoord < -DeadZone
		XCoord = XCoord + DeadZone
	else
		XCoord = 0 
	endif
	
	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).bit0 = 0) and LastButton(0).bit0 then	;Select Button test
		CmdMode = CmdMode ^ 1
		if CmdMode then
			Sound 9,[50\440,25\1,50\440]
		else
			serout SSC32,i38400,["XSTOP",13]
			serout SSC32,i38400,["#0P",DEC H2_RL," #2P",DEC H2_RL," #4P",DEC H2_RL |
				," #16P",DEC 3000 - H2_RL," #18P",DEC 3000 - H2_RL," #20P",DEC 3000 - H2_RL, " S1000",13]
			ArmOnOff = 1
			LargeMotor = 0
			Sound 9,[50\1760,25\1,50\1760]		
		endif
	endif	
	
	if CmdMode then
;H2		
		if (DualShock(2).bit4 = 0) and LastButton(1).bit4 then	;Triangle Button test
			MiddleMode = 4
		elseif (DualShock(2).bit5 = 0) and LastButton(1).bit5	;Circle Button test
			MiddleMode = 5
		elseif (DualShock(2).bit6 = 0) and LastButton(1).bit6	;Cross Button test
			MiddleMode = 6
		elseif (DualShock(2).bit7 = 0) and LastButton(1).bit7	;Square Button test
			MiddleMode = 7
		else
			goto NoSound1
		endif
		Sound 9,[100\1318]
NoSound1
		if (DualShock(2).bit2 = 0) and (H2_Low >= (H2_RL + 20)) then 	;L1 Button test
			H2_Low = H2_Low - 20
			MiddleMode = MiddleMode | 8
		elseif (DualShock(2).bit0 = 0) and (H2_Low <= (H2_High - 20))  	;L2 Button test
			H2_Low = H2_Low + 20
			MiddleMode = MiddleMode | 8
		else
			goto NoSound2
		endif
		Sound 9,[30\((H2_RH - H2_Low) / 2 + 100)]
NoSound2
		if DualShock(1).bit4 = 0 then 	;Up Button test
			H2_Low = H2_Low - 200
			if H2_Low < H2_RL then
				H2_Low = H2_RL
				goto NoSound3
			endif
			MiddleMode = MiddleMode | 8
		elseif DualShock(1).bit6 = 0	;Down Button test
			H2_Low = H2_Low + 200
			if H2_Low > H2_RH then
				H2_Low = H2_RH
				goto NoSound3
			endif
			H2_High = H2_High + 200
			if H2_High > H2_RH then
				H2_High = H2_RH
			endif
			MiddleMode = MiddleMode | 8
		else
			goto NoSound3
		endif
		Sound 9,[50\((H2_RH - H2_Low) / 2 + 100)]
NoSound3
		if (DualShock(2).bit1 = 0) and (H2_High >= (H2_Low + 20)) then	;R2 Button test
			H2_High = H2_High - 20
			MiddleMode = MiddleMode | 8
		elseif (DualShock(2).bit3 = 0) and (H2_High <= (H2_RH - 20))  	;R1 Button test
			H2_High = H2_High + 20
			MiddleMode = MiddleMode | 8
		else
			goto NoSound4
		endif
		Sound 9,[30\((H2_High - H2_RL) / 2 + 100)]
NoSound4
		if (DualShock(1).bit2 = 0) and LastButton(0).bit2 then	;R3 Button test
			MoveModeH2 = MoveModeH2 ^ 1
			Sound 9,[100\1318]
		endif
		if (DualShock(1).bit3 = 0) and LastButton(0).bit3 then	;Start Button test
			GP2D12Enable = GP2D12Enable ^ 1
			if GP2D12Enable then
				Sound 9,[100\880,100\1480]
			else
				LockForward = 0
				LargeMotor = 0
				sound 9,[100\1480,100\880]	
			endif
		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 NoSound5
		endif
		Sound 9,[100\(MaxSpeed * 10 + 100)]
NoSound5	
		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
		if LockForward then
		 	if (XR > 0) then
				XR = 0
			endif
			if (XL > 0) then
				XL = 0
			endif
		endif
		if MiddleMode > 3 then
			MiddleMode = MiddleMode & 3
			if MiddleMode = 0 then
				H2_Middle = H2_Low
			elseif MiddleMode = 1
				H2_Middle = (H2_High - H2_Low) / 3 + H2_Low
			elseif MiddleMode = 2
				H2_Middle = (H2_High - H2_Low) * 2 / 3 + H2_Low
			else
				H2_Middle = H2_High
			endif		
			serout SSC32,i38400,["LH",DEC 3000 - H2_High," LM",DEC 3000 - H2_Middle," LL",DEC 3000 - H2_Low,13]
			serout SSC32,i38400,["RH",DEC H2_High," RM",DEC H2_Middle," RL",DEC H2_Low,13]
			if Speed = 0 then
				Speed = 200 	;Ensure legs updating
			endif
		endif
		serout SSC32,i38400,["XS",DEC Speed," XR",SDEC XR," XL",SDEC XL,13]
		
		if GP2D12Enable then
			adin AX0,2,AD_RON,GP2D12
			LockForward = 0
			LargeMotor = 0
			if GP2D12 > GP2D12_Threshold then
				LockForward = 1
				LargeMotor = 255
				Sound 9,[20\200]
			elseif GP2D12 > GP2D12_SoundThreshold
				LargeMotor = GP2D12
				Sound 9,[10\(GP2D12 * 10)]
			endif
		endif
		
		nap 2 ;internal sleep mode, approx 38ms, use value 3 or 4 with some slow wireless controller
	else
;Arm
		if (DualShock(2).bit6 = 0) and LastButton(1).bit6 then	;Cross Button test
			ParkArm = 1
		elseif (DualShock(2).bit4 = 0) and LastButton(1).bit4	;Triangle Button test
			ArmOnOff = ArmOnOff ^ 1
			if ArmOnOff then
				gosub ArmUpdatePosition
				Sound 9,[100\523,100\659,100\783]	
				ParkArm = 0
				NeedServosOn = 1
			else
				serout SSC32,i38400,["#",DEC BasePin,"P0 #",DEC ShoulderPin,"P0 #", |
					DEC ElbowPin,"P0 #",DEC WristPin,"P0 #",DEC GripPin,"P0 #",DEC WristRotPin,"P0",13]
				Sound 9,[100\783,100\659,100\523]
			endif
		endif
		if ArmOnOff = 0 then ShortCut2
		
		if (DualShock(1).bit1 = 0) and LastButton(0).bit1 then	;L3 Button test
			ArmDirection = ArmDirection ^ 1
			Sound 9,[100\1318]
		endif
		
		if XCoord or YCoord or WCoord or ParkArm then
			if ParkArm then
				gosub ArmInit
			else
				if WCoord then
					Wrist_Angle = Wrist_Angle - (WCoord / (AxisSpeed * 2))
					if Wrist_Angle > Wrist_AngleMax then
						Wrist_Angle = Wrist_AngleMax
					elseif Wrist_Angle < Wrist_AngleMin
						Wrist_Angle = Wrist_AngleMin
					endif
					gosub GetPos
					if TmpDistance > 0 then
						Distance = TmpDistance / 127
						YPos = TmpYPos / 127
						Wrist_TableAngle = Elbow_Angle - Shoulder_Angle - Wrist_AngleShift - Wrist_Angle
					else
						Wrist_Angle = Wrist_Angle + (WCoord / (AxisSpeed * 2))
					endif
				endif
				if ArmDirection then
					Distance = Distance + (XCoord / AxisSpeed)
				else
					Distance = Distance - (XCoord / AxisSpeed)
				endif
				if Distance < 0 then 
					Distance = 0
				endif
				YPos = YPos - (YCoord / AxisSpeed)	
				gosub SetAngle
			endif
			
			Shoulder_Pulse = (Shoulder_PulseMax - Shoulder_PulseMin) * (Shoulder_Angle - Shoulder_AngleMin) |
				/ (Shoulder_AngleMax - Shoulder_AngleMin) + Shoulder_PulseMin
			if Shoulder_Pulse > Shoulder_PulseMax then
				Shoulder_Pulse = Shoulder_PulseMax
			elseif Shoulder_Pulse < Shoulder_PulseMin
				Shoulder_Pulse = Shoulder_PulseMin		
			endif
			
			Elbow_Pulse = (Elbow_PulseMax - Elbow_PulseMin) * (Elbow_Angle - Elbow_AngleMin) |
				/ (Elbow_AngleMax - Elbow_AngleMin) + Elbow_PulseMin
			if Elbow_Pulse > Elbow_PulseMax then
				Elbow_Pulse = Elbow_PulseMax	
			elseif Elbow_Pulse < Elbow_PulseMin
				Elbow_Pulse = Elbow_PulseMin		
			endif
			
			Wrist_Pulse = (Wrist_PulseMax - Wrist_PulseMin) * (Wrist_Angle - Wrist_AngleMin) |
				/ (Wrist_AngleMax - Wrist_AngleMin) + Wrist_PulseMin
			if Wrist_Pulse > Wrist_PulseMax then
				Wrist_Pulse = Wrist_PulseMax
			elseif Wrist_Pulse < Wrist_PulseMin
				Wrist_Pulse = Wrist_PulseMin	
			endif
	
			gosub ArmUpdatePosition
		endif
		
		if ZCoord or NeedServosOn then
			Base_Pulse = Base_Pulse + ZCoord / 8
			if Base_Pulse > Base_PulseMax then
				Base_Pulse = Base_PulseMax
			elseif Base_Pulse < Base_PulseMin
				Base_Pulse = Base_PulseMin	
			endif
			serout SSC32,i38400,["#",DEC BasePin,"P",DEC Base_Pulse," T100",13]
		endif
		
		if DualShock(2).bit2 = 0 then 	;L1 button test
			Grip_Pulse = Grip_Pulse - 25
		elseif DualShock(2).bit0 = 0	;L2 button test
			Grip_Pulse = Grip_Pulse + 25
		elseif NeedServosOn = 0
			goto Shortcut1
		endif
		if Grip_Pulse > Grip_PulseMax then
			Grip_Pulse = Grip_PulseMax
		elseif Grip_Pulse < Grip_PulseMin
			Grip_Pulse = Grip_PulseMin	
		endif
		serout SSC32,i38400,["#",DEC GripPin,"P",DEC Grip_Pulse," T100",13]

Shortcut1		
		if DualShock(2).bit3 = 0 then	;R1 button test
			WristRot_Pulse = WristRot_Pulse - 25
		elseif DualShock(2).bit1 = 0	;R2 button test
			WristRot_Pulse = WristRot_Pulse + 25
		elseif NeedServosOn = 0
			goto Shortcut2
		endif
		if WristRot_Pulse > WristRot_PulseMax then
			WristRot_Pulse = WristRot_PulseMax
		elseif WristRot_Pulse < WristRot_PulseMin
			WristRot_Pulse = WristRot_PulseMin	
		endif
		serout SSC32,i38400,["#",DEC WristRotPin,"P",DEC WristRot_Pulse," T100",13]
		NeedServosOn = 0
Shortcut2
		nap 1 ;internal sleep mode, approx 19ms, use value 2 with some slow wireless controller
	endif
	
	LastButton(0) = DualShock(1)
	LastButton(1) = DualShock(2)
	
	goto main
	
;--------------------------------------------------------------------
;-------------Sub Update Position
ArmUpdatePosition
	serout SSC32,i38400,["#",DEC ShoulderPin,"P",DEC Shoulder_Pulse, |
		" #",DEC ElbowPin,"P",DEC Elbow_Pulse," #",DEC WristPin,"P",DEC Wrist_Pulse," T",DEC Time,13]	
	Time = 150
	
	return
	
;-------------Sub Arm Init
ArmInit
	
	Shoulder_Angle = Shoulder_AngleDef
	Elbow_Angle = Elbow_AngleDef
	Wrist_Angle = Wrist_AngleDef
	Base_Pulse = Base_PulseDef
	Grip_Pulse = Grip_PulseDef
	WristRot_Pulse = WristRot_PulseDef
	gosub GetPos
	Distance = TmpDistance / 127
	YPos = TmpYPos / 127
	Wrist_TableAngle = Elbow_Angle - Shoulder_Angle - Wrist_AngleShift - Wrist_Angle
	if ParkArm then
		serout SSC32,i38400,["#",DEC BasePin,"P",DEC Base_Pulse," S1000",13]
		serout SSC32,i38400,["#",DEC GripPin,"P",DEC Grip_Pulse," S1000",13]
		serout SSC32,i38400,["#",DEC WristRotPin,"P",DEC WristRot_Pulse," S1000",13]	
		ParkArm = 0	
		Time = 750
	endif
	ArmOnOff = 1
	ParkArm = 0
	
	return
	
;-------------Sub H2 Init
H2Init
	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
	H2_High = H2_RH
	H2_Middle = H2_RM
	H2_Low = H2_RL
	MiddleMode = 1
	
	return
	
;-------------Sub Arm SetAngle then GetPos
SetAngle
		
	TmpDistance = Distance * 127 - Hand_Length * COS(Wrist_TableAngle)
	
	if YPos > 0 then
		TmpYPos = YPos * 127 + Hand_Length * SIN(Wrist_TableAngle)
	else
		TmpYPos = Hand_Length * SIN(Wrist_TableAngle) - (-YPos) * 127 ; **** due to bug in Basic ****
	endif
	
	if TmpDistance < 0 then
		TmpDistance = 0
		if ArmDirection then
			Distance = Distance - (XCoord / AxisSpeed)
		else
			Distance = Distance + (XCoord / AxisSpeed)
		endif
	endif
	if TmpYPos > 0 then
		TmpSEWSEW = (TmpYPos * TmpYPos + TmpDistance * TmpDistance) / 16129
	else
		TmpSEWSEW = ((-TmpYPos) * (-TmpYPos) + TmpDistance * TmpDistance) / 16129 ; **** due to bug in Basic ****
	endif
	TmpSEW = SQR(TmpSEWSEW)
	if TmpSEW > (Arm_Length + Forearm_Length) then
		TmpSEW = Arm_Length + Forearm_Length
		if ArmDirection then
			Distance = Distance - (XCoord / AxisSpeed)
		else
			Distance = Distance + (XCoord / AxisSpeed)
		endif
		YPos = YPos + (YCoord / AxisSpeed)
		TmpSEWSEW = TmpSEW * TmpSEW
	endif
	TmpCos = -(Arm_Length * Arm_Length + Forearm_Length * Forearm_Length - TmpSEWSEW)
	if TmpCos > 0 then
		TmpCos = TmpCos * 127 / (2 * Arm_Length * Forearm_Length)
	else
		TmpCos = -((-TmpCos) * 127 / (2 * Arm_Length * Forearm_Length)) ; **** due to bug in Basic ****
	endif 
	gosub ACos
	If TmpAngle > Elbow_AngleMax then
		Elbow_Angle = Elbow_AngleMax
	elseif TmpAngle < Elbow_AngleMin
		Elbow_Angle = Elbow_AngleMin
	else
		Elbow_Angle = TmpAngle
	endif
	
	TmpCos = TmpDistance / TmpSew
	gosub ACos
	Shoulder_Angle = TmpAngle
	TmpCos = (Forearm_Length * Forearm_Length - Arm_Length * Arm_Length + TmpSEWSEW) 
	if TmpCos > 0 then
		TmpCos = TmpCos * 127 / (2 * Forearm_Length * TmpSEW) 
	else
		TmpCos = -((-TmpCos) * 127 / (2 * Forearm_Length * TmpSEW)) ; **** due to bug in Basic ****
	endif
    gosub ACos
    if TmpYpos > 0 then
    	TmpAngle = TmpAngle + Shoulder_Angle
    else
    	TmpAngle = TmpAngle - Shoulder_Angle
    endif
    If TmpAngle > Shoulder_AngleMax then
		Shoulder_Angle = Shoulder_AngleMax
	elseif TmpAngle < Shoulder_AngleMin
		Shoulder_Angle = Shoulder_AngleMin
	else
		Shoulder_Angle = TmpAngle
	endif 
	
	Wrist_Angle = Elbow_Angle - Shoulder_Angle - Wrist_AngleShift - Wrist_TableAngle
	If Wrist_Angle > Wrist_AngleMax then
		Wrist_Angle = Wrist_AngleMax
	elseif Wrist_Angle < Wrist_AngleMin
		Wrist_Angle = Wrist_AngleMin
	endif
	
;-------------Sub Arm GetPos
GetPos
	TmpDistance = Arm_Length * COS(Shoulder_Angle) + Forearm_Length * COS(Shoulder_Angle - Elbow_Angle) |
		+ Hand_Length * COS(Shoulder_Angle - Elbow_Angle + Wrist_AngleShift + Wrist_Angle) 
	
	TmpYPos = Arm_Length * SIN(Shoulder_Angle) + Forearm_Length * SIN(Shoulder_Angle - Elbow_Angle) |
		+ Hand_Length * SIN(Shoulder_Angle - Elbow_Angle + Wrist_AngleShift + Wrist_Angle) 
		
	return
	
;-------------Sub Arc Cosinus
ACos
	if TmpCos > 0 then
		TmpAngle = ArcCos(TmpCos)
	else
		TmpAngle = 128 - ArcCos(-TmpCos)
	endif
	
	return
	
;--------------------------------------------------------------------