The Law of Cosines relates the lengths of the sides of a triangle to the cosine of one of its angles.
This can be used to find the angles of the servo arms.
(Click on the images if you want a larger view)
The goal is to find the two angles, A1 and A2, of the servo arms
- Px is the desired X location of the pen
- Py is the desired Y location of the pen
- L1 is the length of the upper servo arm (50mm)
- L2 is the length of the end of the Pen Arm (50mm)
Find the distance “D” of the pen to hub using the Pythagorean Theroem and the angle T1 using arctangent.
Px2 + Py2 = D2
rewritten … D = Sqrt(Px2 + Py2)
T1 can be found using the arctangent or inverse tangent formula. Note: When programming use the atan2(x,y) function to preserve the quadtrant.
T1 = atan2(Py,Px)
Find T2 using the Law of Cosines
L12 + D2 – L22 = 2 * L1 * D *cos(T2)
rewritten … T2 = acos( (L12 + D2 – L22) / (2 * L1 * D))
Find T3 using the Law of Cosines. We want the left one of the two T3 angles, but since the linkages form a parallelogram that same angle shows occurs in several places. We will use the right one and the dimensions associated with it.
L12 + L22 – D2 = 2 * L1 * L3 * cos(T3)
rewritten … T3 = acos( (L12 + L22 – D2) / (2 * L1 * L2))
Determine A1 and A2 from the angles we figured out.
A1 = T1 + T2
A2 = A1 + T3
I think I will switch the code to use this method. I think I can optimize it better in C code. The speed of the code is important. The faster it runs, the most times per second we can run it. The more often we run it, the smoother it will run.