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...

The 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 perform the calculations in the Cartesian coordinate system by taking the axis of servo S1 as the origin. The following little formula you've probably learnt (and forgot) will come in handy. Its refered to as the Law of cosines.

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).

 O3 = Arccos ( ( L2^2 + L3^2 - R2^2 ) / (2.L2.L3) )

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^2 - L3^2) / (2 . L2 . R2) )

O2a = Arccos ( ( L1^2 + R2^2 - R3^2) / (2 . L1 . 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.