High-speed, high-precision, 32 output servo driver (V1.1)
JasonDorie
Posts: 1,930
The standard Servo32 driver is good - it'll run up to 32 servos at 50Hz with a precision of 1us - that's 1000 steps within the standard range of 1ms to 2ms pulse widths.
I've been implementing a few different things with the Propeller lately, and I keep finding myself re-implementing the Servo32 object to get either higher output rates (multi-copters often go up to 400Hz) or higher precision - 1000 steps is good, but digital servos will often do 4000.
Doing either of these usually sacrifices the number of servos that can be controlled, but I thought it should be possible to write one driver that does it all, so I did.
The attached Servo32-HighRes driver:
- produces 32 simultaneous outputs
- allows the user to define the output rate - tested at up to 500Hz
- allows per-pin configuration of output rate - Slow (normal 50Hz output) or Fast (user defined rate)
- Each output has a precision of 0.125us (10 clocks) at 80MHz (8000 steps in the 1ms to 2ms range)
- Is safe down to a minimum pulse length of 0.44ms
- There's no "glitch zone" like there is with the Parallax driver. It works fundamentally different, so the timing is very stable..
Here's me waving an activity board past the camera. The right-most LEDs are showing a 50Hz output rate, while the remaining 6 are at 400Hz.
The code works by sorting the array of servo pulse lengths and pins in order of pulse length. Then it merges entries in the array whose lengths are the same, OR'ing the pin assignments together.
Then it just does a series of waitcnt and ANDN operations to wait for each pin set to expire and shut them off. Sorting and compacting the array takes a maximum of ~35000 clocks when using all 32 servos, and that work is hidden in the initial pulse wait - Servos usually have a minimum pulse length of 1ms, so I turn everything on, then do the sorting work during that initial delay.
I'd be curious to hear feedback on the code - It's the first low-level driver I've written from scratch in a while, so I'd be interested to hear if anyone has faster or more compact ways of doing some of the self-modifying bits, or just comments on general readability.
It's now running my Sideway scooter, so it's had real-world testing.
It's worth mentioning that the values you pass it are different than the normal Servo32 object because of the higher precision. This driver treats 12000 as the center, with 8000 to 16000 as the normal range (1ms to 2ms). There's also a SetRC function that takes signed numbers from -4000 to +4000.
Hope someone gets some use out of it.
Edit - I've updated this to V1.1, made the worst case significantly faster, and reduced the code footprint.
I've been implementing a few different things with the Propeller lately, and I keep finding myself re-implementing the Servo32 object to get either higher output rates (multi-copters often go up to 400Hz) or higher precision - 1000 steps is good, but digital servos will often do 4000.
Doing either of these usually sacrifices the number of servos that can be controlled, but I thought it should be possible to write one driver that does it all, so I did.
The attached Servo32-HighRes driver:
- produces 32 simultaneous outputs
- allows the user to define the output rate - tested at up to 500Hz
- allows per-pin configuration of output rate - Slow (normal 50Hz output) or Fast (user defined rate)
- Each output has a precision of 0.125us (10 clocks) at 80MHz (8000 steps in the 1ms to 2ms range)
- Is safe down to a minimum pulse length of 0.44ms
- There's no "glitch zone" like there is with the Parallax driver. It works fundamentally different, so the timing is very stable..
Here's me waving an activity board past the camera. The right-most LEDs are showing a 50Hz output rate, while the remaining 6 are at 400Hz.
The code works by sorting the array of servo pulse lengths and pins in order of pulse length. Then it merges entries in the array whose lengths are the same, OR'ing the pin assignments together.
So this: Becomes this: 00001 1500 00100 1200 00010 1500 00011 1500 00100 1200 11000 1700 01000 1700 10000 1700
Then it just does a series of waitcnt and ANDN operations to wait for each pin set to expire and shut them off. Sorting and compacting the array takes a maximum of ~35000 clocks when using all 32 servos, and that work is hidden in the initial pulse wait - Servos usually have a minimum pulse length of 1ms, so I turn everything on, then do the sorting work during that initial delay.
I'd be curious to hear feedback on the code - It's the first low-level driver I've written from scratch in a while, so I'd be interested to hear if anyone has faster or more compact ways of doing some of the self-modifying bits, or just comments on general readability.
It's now running my Sideway scooter, so it's had real-world testing.
It's worth mentioning that the values you pass it are different than the normal Servo32 object because of the higher precision. This driver treats 12000 as the center, with 8000 to 16000 as the normal range (1ms to 2ms). There's also a SetRC function that takes signed numbers from -4000 to +4000.
Hope someone gets some use out of it.
Edit - I've updated this to V1.1, made the worst case significantly faster, and reduced the code footprint.
1933 x 1471 - 587K
Comments
I can see that adjacent pulses need care to be >= 0.125us spaced, which does tend to yield a LSB of the same amount, but outside of that caveat, you may be able to get 12.5ns granularity of edge definition ?
- Not sure what cost that would have to your Sorting and compacting ~32000 clocks ?
Thinking some more about this, you could have one 'higher precision/priority' channel, that gets first rights to an edge, right down to 12.5ns, and other channels have to snap-behind that
A bit off-topic: I decided to try and look up a full definition for "servo" ... I'm not finding satisfactory results. They're all too vague. The best I think I get is a mechanism that uses feedback to control mechanical motion.
But even that is fuzzy. Does that cover, say, pumped water flow in a pipe. And I'm not talking about the pump speed here either, the pump can have it's own servo but the flow of the water can be metered and an additional outer servo loop applied to that as well.
And then one could add another loop above that to regulate flow, say, seasonally too. This control loop could have multiple feedbacks on various environmental inputs. Does this still count as another servo?
I guess there never will be a hard and fast answer. A servo is whatever one wants it to be.
"Motor with feedback control" might not be too bad a definition. That eliminates all the outer loop possibilities but should it? There is often multiple layered loops involved even in a single motor.
It's common on the industrial servos (CNC equipment) to have three loops from current that operates in the low MHz to velocity operating at high kHz to position operating from a few kHz to a few tens of KHz all inside the one box with a profile generator live feeding the positional setpoint at maybe even lower rates.
I considered that, but that would mean either my code or the user code would be responsible for making sure no adjacent timings were < 0.125us, and that isn't trivial to do. There's also not going to be many (any?) devices out there that would make use of that precision, so I opted for simple.
My ultimate use case is a multi-rotor with both ESCs and servos (some high-speed digital and some analog), probably 16 to 20 outputs needed in all. I wanted a driver that would handle all of it, and it didn't exist. It does now.
Jason
That's a great piece of work. Love the sort optimization idea.
I have to wonder about the crazy speed and resolution requirements though. I can't imagine any part of a quad copter or model aircraft that can respond at 400Hz or notice much a change of 1/4 of a thousandth in it's control command.
Still, if you can do it, why not?
Good Idea!
I plan to, but I figured a review and some testing would be good first.
Larger quads won't notice it, for sure, but I certainly feel a difference between 50hz and 200hz, even on an Elev-8 sized quad. A small quad (200mm range) might actually be able to use it because the masses involved are so much lower - things happen much faster at that scale.
So long as it still fits inside the Time and code ceilings, it is an improvement that remains available.
I agree not simple analog designs, but the slave unit could be using (say) another Prop and a stepper motor, for a camera pan and tilt system, and that can resolve to 1 SysCLK LSB to give 16.87 bits of precision over the simple, common interface.
Anyone can learn a lot about the Propeller and how to optimize code by studying your projects. You seem to have a gift for modifying code to perform better. I am still learning from your other projects.
This one may actually be an easier entry point, as it doesn't deal with the exotic math choices involved in rotation and direction calculations. It just demonstrates how to get more out of 32 servos and a Propeller.
It certainly makes the case for the Propeller with 8 CPUs over using interrupts and a single CPU. Peak performance with readable code.