Buildlog.Net Blog

Yet Another Way to do the Kinematics

Paul Kaplan, originator of the Easel project, came up with another way to do the kinematics for the Line-us Clone. My method used intersecting circles. His method uses the Law of Cosines.

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

The goal is to find the two angles, A1 and A2, of the servo arms

Known Values

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

Step 1

Find the distance “D” of the pen to hub using the Pythagorean Theroem and the angle T1 using arctangent.

Px² + Py² = D²

rewritten … D = Sqrt(Px² + Py²)

 

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)

Step 2

 

Find T2 using the Law of Cosines

L1² + D² – L2² = 2 * L1 * D *cos(T2)

rewritten …  T2 = acos( (L1² + D² – L2²) / (2 * L1 * D))

Step 3

 

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.

L1² + L2² – D² = 2 * L1 * L3 * cos(T3)

rewritten … T3 = acos( (L1² + L2² – D²) / (2 * L1 * L2))

Step 4

 

Determine A1 and A2 from the angles we figured out.

A1 = T1 + T2

A2 = A1 + T3

Conclusion

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.