Archive for the 'Programming' Category

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.

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)

Step 2



Find T2 using the Law of Cosines

L12 + D2 – L22 = 2 * L1 * D *cos(T2)

rewritten …  T2 = acos( (L12 + D2 – L22) / (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.

L12 + L22 – D2 = 2 * L1 * L3 * cos(T3)

rewritten … T3 = acos( (L12 + L22 – D2) / (2 * L1 * L2))

Step 4



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.


Hobby/RC Servo Control in PSoC



The PSoC family is my go to line of processors for prototyping.  It is like having a breadboard full of digital and analog circuits that you can wire up on the fly. I have been doing some stuff with hobby servos lately so I needed to figure out how to do it on the PSoC.

Hobby Servos


From Wikipedia


Image from Adafruit


Hobby servos set their rotation based on the length or a repeating pulse. The pulse should be 1ms to 2ms long and repeat every 20ms.  One end of the rotation is at 1ms and the other is at 2ms.

The PSoC PWM  Component


The PWM component is perfect for this job.  The PWM component can be setup to have a period and an on time.  The period should be 20ms and the on time would be between 1ms and 2ms.  The component uses a clock and two counter values.  The component will count on every clock pulse.  It resets the counters after the period count has been reached and the CMP value determines how long the pulse is logic high.

The PWM output goes to the servo control line.  Here is the configuration dialog box for the PWM component. The graph at the top is a good reference for what the output will look like.


The goal is to have a pretty decent resolution to set the 1ms to 2ms pulse.  I chose a 2MHz clock.  I picked the fastest clock that would still fit within the 16bit (65535) limit of the control.  PSoC clocks are derived from system clocks, so you need to pick values easily divided down from them.  The IDE helps with creation of these clocks.  At 2Mhz the period (repeat rate) should be set to 40,000.  The equation is the clock * period(in second) = period counts (2,000,000 counts/sec * 0.02 secs = 40,000 counts).

The CMP Value is how many counts the high pulse should last.  The equation is the same. For 1ms the count would be (2,000,000 cnts/sec * 0.001secs =  2,000 counts) and for 2ms the counts would be 4,000.  The range is 2,000 to 4,000 (2,000 count resolution).  This is better than most hobby servos can do.

The Code

The IDE will generate a bunch of functions, a custom API, for each component used when the application is built. There are two PWM Component functions we need to use for this application .

  • PWM_Servo_Start() This will initialize the component and get it running. This is called once at the beginning of the program.
  • PWM_Servo_WriteCompare(val) This sets the CMP Value that will be used to set the pulse length.

I also wrote a function the can set the value by degrees.

void setServo(float degrees)
unsigned int val;
// convert degrees to compare value
// 2000 to 4000 = 0 to 180
// value is
val = (degrees / 180.0 * 2000.0) + 2000;


The Results

Here is a screen shot of my logic analyzer. The output was set for 1/2 rotation. The pulse is 1.51ms and the period is 20.14ms.  That is close enough for me.  It is likely the clock speed is different between the PSoC and  and the analyzer.



Typically you will have to tune the to the actual servos used.  Just tweak the endpoint values until you get the rotation you want.