We thought you might be interested in knowing the mechanics and math involved in four stage arm control. Its quite simple really. We hope this will help a few new hackers with their future builds. Here we go...
aim of this algorithm is to determine the angles that the servos should take
for the robotic arm holding the pen to be positioned at (X3,Y3).
We start by assuming we know (X1,Y1).
S4 servos angle does not need to be calculated as it is only needed for lifting and placing the pen on the paper. We can therefore ignore it in this derivation.
Since L1, L2 and now R2 is known, by using equation we find the angle to be moved by servo 3 (O3).
Similarly, we find O2a and O2b as marked in figure. Adding O2a and O2b, we get the angle O2 to be moved by servo 2.
O2a = Arccos ( ( L2^2 + R2
So we can sum up those angles to find out angle O2.
O2 = O2a + O2b
Great! But we still don't know the value of O1 . This one's a little tricky, take a look at the following figure. We've divided the drawing canvas into three regions.
If the point to be drawn is further than D1 from the origin (as shown), then O1 = Arctan ( y/x ). If the point to be drawn is nearer than D2 from the origin, then O1=Arctan ( y/x ) + pi/2. If the point to be drawn is nearer than D21 from the origin but farther than D1, then O1 = Arctan( y/x )+( pi/2 ).( D2 - R3 ) / ( D3 - D2 ).
Now that we've deduced O1, we can derive points ( X1, Y1 ) using
X1 = L1 . Cos( O1 ) and Y1 = L1 . Sin( O1 ).
There! That wasn't so hard, was it? Next time we'll try to give you some insight into the algorithm that processes the images.