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.
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, 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
;--------------------------------------------------------------------
Login
Register
FAQ
Search
