Need help with code for a custom built arm

Lynxmotion Robot Arms
1

Hello,
I am building a mobile robot with 4DOF arm attached to it. the robot will use an Atom pro 28, Bot-Board 2, and the SSC-32. I finished the assembling the robot, and installed all the electronics.

http://i412.photobucket.com/albums/pp204/weyandjim/img148.jpg

I have some familiarity using the Atom pro 28 with the SSC-32. I am also controlling the arm with a wireless PS@ controller. I’m having some trouble understanding and modifying code that I downloaded to operate the Lynxmotion style arms. The lengths of my arms much longer, at 11.5 in for the arm, 11.5 inch for the forearm, and the hand/gripper is about 5" long. When I run the unmodified code on the Atom pro, the arm moves when I push the joysticks, but the movement is very random. I would assume this is due to the fact that I didn’t rescale the arms in the code. I tried to chage the arm lengths constants, but thus sent the arm into a seizure. I worked out all the inverse kinematics from the code and wrote a MATLAB program to display the arm positions by changing the angles based on my inputs. I understand most of the program, but the SetAngle sub-routine is a little confusing. What areas of the coded need to be changed to allow for my long arms to work with this code?

Here is the code that I am trying to modify. Thanks for all the help! - James

[code];--------------------------------------------------
; 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, [email protected]
; Modified by James Frye, [email protected]
;
;--------------------------------------------------
;
; 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

;--------------------------------------------------------------------[/code]

2

Hi James,

Sorry no one got back to you sooner on your question. It’s a little tough! I just happened across it while trying to decide where to post a question of my own.

Have you made any progress?

I notice that your arm is longer, so I suspect the variables are overflowing when the lengths of the various segments are squared. this is part of the IK calcs that you see in the SetAngle sub-routine you asked about.

I didn’t spend more then a few minutes looking at the SetAngle code, but it’s very similar to the IK calcs done in the Hexapod. For that, we solve two triangles, and then do a “Cosine Law” calculation. You can probably read through the IK calculation description for the hexapod, and see the similarities.

Back to you problem, you’ll probably have to go to a long instead of a word for some of the “Tmp” variables to get around that problem.

About all I can offer for now.

Alan KM6VV

3

Can any one help me how to get the Matlab code to control AL5D arm? I want to control the arm from matlab environment but I don’t know how to start with you it.

4

Here I’m guessing as to what the problem is. I think that the coordinate variables were overflowing as they were only Sbytes. I changed them to Sword to increase the range for the calculations. I also added in the custom lengths for your arm. I’m not able to test this for you so you’ll have to let me know if anything changes…

[code];--------------------------------------------------
; 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, [email protected]
; Modified by James Frye, [email protected]
; Modified by Devon Simmons aka Dev5994
;
;--------------------------------------------------
;
; 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:
;--------------------------------------------------

;Custom Arm
Arm_Length con 292 ;11.5" = 292.1mm (11.5 * 25.4)
Forearm_Length con 292
Hand_Length con 127

;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

;DS I changed these four variables from Sbyte to Sword
XCoord var Sword
YCoord var Sword
ZCoord var Sword
WCoord var Sword

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
;--------------------------------------------------------------------[/code]