;--------------------------------------------------
; Control a Lynxmotion Arm using a PS2 controller
;  
; Needed hardware:
;  Bot Board I/II with Basic Atom Pro 28
;  SSC-32
;  Playstation 2 controller
;
;
; Programmer: Laurent Gay, lynxrios@yahoo.fr
;  Modified by James Frye, support@lynxmotion.com
;
;--------------------------------------------------
;
; Tips:
;
; Scroll down to "Arm selection" to set the arm dimensions
; 
; 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
 
;Arm
BasePin con 0
Base_PulseDef con 1500
Base_PulseMin con 850		
Base_PulseMax con 2150		

ShoulderPin con 1
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 2
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 3
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 4
Grip_PulseDef con 1500		
Grip_PulseMin con 1000		
Grip_PulseMax con 1800
		
WristRotPin con 5
WristRot_PulseDef con 1500	
WristRot_PulseMin con 750	
WristRot_PulseMax con 2250

;Arm Selection:
;--------------------------------------------------

;AL5A
;Arm_Length con 94			;3.75" = 94mm (3.75 * 25.4)
;Forearm_Length con 108		;4.25" = 108mm (4.25 * 25.4)
;Hand_Length con 87			;3.48" = 87mm (3.48 * 25.4) (With no rotate)
;Hand_Length con 113		;4.44" = 113mm (4.44 * 25.4) (With LW rotate)
;Hand_Length con 100		;3.94" = 100mm (3.94 * 25.4) (With HD rotate)

;AL5B
Arm_Length con 119			;4.66" = 119mm (4.66 * 25.4)
Forearm_Length con 127		;5.00" = 127mm (5.00 * 25.4)
Hand_Length con 87			;3.48" = 87mm (3.48 * 25.4) (With no rotate)
;Hand_Length con 113		;4.44" = 113mm (4.44 * 25.4) (With LW rotate)
;Hand_Length con 100		;3.94" = 100mm (3.94 * 25.4) (With HD rotate)

;AL5C
;Arm_Length con 156			;6.14" = 156mm (6.14 * 25.4)
;Forearm_Length con 154		;6.06" = 154mm (6.06 * 25.4)
;Hand_Length con 87			;3.48" = 87mm (3.48 * 25.4) (With no rotate)
;Hand_Length con 113		;4.44" = 113mm (4.44 * 25.4) (With LW rotate)
;Hand_Length con 100		;3.94" = 100mm (3.94 * 25.4) (With HD rotate)

;AL5D
;Arm_Length con 147			;5.79" = 147mm (5.79 * 25.4)
;Forearm_Length con 187		;7.36" = 187mm (7.36 * 25.4)
;Hand_Length con 87			;3.48" = 87mm (3.48 * 25.4) (With no rotate)
;Hand_Length con 113		;4.44" = 113mm (4.44 * 25.4) (With LW rotate)
;Hand_Length con 100		;3.94" = 100mm (3.94 * 25.4) (With HD rotate)

;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)
ParkArm var Bit
NeedServosOn var Bit
ArmDirection var Bit	;1 = Left to right, 0 = right to left
ArmOnOff var Bit
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
	
LastButton(0) = 255
LastButton(1) = 255

;Arm engine
ArmDirection = 1	;1 = Left to right, 0 = Right to left
ArmOnOff = 1
ParkArm = 0
NeedServosOn = 1
gosub ArmInit

pause 500

;--------------------------------------------------------------------
;-------------Main loop
main
;DS2
	low SEL
	shiftout CMD,CLK,FASTLSBPRE,[$1\8,$42\8]
	shiftin DAT,CLK,FASTLSBPOST,[DualShock(0)\8, DualShock(1)\8, DualShock(2)\8, DualShock(3)\8, |
		DualShock(4)\8, DualShock(5)\8, DualShock(6)\8]
	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
	
;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 or NeedServosOn 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
	pause 36		
	
	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 Arm SetAngle then GetPos
SetAngle
		
	TmpDistance = Distance * 127 - Hand_Length * SIN(Wrist_TableAngle + 64)
	
	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 * SIN(Shoulder_Angle + 64) + Forearm_Length * SIN((Shoulder_Angle - Elbow_Angle) + 64) |
		+ Hand_Length * SIN((Shoulder_Angle - Elbow_Angle + Wrist_AngleShift + Wrist_Angle) + 64) 
	
	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
;--------------------------------------------------------------------