Author Topic: Servo Calibrator  (Read 2723 times)


  • Member
  • ****
  • B
  • Posts: 22
Servo Calibrator
« on: May 21, 2019, 10:20:42 AM »
This tool finds the relationship between servo pulse time and rotational angle. A simple brief that uncovered a number of tricky issues.

When constructing any robot that moves, work has to be done to define the movement. If using pulsed servos then this ‘definition’ will likely be in µS and quite specific to the actual motor. Should the motor fail and be replaced, or a second bot made, then the definition will no longer be accurate.

Calibrating each joint in degrees, rather than µS, will remove the need to redo the original work.

So an optical shaft encoder is coupled to the joint ‘axle’ to act as an electronic protractor. Then a rotary switch is used to manually find the mechanical limits of the joint plus a reference point.

Next a Nano runs a series of tests and calculates the three coefficients of a second order polynomial ‘best fit’ curve in the form:

y = ax^2 + bx + c

This relationship can then be used to convert from degrees to µS.

*Tester.jpg (75.45 kB . 1024x575 - viewed 204 times)

The bot under test must be held firmly so that the shaft encoder can be positioned to measure each joint. The test setup shown is simple as it is directly coupled to the servo. Think of R2D2 accessing the Death Star systems.

In many cases the servo may act on the joint indirectly via link rods or cables. It is that joint rotation that we must measure, so special adaptors may be required.

The Process

Firstly with the servo connected to the calibrator tool, the limits are established using an EC11 rotary switch. Results are displayed on screen:

Servo Calibrator
Rotate and press knob at servo reference = 1360 uS
Rotate right and press knob at servo maximum = 2240 uS
Rotate left and press knob at servo minimum = 570 uS

Next the Nano zeroes the shaft encoder at the reference point and spins the servo to check for a secure mechanical interconnection:

            uS       deg
Min   570    78.45
Max   2240   -75.00
Min   570    78.60
Max   2240   -75.00
Min   570    78.60
Max   2240   -75.45

Then shaft encoder readings are taken every 100 µS:

uS             deg
570            78.60
670            66.00
770            57.00
870            44.70
970            36.45
1070    26.55
1170    17.25
1270    8.55
1370   -0.30
1470   -9.60
1570   -17.55
1670   -26.70
1770   -34.65
1870   -43.20
1970   -51.45
2070   -60.60
2170   -69.00
2240   -75.15 

And finally the coefficients for µS = a*(deg)^2 + b*(deg) + c are printed:

a = 0.00976096
b = -11.08201885
c = 1364.44213867

Checking the trendline conversion using -60.60 deg, the result is 2072 µS. 

EC11 Switch

This dual pole rotary switch has a knurled shaft and is detented. In this state both A and B contacts are open. One click of rotation will produce a pattern of closures. Connected to the Nano inputs configured with internal pullup resisters, a 2-bit Gray Code is detected.

If the switch is spun too quickly then pulses will be missed. This is not important as the servo is moved under human control and errors can be corrected. The mechanical switch contacts will bounce so a state machine approach is used with the clocking set to 3mS. At clock time the contacts are inspected and a servo pulse is generated if the refresh time of 40mS has passed.

Direction can be determined from the Gray Code changes and the servo pulse time is incremented or decremented by 20 µS each click.

Pushing down on the switch shaft closes the C contact. Again very noisy from switch bounce and the human finger action. The code waits one second before again interrogating this input.

EC11 Diagram.jpg
*EC11 Diagram.jpg (20.56 kB . 408x378 - viewed 208 times)

These states are easily coded using the Arduino switchcase structure eg:

  switch (bounce) {           
case 0:   
  if (ECB == 0)
  bounce = 1;


  }                           //end of switchcase bounce

Electronic Protractor

This uses optical sensors and an etched disc to produce a 2-bit Gray Code. Ideally an 8-bit or 10-bit disc could be interrogated just once when it had stopped. My encoder is a precision instrument and cost £10, but for that you only get two bits.

The servo speed of rotation is not too different from the shaft encoder output rate. However, without using interrupts some code changes will be missed. But the problem with using interrupts whilst driving a pulsed servo is the loss of clocking stability. So the servo will twitch. Nick Gammon’s blog delves into the underlying code overhead for push and pop. Even if the ISR immediately returns, the servo will still move about.

The special nature of the Gray Code is that it only changes one bit at a time. So if we are at state 00 and see a change to 01 it moved left, say. Change to 10 and it moved right. See a change to 11 and it must be an error.

Now put that error in context. If the previous rotation was to the left then it likely means we missed a move to the left and could correct for the code error. Mostly we can survive a single missed move and in practice this seems to hold here. Of course the state diagram is a bit more complicated.

SE Diagram.jpg
*SE Diagram.jpg (30.01 kB . 458x394 - viewed 202 times)

The stated accuracy of my encoder is 600 P/R which suggests 0.6 degree increments. But this seems to assume a ‘count’ for each completed 2-bit pattern. In practice the Gray Code changes four times per ‘count’ so it actually can detect a 0.15 degree movement.

Once the shaft is turned, the state machine will eventually synchronise, no matter where it began. Then the shaft is moved to the reference point and the count is zeroed.   

Regression Analysis

I like to paste test results into an Excel spreadsheet and see the data graphed. Excel can also apply a trend line and show the equation.

*Trendline.jpg (13.49 kB . 338x194 - viewed 209 times)

But for this calibration tool the trendline coefficients will be calculated by the Nano.

John Brooks’s blog shows a straight line example which works ok. Most of the servos I have tested showed a slight curve mainly due to the non-linearity of the positioning pot. A second order curve is a better fit.

Now it gets interesting. Fortunately, Dr Vogler at The Math Forum explains in detail how to use least squares quadratic regression. With great care I turned these complex expressions into C++ but still ran into a problem with the results.

Coding exactly the same functions into Excel the ansers were correct and matched the secret trend line coefficients. So what went wrong with the Nano?

It seems that, apart from the Due maybe, Arduino C++ uses only 32-bit floats on the Atmega chip. Code for double and it will be just the same as a float. XP is obviously a 64-bit environment and can handle large numbers with many places of decimal.

Test just one small part of the regression calculation eg: ∑x*∑y*(∑x)^4. Using the values printed out during calibration, Excel might give 3.227565150656 E22.

A 10-digit calculator shows 3.2275539 E22 so decimal accuracy has been lost after eight digits. If asked, the Nano simply shows an overflow in any term with powers above ^2. The numbers get huge.

Luckily a work around is available - the x and y values must be pre-scaled!
Divide each down by 10000 and the calculation becomes much more manageable. Of course at the end the coefficients must be re-scaled to get back to µS and degrees.


The video shows a 3-motor leg module under development. It has protruding 2mm axles for coupling to the calibration tool.

The 9g pitch motor has one direct link to the upper leg segment. The heel motor has two links via a bell crank to move the lower leg segment. If the pitch motor is still, the heel motor can rotate the ankle. If the heel motor is still and the pitch motor turns, then the heel angle will change and is the sum of both rotations.

The point of the demonstration is this:

•   The pulsed servos were calibrated in degrees.

•   3D Arduino IK equations convert from IKx, IKy and IKw coordinates.

•   Simple x,y,w sequences now generate the gait.

•   The leg moves in practice as designed in theory.

•   Riding height etc can now be changed on the fly.


Working with the PCA9685

A few servos can easily be driven using the delayMicroseconds instruction.

For a bot with a dozen or more motors the PCA9685 breakout board offers very convenient connectivity. Pulse times are set via the two-wire i2c bus and the servos will be constantly refreshed by the board.

*PCA9685.jpg (14.7 kB . 225x225 - viewed 223 times)

This is a clever re-use of a chip originally designed to control LEDs. But it doesn’t use microseconds for the pulse times so will require conversion of the calibrator results.

First we determine the refresh cycle time, say 20 mS. A prescale value is then set in control register 0xFE. My little ‘scope showed the refresh time to be shorter than expected but it could be tweaked by changing the prescale value.

The chip divides the cycle time into 4096 parts. So if it were exactly 20 mS then each chunk of time would be 4.8828 µS. This is the smallest change that can be made to the pulse time and we will need another float calculation for the conversion.

So it has got messy again! Better to include the PCA9685 in the original calibration process and find a relationship between angle and the number of ‘chunks of time’. Then we can just send whole numbers via the i2c bus.

If you want you can interleave the pulse times or stagger them to avoid power glitches. The simplest approach is to have each pulse start at time 0 and finish after p * dt. We don’t need to know how long that is as long as it is calibrated out.

So the revised process will have the Nano step 20 * dt (about 98 µS) and measure the angles.


« Last Edit: May 21, 2019, 10:30:38 AM by BaldwinK »


  • calling from free Europe
  • Sr. Member
  • *****
  • Posts: 470
Re: Servo Calibrator
« Reply #1 on: May 22, 2019, 02:28:13 AM »
like this for the little marauders to experiment with, thou am thinking "with coffee in hand " that every servo has a pot inside , could we  take a wire on the pots output pin and feed that back  to an ADC pic on a mcu with some voodoo maths and get ourselves some kind of feed  back,, removing the need for  a shaft encoder in a bot?

PS love the peg  on the  shaft very rebelish  solution   ^-^
I build bots


  • Member
  • *
  • B
  • Posts: 86
Re: Servo Calibrator
« Reply #2 on: May 24, 2019, 04:01:10 AM »
Jinx, when I bought the Dagu service droid it come with 2 little servo's that had an extra wire to connect the pot to an analog input on a mcu. OddBot came up with that solution for the pincher and gripper.

My biggest concern with hobby servo's is the accuracy & repeatability. It's not the best feature of cheap ass Chinese servo's...


* Search

* Recent Topics

The unnamed (yet) quatruped spider project by tinhead
[July 01, 2020, 04:22:11 PM]

"1984 Nixie Time" by 1 what
[May 08, 2020, 01:04:18 AM]

2D Side Scroller Cyberpunk themed by Killer Angel
[February 06, 2020, 06:39:40 AM]

A new wing design for model aircraft / drones by OddBot
[February 06, 2020, 04:42:06 AM]

SDR (Software Defined Radio) by Gareth
[February 02, 2020, 06:15:42 AM]

Circuit Math by ZeroMax
[January 31, 2020, 01:50:18 PM]

NanOMeter by Protowrxs
[January 01, 2020, 12:59:44 PM]

Investigating the VL53L0X Laser Rangefinder by erco
[December 30, 2019, 10:45:44 PM]

PS4 Single Handed Controller Deployed (part 7 of 7) by Gareth
[December 30, 2019, 09:52:29 AM]

"D" -Pad Workio just like Magic (Will Merlin stay or Go) (part 6 of 7) by Gareth
[December 30, 2019, 09:51:27 AM]

PS4 Joystick Digitals 4,5,6,7,10 - Analog's Lx,Ly,Rx,Ry Workio (part 5 of 7) by Gareth
[December 30, 2019, 09:50:37 AM]

Menu Workio ! (part 4 of 7) by Gareth
[December 30, 2019, 09:49:49 AM]

L1 trigger design Workio (Hori controller) (part 3 of 7) by Gareth
[December 30, 2019, 09:48:50 AM]

Hori aka PS4 Joystick Mappings (part 2 of 7) by Gareth
[December 30, 2019, 09:47:17 AM]

PS4 Single Left-Handed Controller (part 1 of 7) by Gareth
[December 30, 2019, 09:44:58 AM]

* Recent Posts

Re: The unnamed (yet) quatruped spider project by tinhead
[July 01, 2020, 04:22:11 PM]

Re: The unnamed (yet) quatruped spider project by jinx
[July 01, 2020, 04:06:19 PM]

Re: "1984 Nixie Time" by 1 what
[May 08, 2020, 01:04:18 AM]

Re: "1984 Nixie Time" by tomasp
[April 13, 2020, 06:03:28 PM]

Re: 2D Side Scroller Cyberpunk themed by Killer Angel
[February 06, 2020, 06:39:40 AM]

A new wing design for model aircraft / drones by OddBot
[February 06, 2020, 04:42:06 AM]

Re: "1984 Nixie Time" by Gareth
[February 02, 2020, 06:23:01 AM]

Re: SDR (Software Defined Radio) by Gareth
[February 02, 2020, 06:15:42 AM]

Re: SDR (Software Defined Radio) by ZeroMax
[January 31, 2020, 01:54:21 PM]

Re: "1984 Nixie Time" by ZeroMax
[January 31, 2020, 01:52:29 PM]

Circuit Math by ZeroMax
[January 31, 2020, 01:50:18 PM]

Re: 2D Side Scroller Cyberpunk themed by ZeroMax
[January 31, 2020, 01:45:33 PM]

NanOMeter by Protowrxs
[January 01, 2020, 12:59:44 PM]

Re: Investigating the VL53L0X Laser Rangefinder by erco
[December 30, 2019, 10:45:44 PM]

PS4 Single Handed Controller Deployed (part 7 of 7) by Gareth
[December 30, 2019, 09:52:29 AM]