1

##### Tutorials / Servo Calibrator

« Last post by**BaldwinK**on

*»*

**Today**at 10:20:42 AMThis 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.

[ You are not allowed to view attachments ]

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.

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

PHASE ONE: FIND SERVO LIMITS

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

PHASE TWO: SHOW WORKING LIMITS

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

PHASE THREE: CALIBRATION

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

PHASE FOUR: TRENDLINE COEFFICIENTS

a = 0.00976096

b = -11.08201885

c = 1364.44213867

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

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.

[ You are not allowed to view attachments ]

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

switch (bounce) {

case 0:

if (ECB == 0)

bounce = 1;

...

...

break;

...

} //end of switchcase bounce

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.

[ You are not allowed to view attachments ]

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.

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.

[ You are not allowed to view attachments ]

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.

[ You are not allowed to view attachments ]

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.

[ You are not allowed to view attachments ]

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.

[ You are not allowed to view attachments ]

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.

[ You are not allowed to view attachments ]

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

PHASE ONE: FIND SERVO LIMITS

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:*PHASE TWO: SHOW WORKING LIMITS

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:*PHASE THREE: CALIBRATION

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:*PHASE FOUR: TRENDLINE COEFFICIENTS

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.

[ You are not allowed to view attachments ]

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

switch (bounce) {

case 0:

if (ECB == 0)

bounce = 1;

...

...

break;

...

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

[ You are not allowed to view attachments ]

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.

[ You are not allowed to view attachments ]

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.

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

[ You are not allowed to view attachments ]

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

[ You are not allowed to view attachments ]

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.

[ You are not allowed to view attachments ]