Not hard.
Say you have 3 servos: Rotate, Sholder and Elbow --Pretty much what you have not including the gripper. You are in position A and you want to goto position B… Smoothly…
You will have (2) sets of (3) variables. One set will be used for the (3) servo’s current position. We will call these variables Current1 current2 and current3. Current1 is the current servo position for rotate, current2 is the current position of the sholder and current3 is the current postion of the elbow-joint servo. We have another set of (3) variables that we will use to tell where we are going. Say you want the arm in a new position, you want to “request” a new position. Actually, you want to request a new position for all 3 servos. Thus we will call these 3 variables Requested1, Requested2 and Requested3. Again, these are just different names for b3,b4 and b5.
We start everything by centering all the servos at the beginning of the program. All the servos need to start at say, position 125. Now, “seed” all the “currents” and “requested” with 125. Current1=125, Current2=125, Requested1=125… etc. etc.
Now when we go into the main program loop, the servos are all at position 125, all the “current” variables are at 125 and “where we want to go” (requested) are all at 125 as well. We know where we are, and we want to go where we allready are.
To Make a “Move” you need a simple subroutine. Lets say we just started (so we are centered) and we want to raise the arm. Lets also say that the “raised arm” numbers are: Rotate 125, sholder 250 and elbow 250. First, we make the Requested variables reflect what we want. Requested1=125, Requested2=250 and Requested3=250. Next, you call on your “move servos” subroutine.
Moveservos:
do
If requested1>current1 then current1=current1+1
if requested1<current1 then current1=current1-1
if requested2>current2 then current2=current2+1
if requested2<current2 then current2=current2-1
if requested3>current3 then current3=current3+1
if requested3<current3 then current3=current3-1
servo pin,current1
servo pin,current2
servo pin,current3
pause a little to control overall speed
loop until current1=requested1 and current2=requested2 and current3=requested3 '<–when everything matches, we are where we want to go
return
That’s it --If what you want is different than what you have, step a little until you are there. You may need to add some min/max’s to stay on the safe side. It is a chunky way to do it, but it will work fine.
