5dof arm home position

Lynxmotion Robot Arms
1

Ok, As everyone should know, I finally got my 5DOF arm to work well with the PS2 controller and the Basic Atom 28. I now want to make a “home position” for the arm to allow it to “rest” while not in use. I want to use the “O” or square button to do so since they are not programmed for use. Unfortunately I dont know where to start. Can anyone give me a little starting point in code so I know where to begin? Thanks.

Also, I would like to know how I can utilize the “D pad” on the PS2 controller to controll a Pan and Tilt assembly for a camera. I would use the Hitek but unfortunately can’t seem to get it working properly when the robot is also in use.

Any help would be appreciated.

2

BTW, these codes need to work with this Lynx Arm program by Laurent Gay. Is there a specific order I have to put them into the code or just add them somewhere at the bottom? Thanks.

[code];**************************************************
;**** 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] ***
;**************************************************
;
; 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 1500
Base_PulseMin con 850
Base_PulseMax con 2150

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 24
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 25
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 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)

;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 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
	serout p15,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 p15,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 p15,i38400,"#",DEC WristRotPin,“P”,DEC WristRot_Pulse," T100",13]
NeedServosOn = 0
Shortcut2
nap 1 ;internal sleep mode, approx 19ms

LastButton(0) = DualShock(1)
LastButton(1) = DualShock(2)

goto main

;--------------------------------------------------------------------
;-------------Sub Update Position
ArmUpdatePosition
serout p15,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 p15,i38400,"#",DEC BasePin,"P",DEC Base_Pulse," S1000",13]
	serout p15,i38400,"#",DEC GripPin,"P",DEC Grip_Pulse," S1000",13]
	serout p15,i38400,"#",DEC WristRotPin,"P",DEC WristRot_Pulse," S1000",13]	
	Time = 750
endif
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

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

3

HAHA! SUCCESS! I realised that I dont need to put the pan and tilt system on the ps2 controller because I got it to work with my Hitek 4. I realised last night that every time I used the Pan and tilt at the same time the motors were running, it was drawing too much current and draining the motor controller of power, which is why it kept resetting and getting out of controll. I hooked up a 9v battery just now to my hitek reciever and WALA! No “interference” :smiley:

4

Yahooo! Great job Joe!

I may be as prowd of you as you are. :wink: hehehe

5

Thanks. I know you told me a while ago about the amount of energy I needed and what I currently had, which I tried before but never worked. Than I upgraded to a nine volt battery :smiley:

6

Ok, never mind, the servos seem to be interfering with the robot’s controls again. :confused: So i have decided to try and have them run off my abb and ssc 32 again for better control and convinience. Anyways, how can I program the ps2 controllers D pad to control the pan and tilt and how do I incorporate it into the above code :question:

7

C’mon Joe… We are going to need a lot more information in order to sort this out. I’m sure you have a simple wiring error. Any chance you can provide a run down of the jumpers installed on all boards, what is connected to the power terminals, and what batteries are connected to what? If it’s complete and accurate we can figure out everything from that.

8

Ok, well the test last night was with the motors running and the servos moving all at the same time. It worked really well and there didnt seem to be a problem. Today when I ran everything (motors with the wheels on now), the problem started up again. This is what happened. When I moved the Servos rapidly with the robot turning and running, the scorpion started to blink out of controll and sent the robot in all different directions. EVerything is set up as it should be, I triple checked everything. I just rather would like to have all the servos running on the ABB and SSC 32 because Its more organised and will be alot easier to move the arm and the camera at the same time.

9

Uggg… You say “everything is set up as it should be” but I submit to you it is not. Else if it were, we would not be having this conversation… :wink:

Before we try to modify any programs and stuff I think it’s a good idea to figure out what is going on. Because you will not provide me with details I am forced to assume things. :frowning: I assume you are using the tutorial for 4WD1 with either an L5 or L6 connected to the top.

lynxmotion.com/images/html/build085.htm

So you are having trouble with the Scorpion when moving servos on the RC stick radio. What is powering the RC receiver? What battery, and is it powering anything else? You have the Scorpion connected to the Bot Board I/O pins 0 and 1 for control right? I suspect you are powering the RC receiver from the Bot Boards 5vdc regulator and when you move the pan and tilt servos the Bot Boards regulator is browning out and the Atom is resetting. Just my best guess with the information provided. Let me know…

10

nope. the scorpion is not connected to the ABB or ssc. I am using the hitek controller to control the bot seperately from the arm (makes things a lot easier and organised). Also, the RC reciever is powered by a nine volt battery (duracell) as you suggested. The arm is not part of the rover BTW.

11

Hoookay…

So you have the Scorpion connected to the RC receiver along with 2 servos for a pan and tilt. When you move the pan and tilt servos using the remote control the Scorpion gets spooky. Dude you probably never disabled the battery elimination circuitry on the Scorpion. So when your 9vdc battery was drained down by the BEC on the Scorpion then the receiver was being powered only by the Scorpion. The Scorpions BEC can’t supply enough current for servos, only the receiver by itself. So when you move the pan and tilt servos the BEC on the Scorpion browns out and the Scorpion looses it’s mind. I never told you to power the RC receiver from a 9vdc battery. The easy fix is to disable BEC on the Scorpion, thisa can be done by pulling the red wires from the cables that connect the Scorpion to the receiver, or cutting the BEC disable trace on the Scorpion, then provide the battery power from the Scorpion (the one that powers the motors) to the RC receiver. This will provide the servos connected to the receiver with enough current so it will stop crashing the Scorpion. Whew!..

12

ahhh. Ok, well the BEC on the scorpion dosent seem to be soldered together…SO i just have to take out the red wires from the cables that connect the scorpion to the motor controller?? hmmmm…How come the scorpion has a BEC? Is it to protect it??

13

Finally some progress. :smiley:

The BEC is to power the receiver without useing a battery just for that purpose. BEC=battery elimination circuitry.

14

ok, so im going to pull out the red wires from the cables. Ill let you know that it worked in a sec…

15

Ya, funny thing…It didnt work. and now becuase I took out those wires, I cant get the signal from my RC to the reciever so the bot dosent even move now… :open_mouth:

16

Are you sure the motor battery is connected to the RC receivers battery input? Can you move the servos in the pan and tilt?

17

JIM UR DA MAN! WOAH! Yep, there was a loose wire, but now it works good. Thanks…Hey, I learn from my mistakes :smiley:

18

Yaaaahoooey! You made my head hurt. :open_mouth: But I’m really glad it’s going again. LOL

19

Your head or his robot? Heh heh, I had to ask… :smiley::smiley:

8-Dale

20

I will let you know… you know… you know… tomorrowowowow. :open_mouth:
hehehe