Seeking Smoother Robotic Arm Movement

[peterclones]

I'm new to Picaxe and have built a 3 servo robotic arm with a 08M2. It works fine using a modified version of Erco's code from his 6 servo arm. It moves each servo 1 degree simultaneously with a nice subroutine. However, I'd like each servo to end up at the target, or "goal" as it's called in the code, at the same time for a smoother transition. Has anyone seen such a code here or know how to implement it? I'd imagine it would take a fair bit more complexity and math equations. Here's what I have now in my "move" subroutine for servos 1, 2 and 4:

move:'                 			move subroutine  move slowly to new goal coordinates
test1:
if servo1=goal1 then test2
if servo1<goal1 then inc servo1
elseif servo1>goal1 then dec servo1
endif
servopos 1,servo1  ' 			finger up down

test2:
if servo2=goal2 then test4
if servo2<goal2 then inc servo2
elseif servo2>goal2 then dec servo2
endif
servopos 2,servo2  ' 			elbow up down

test4:
if servo4=goal4 then test5
if servo4<goal4 then inc servo4
elseif servo4>goal4 then dec servo4
endif
servopos 4,servo4  ' 			shoulder rotate left right
test5:
pause delay
if servo1=goal1 and servo2=goal2 and servo4=goal4 then fin
goto test1          ' 			loop back until motion stops
fin:		        ' 			servos stopped, all goals reached
return

 

[erco]

That's some sweet code you have there, peterclones! Actually your idea wouldn't be that hard to implement. First, figure what the farthest travel distance is of all your 3 servos, then make a single loop based to inc/dec that servopos. Inside that same loop, inc/dec the other servos proportionally. Give it a shot and come back if you need more help.

 

[BeanieBots]

As erco suggests, you will need to calculate the distance (angle) that each servo needs to travel.
However, rather than make multiple loops inside each other, it is easier to adjust the amount by which you increment each servo. To get finer resolution it might also be worth working with number ten times larger than the actual servo number number required such that it is possible to increment by 0.1.

 

[peterclones]

Your ideas make sense but I think it might be beyond my coding skills, which is why I was hoping someone had done it before me. I thought I read somewhere awhile back that someone had done it.

It is a sweet code though, Erco.

 

[MFB]

An approach that may at least provide some of the information (outlined in the above posts) you need might be to develop the kind of 'teach pendant' used on industrial robots. This would allow you to manually drive the arm whilst recording the feedback from the position sensors.

 

[Buzby]

The way I did it years ago, before PICAXE, was a bit like this :

First, calculate the distance ( or angle ) each motor needs to move.

I.e
Distance1 = EndPosition1 - StartPosition1
Distance2 = EndPosition2 - StartPosition2
Distance3 = EndPosition3 - StartPosition3

You now have one distance for each servo, Dist1, Dist2, Dist3.

Now decide how many steps for the complete movement, CStep.

Each servo will make a step at the same time, so the step size needs to be different for each servo.

StepSize1 = Dist1 / Cstep
StepSize2 = Dist2 / CStep
StepSize3 = Dist3 / CStep

Then do a simple loop CStep times, each time moving each servo by the revelant StepSize.

For StepCount = 0 to Cstep

[I]Move servo1 by StepSize1
Move Servo2 by StepSize2
Move Servo3 by StepSize3[/I]

next StepCount

This is the basic idea. You will, as BeaniBots said, get better resolution working with scaled-up numbers.
Also, you will need to handle running the servos in both directions, so there need to be some negative number handling.

Try it with one axis first, to get the algorithm OK, then just add in more axies.

Good Luck,

Buzby

 

[BeanieBots]

Have a look here http://www.picaxeforum.co.uk/showthread.php?7760-Autonomous-Hexapod&highlight=hexapod to see if you can pull any inspiration.
The code is only for 3 servos and uses pulsout (written before servopos was available).
It is not ideal because it assumes all movements will take about the same time irrespective of distance but it does interleave all the movements.

 

[hippy]

Each servo will make a step at the same time, so the step size needs to be different for each servo.

StepSize1 = Dist1 / Cstep
StepSize2 = Dist2 / CStep
StepSize3 = Dist3 / CStep

Then do a simple loop CStep times, each time moving each servo by the revelant StepSize.

Watch out for rounding errors; for example if StepSize1 should be 1.2 but you can only move 1 step at a time, after 10 steps you will have have only moved 10 steps not the desired 12.

You can mitigate that by using fractionals; a word value can be considered with its MSB representing 0-255 and the LSB representing 256th's ...

w0 = w0 + wordStepSize
ServoPos SERVO_PIN, b1 ; b1 = w0 MSB

Even so there can still be some error so the best way is probably to calculate where the servo should be between start and desired end position at each timed step.

 

[jonnymelon]

I've been working on a robotic arm project myself and am a total noob, but after a lot of questions and very helpful replies from folks on the forum I managed to write this code for three servos. I don't know if it makes much difference, but it was on a 18m2.

startup :servo 4,225 servo 3,225 servo 2,225 ;start all servos at 225
main:for b0=225 to 150 step-1
 servopos 4,b0   ;move all servos down to 150
servopos 3,b0
servopos 2,b0
pause 50
next b0
for b2=150 to 75 step-1   ;servo 4 and 2 carry on to 75
servopos 4,b2
servopos 2,b2
pause 50
next b2
wait 10

for b1=75 to 225
servopos 4,b1  ;servo 4 and 2 go back up to 225
servopos 2,b1
pause 50
next b1
for b2=150 to 75 step-1
servopos 3,b2   ;servo 3 carries on down to 75
pause 50 
next b2
wait 10
for b1=75 to 225   ;servo 3 returns to 225
servopos 3,b1
pause 50
next b1
goto main

Just change the value between 75 and 225 for your positions. I found the 'pause 50' gave a nice smooth movement here, but you can change the '50' to suit.

 

[peterclones]

Thanks guys. Your advice paints a clearer picture as to what I'll have to do. I'm actually getting decent results today with Erco's code, so I may just stick with that for now and see if I want to take it to the next level. I can see the "Select Case" command in that Hexapod code coming in very handy when comparing several conditions at once, if I wanted to have all servos land on the target simultaneously.