;**************************************************
;**** Basic Atom with SSC-32 and PS2 DualShock ****
;********* or MadCatz wireless controller *********
;** 4WD program #1 : Tank move + 1 joystick move **
;******** Sharp GP2D12 IR Sensor Support **********
;*************** L5-L6 Arm Support ****************
;*********** Bot Board Buzzer support *************
;**************************************************
;*** Programmer: Laurent Gay, lynxrios@yahoo.fr ***
;**************************************************
;
; 4WD Control Mode 1 :
; Control the right wheels with the right joystick
; Control the left wheels with the left joystick
; the global speed is mixed 
;
; Push the R3 button (Right joystick push button) to swap between control mode
;
; 4WD Control Mode 2 :
; Control All wheels with the right joystick
; the global speed is mixed too
;
; Right and Left buttons adjust the speed limit (Mode 1 & 2)
;
; Adjust the Scorpion left neutral value with the L1/L2 buttons
; Adjust the Scorpion right neutral value with the R1/R2 buttons
;
; 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 4WD 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
;4WD
Range4WD con 300			;Right and Left Range: min = 1500 - Range4WD, max = 1500 + Range4WD
Scorpionconnect con 1 		;1 : Left channel => Bot Board pin 0, Right channel => Bot Board pin 1
							;0 : Left channel => SSC-32 pin 12, Right channel => SSC-32 pin 13
						
;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

;Sharp GP2D12 IR sensor
GP2D12_Threshold con 250
GP2D12_SoundThreshold con 112 
 
;Arm
BasePin con 8
Base_PulseDef con 1500
Base_PulseMin con 700		
Base_PulseMax con 2300		

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 10
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 11
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 12
Grip_PulseDef con 1500		
Grip_PulseMin con 1000		
Grip_PulseMax con 1800
		
WristRotPin con 13
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
LeftNeutral4WD var Word		
RightNeutral4WD var Word	

index var Byte
DualShock var Byte(7)
LastButton var Byte(2)
Right4WD var SWord
Left4WD var SWord
CmdMode var Bit			;1 = 4WD, 0 = Arm
MoveMode4WD 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
MaxSpeed 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
MoveMode4WD = 0		;1 = One Joystick, 0 = Tank
GP2D12Enable = 1
ArmDirection = 1	;1 = Left to right, 0 = Right to left
ArmOnOff = 1
LockForward = 0
LargeMotor = 0
MaxSpeed = Range4WD / 2
LeftNeutral4WD = 1500	;put here your scorpion left channel neutral value
RightNeutral4WD = 1500	;put here your scorpion Right channel neutral value

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

;Arm engine
ParkArm = 0
NeedServosOn = 1
gosub ArmInit

;4WD engine
pause 500

if Scorpionconnect then		
	low	p0				;Ensure pulsout commands are positive going,
	low	p1
endif

gosub Init4WD

;--------------------------------------------------------------------
;-------------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
			gosub Init4WD
			ArmOnOff = 1
			LargeMotor = 0
			Sound 9,[50\1760,25\1,50\1760]		
		endif
	endif	
	
	if CmdMode then
;4WD		
		if (DualShock(1).bit2 = 0) and LastButton(0).bit2 then	;R3 Button test
			MoveMode4WD = MoveMode4WD ^ 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 < Range4WD) then	;Right Button test
			MaxSpeed = MaxSpeed + 10
		elseif (DualShock(1).bit7 = 0) and (MaxSpeed > 10)			;Left Button test
			MaxSpeed = MaxSpeed - 10
		else
			goto NoSound1
		endif
		Sound 9,[100\(MaxSpeed * 5 + 100)]
NoSound1
		if DualShock(2).bit2 = 0 then 	;L1 button test
			LeftNeutral4WD = (LeftNeutral4WD - 1) min 1400
		elseif DualShock(2).bit0 = 0	;L2 button test
			LeftNeutral4WD = (LeftNeutral4WD + 1) max 1600
		else
			goto NoSound2
		endif
		Sound 9,[30\((LeftNeutral4WD - 1300)*5)]
NoSound2	
		if DualShock(2).bit3 = 0 then 	;R1 button test
			RightNeutral4WD = (RightNeutral4WD - 1) min 1400
		elseif DualShock(2).bit1 = 0	;R2 button test
			RightNeutral4WD = (RightNeutral4WD + 1) max 1600
		else
			goto NoSound3
		endif	
		Sound 9,[30\((RightNeutral4WD - 1300)*5)]
NoSound3		
		if MoveMode4WD then	;One Joystick Mode
			
			Right4WD = ZCoord + WCoord
			Left4WD = WCoord - ZCoord
			
			if Right4WD > 100 then
				Right4WD = 100
			elseif Right4WD < -100
				Right4WD = -100
			endif
			
			if Left4WD > 100 then
				Left4WD = 100
			elseif Left4WD < -100
				Left4WD = -100
			endif
			
			Right4WD = abs(Right4WD) * MaxSpeed / 100 	; **** due to bug in Basic ****
			if (ZCoord + WCoord) < 0 then
				Right4WD = -Right4WD
			endif
			Left4WD = abs(Left4WD) * MaxSpeed / 100 	; **** due to bug in Basic ****
			if (WCoord - ZCoord) < 0 then
				Left4WD = -Left4WD
			endif	
		else				;Tank Mode
			Right4WD = abs(WCoord) * MaxSpeed / 100 ; **** due to bug in Basic ****
			if WCoord < 0 then
				Right4WD = -Right4WD
			endif
			Left4WD = abs(YCoord) * MaxSpeed / 100 	; **** due to bug in Basic ****
			if YCoord < 0 then
				Left4WD = -Left4WD
			endif
		endif
		if LockForward then
		 	if (Right4WD < 0) then
				Right4WD = 0
			endif
			if (Left4WD < 0) then
				Left4WD = 0
			endif
		endif
	
		if Scorpionconnect then
			pulsout 0,(LeftNeutral4WD + Left4WD)
			pulsout 1,(RightNeutral4WD + Right4WD)
		else
			serout SSC32,i38400,["#12P",DEC (LeftNeutral4WD + Left4WD)," #13P",DEC (RightNeutral4WD + Right4WD),13]
		endif
		
		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
		if Scorpionconnect then
			pause 20
		else
			nap 1 	;internal sleep mode, approx 19ms		
		endif
	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		
	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 4WD Init
Init4WD
	if Scorpionconnect then
		pulsout 0,(LeftNeutral4WD)
		pulsout 1,(RightNeutral4WD)
	else	
		serout SSC32,i38400,["#12P",DEC LeftNeutral4WD," #13P",DEC RightNeutral4WD,13]
	endif
	
	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
;--------------------------------------------------------------------