SCHervo Library and ServoTask

I made some of my usual "improvements" to the standard Arduino Servo motor control library.  Building on my Task Scheduler I've added a speed control, so the time it takes a Hobby Servo motor to move from it's current position to a new one can be controlled over a fairly large range.

It did take some reverse engineering...

The regular Servo library uses Timer 1 (on the 'standard' Arduino ATMega 328's -- I haven't worked this out all for other chips) to operate up to 8 (they say 12 but I think it gets a bit slippery after 8) servo motors. It does this by sequencing the ON pulses for each motor, one-after-the-other, which is pretty slick. Or would be IF the authors had mentioned what they were doing someplace. There's very little internal documentation -- Comments, Please! -- in the code. But I persisted.

My SCHervo library comprises a cleaned up and (hopefully accurately) commented original version, with the usual Schip Secret Sauce additions.  A simple addition is a turnOff() method which just shuts the motor off so it isn't using power trying to hold a position (or speed) that you don't care about. The more complicated addition is a Task to update the position -- and thus speed -- of each motor over a fixed interval. This allows the motor to transit in pseudo-continuous increments over a fairly extended period (up to about 1 minute). The motion is initiated using the startMove() method, can be monitored with ready() which returns TRUE when the motor has (just about) reached its new position, and stopMove() to make it stop at any time in-between.

But first lets review just this much: How do Servos work?

The basic idea is that you send the motor a positive pulse every 20mS (big T in the picture below), where the width of that pulse (little t) is proportional to the position one wants the motor shaft to take -- usually defined as from 0 to 180 degrees. Each degree position translates to a pulse width, which generally varies from 500 to 2500 micro-seconds, where 1500uS is a nominal 90 deg center position. Each motor is a bit different in it's range and widths, but that's the general scope.

picture of a servo drive pulse train

And here is a better description.

Another thing to remember is that the motor doesn't just suddenly go to the new position, but has a finite slew rate, generally something like 60 degrees in 200mS. This works out to 5 or 6 degrees in each 20mS refresh period, which is also the fastest the motor can get any new position input.

So... If we change the delivered pulse width in every 20ms period we can control the motor's angle change speed. And that's what the ServoTask() does. Recall that all of the 8 controllable motor's pulses are sequential, stacked such that the next starts after the previous finishes, and when they are all done there is a slack period until the 20mS refresh times-out. The ServoTask() is posted from the timer interrupt service routine at the beginning of the slack period, and -- in theory -- it will execute and update the desired pulse widths for the next period.

When a motor is started using startMove( endPosition, time, offFlag ) the code calculates how many micro-seconds should be added to the current pulse width in order to transition from the starting to end positions in the given amount of time. I got a little tricky and used a Fixed Point calculation with 3 bits of "sub-precision", to handle longer moves, but that's just between you, me, and the code.

However ... Fixed Point

If you know me, you know, I can't resist a few, more, comments. The Arduino using an ATMega 328 has no hardware support for Floating Point math. Should you make the mistake of including a float value in your program the linker will pull in about 1kB of code to support it. And further, should you blunder into actually using the float in a math-like-way, the result will take (relatively) forever. It's actually even worse, as the ATMega has only a small set of 16bit integer Multiply instructions (along with ADD and SUBTRACT) and NO Divide at all.

So what do you do when you would like to maintain some fractional components in your arithmetic? Why Fixed Point of course!  I'll leave it to wikiP to explain: https://en.wikipedia.org/wiki/Fixed-point_arithmetic

The tldr; of it is that you shift int values up by a consistent number of bits, do your arithmetic, and then shift them back down when you want to get a nice truncated integer again. This needs to be done judiciously because you only gain the precision of the up-bit-shift, and this number of bits is removed from the range of your values. In the case of my 3bit FixP values using a 16bit int, you get a precision of 1/8th or .125 in decimal and a range of +/- 0 to 4096 (12 bits plus sign). Which turns out to be just fine for calculating the micro-second values needed by the servos.

TBD

Note that the old-fashioned Servo.write() interface does not provide a way to determine when the motor is (almost) done moving. My startMove() method tries to account for the slew rate during fast motions by adding some guess at the amount of time needed. But this runs into a bit of trouble from the internal motor controller, which usually treats smaller moves as slower changes (this is how you are sometimes able to get speed control from motors that have been modified for continuous rotation). This behavior also slows the motor down when using the ServoTask() incremental stepping, so the internal how-long? guess is not always right.

I could do a little more hacking to better integrate faster slews -- it shouldn't be THAT hard, heh -- and this would also eliminate the need for the 'regular' write() interface (or draw it into the fold) such that, in all cases, the ready() method will really tell you when the motor is done moving

But. Otherwise. You should be able to just go ahead and use it now...