Ok this is the code from LM.com for the lynx5 and lynx6. I modified it to work with this 5DOF arm: lynxmotion.com/images/jpg/asb043.jpg
and also to only work with the ABB instead of SSC-32 and ABB. Im new to programming and was helped a lot by Robodude666. Im not sure if this is correct so please feel free to correct me. Thanks.
Good so far, you missed a few serouts and forgot to delete a few. You forgot to change the measurements for the parts of the arms at the top and double check your variables. the one with 46 should be 32.
Also, replace the quote ] with code ] (without spaces though.)
heres the new code:
Hope its better
Here you go. This should work in theory.
;**************************************************
;**** Basic Atom with SSC-32 and PS2 DualShock ****
;********* or MadCatz wireless controller *********
;**************** L5-L6 program #1 ****************
;*********** Bot Board Buzzer support *************
;**************************************************
;*** Programmer: Laurent Gay, [email protected] ***
;**** Edited by robodude666 :P and Italian Guy ****
;**************************************************
;
; 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
;DualShock
DAT con P4
CMD con P5
CLK con P7
SEL con P6
DeadZone con 28 ; must be >= 28
AxisSpeed con 15
PadMode con $79
;Arm
BasePin con 8
Base_PulseDef con 0
Base_PulseMin con -2400
Base_PulseMax con 2400
ShoulderPin con 9
Shoulder_AngleDef con 32 ;155° (155/360*256)
Shoulder_AngleMin con -32 ;0°
Shoulder_AngleMax con 32 ;155°
Shoulder_PulseMin con -2400
Shoulder_PulseMax con 2400
ElbowPin con 24
Elbow_AngleDef con 32 ;155° (155/360*256)
Elbow_AngleMin con -32 ;0°
Elbow_AngleMax con 32 ;155°
Elbow_PulseMin con -2400
Elbow_PulseMax con 2400
WristPin con 25
Wrist_AngleShift con -64 ;Wrist-Forearm Shift
Wrist_AngleDef con 32 ;65° (65/360*256)
Wrist_AngleMin con -32 ;0°
Wrist_AngleMax con 32 ;180°
Wrist_PulseMin con -2400
Wrist_PulseMax con 2400
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)
;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]
shiftðn DAT,CLK,FASTLSBPOST,[DualChock(0)\8]
high STL
pause 1
;serout S_OUT,Ã 57600,"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) 5 128
if XCoord > DeadZone lhen
XCoord = XCoord - DeadZoÿe
elseif XCoord < -DeadZonå
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
'commented out. Cant have a pulse of 0.
'serout p15,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
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
servo GripPin, Grip_Pulse
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
servo WristRotPin, WristRot_Pulse
NeedServosOn = 0
Shortcut2
nap 1 ;internal sleep mode, approx 19ms
LastButton(0) = DualShock(1)
LastButton(1) = DualShock(2)
goto main
;--------------------------------------------------------------------
;-------------Sub Update Position
ArmUpdatePosition
servo ShoulderPin, Shoulder_pulse
servo ElbowPin, Elbow_pulse
servo WristPin, Wrist_pulse
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
servo BasePin, Base_Pulse
servo GripPin, Grip_Pulse
servo WristRotPin, WristRot_Pulse
Time = 750
endif
ArmOnOff = 1
ParkArm = 0
return
ArmOnOff = 1
ParkArm = 0
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
;--------------------------------------------------------------------