Propeller controlling 16 Servos using one COG
Andrew E Mileski
Posts: 77
I'm not sure if this is of interest to anyone, or even if it has already been done by others, but I thought I would share anyway.
I was inspired by a YouTube video: Arduino UNO controlling 20 Servos with 15 bit precision and low jitter.
My servo controller allows for 0.6 ms to 2.4 ms position pulses, to allow all servos to reach their end-points (each channel has its own configurable limits, with default from 1.0 ms to 2.0 ms). Also, 16 is a nicer power of two that covers half of the Propeller's I/O pins, and still leaves ample time for processing overhead. Precision is limited by the clock crystal, as it is in steps of one clock cycle.
You might be thinking, "So what! 16 servos is less than 20 servos!"
Unlike the Arduino UNO, the Propeller has 8 processing cores (COGs), so in theory one could easily control 32 servos, but that isn't really practical. A more realistic count would be 28 servos (P28 through P31 reserved), using only two COGS!
Now I have a problem: I don't have 16 or more servos to test all at the same time! I admit I've only tested with two servos so far *gulp*. I'd appreciate others giving me some feedback, especially those with more servos and oscilloscopes!
Attached is v1.2 of ServoX16.spin and the following is a trivial demo program that should move all servos at the same time every two seconds. Please note that FOR SIMPLICITY this demo specifies positions using the less-precise microsecond method rather than the max-precision cycles method.
I was inspired by a YouTube video: Arduino UNO controlling 20 Servos with 15 bit precision and low jitter.
My servo controller allows for 0.6 ms to 2.4 ms position pulses, to allow all servos to reach their end-points (each channel has its own configurable limits, with default from 1.0 ms to 2.0 ms). Also, 16 is a nicer power of two that covers half of the Propeller's I/O pins, and still leaves ample time for processing overhead. Precision is limited by the clock crystal, as it is in steps of one clock cycle.
You might be thinking, "So what! 16 servos is less than 20 servos!"
Unlike the Arduino UNO, the Propeller has 8 processing cores (COGs), so in theory one could easily control 32 servos, but that isn't really practical. A more realistic count would be 28 servos (P28 through P31 reserved), using only two COGS!
Now I have a problem: I don't have 16 or more servos to test all at the same time! I admit I've only tested with two servos so far *gulp*. I'd appreciate others giving me some feedback, especially those with more servos and oscilloscopes!
Attached is v1.2 of ServoX16.spin and the following is a trivial demo program that should move all servos at the same time every two seconds. Please note that FOR SIMPLICITY this demo specifies positions using the less-precise microsecond method rather than the max-precision cycles method.
CON _CLKMODE = XTAL1 | PLL16X _XINFREQ = 5_000_000
OBJ servo : "ServoX16"
PUB Main servo.Start(0) ' Lowest pin is P0 servo.Enable(-1, 0) ' Enable all channels repeat servo.SetPWMicro(-1, 1_000) ' 1.0 milliseconds waitcnt(CLKFREQ * 2 + CNT) servo.SetPWMicro(-1, 1_250) ' 1.25 milliseconds waitcnt(CLKFREQ * 2 + CNT) servo.SetPWMicro(-1, 1_500) ' 1.5 milliseconds waitcnt(CLKFREQ * 2 + CNT) servo.SetPWMicro(-1, 1_750) ' 1.75 milliseconds waitcnt(CLKFREQ * 2 + CNT) servo.SetPWMicro(-1, 2_000) ' 2.0 milliseconds waitcnt(CLKFREQ * 2 + CNT)
spin
35K
Comments
One problem you can get into with a driver like yours is that, if you attempt to move all the servos at the same time, you'll get a huge spike in servo motor current and that may cause all sort of noise and reset glitches. The Servo32 object gets around this by dividing the servos into groups of 8 so that the maximum start current draw is only for 8 servos. The next group of 8 is delayed by about 5ms. The comments at the beginning of the Servo32 source discusses the details.
Servos in general are fairly sloppy in terms of control. 1us resolution for the control pulses is much finer than you'll be able to get from the mechanics or servo mechanism.
I hadn't consider the power issue, though I was just looking at supporting opto-couplers (easy).
I would think that it would be up to the code commanding the servos to not move them all at once. My ServoX16.spin could be wrapped with an object that did that if desired. EDIT: Actually, I could probably just add a move-delay after say every four channels are updated.
I knew about the resolution issue. I just wanted to have a similar title and boast the Propeller's 32 bit architecture. Some people don't know better, and just look at the advertising ;-)
"I want more gee-bees!" (Warning: contains strong language.)
That's exactly what my ServoX16.spin is doing (documented in the source), though admittedly not all in SPIN.
Going down to 0.6 ms or even one cycle isn't a problem, and 2.4 ms should be do-able. I only limited to 0.8 to 2.2 ms because I'm not aware of servos that need anything different.
It depends on your crystal of course, but you are right.
It only refers to the 31 bits of the counters.
I'll change the title to avoid confusion.
It's hard to believe someone may not have seen it but it appears you might have missed the many times I've posted this video to the forum.
Here's the forum thread.
http://forums.parallax.com/discussion/137597/quickstart-driving-32-servos-video/p1
As Mike Green pointed out 1us is really beyond the precision of most servos' mechanics. I found when I centered the servos of , I had a hard time seeing differences smaller than 10us.
Speaking of my hexapod, who only uses 16 servos? Come on, you need at least 18. Preferably 22.
Apparently with some latches, it's possible to drive 144 servos with a Prop. Sounds like a good subject of a future video.
That's exactly what my ServoX16.spin is doing (documented in the source), though admittedly not all in SPIN.
Going down to 0.6 ms or even one cycle isn't a problem, and 2.4 ms should be do-able. I only limited to 0.8 to 2.2 ms because I'm not aware of servos that need anything different.
I was under the impression that the 180 degree range for Hitec servos was 600 to 2400 microseconds; this is why I allow it. As I stated, I run my pulse code in Spin because the counter actually generates the pulse output, hence there is no penatly, and while the pulse slot is active I can do the math for ramping to the servo's target postion. This is from my 8-channel servo driver (which I find is plenty for my projects).
(Sorry, I haven't yet figured out nice formatting in these fouled-up newly-improved forums....)
pri servo8(count, base) | slot, idx, chmask ' launch with cognew '' Runs up to 8 servos '' -- count is number of servos (1 to 8) '' -- base is LSB pin of contiguous group '' -- runs in own cog; uses ctra count := 1 #> count <# 8 ' keep legal
dira := (-1 >> (32-count)) << base ' make servo pins outputs frqa := 1 ' preset for counter phsa := 0 slot := cnt ' start slot timing repeat repeat idx from 0 to 7 ' run 8 slots (20ms) chmask := 1 << idx if ((idx < count) and (chmask & enablemask)) ' active and enabled channel? ctra := (%00100 << 26) | (base + idx) ' PWM/NCO mode on servo pin phsa := -(pos[idx] * us001) ' set pulse timing if (pos[idx] < target[idx]) ' update for speed pos[idx] := (pos[idx] + delta[idx]) <# target[idx] elseif (pos[idx] > target[idx]) pos[idx] := (pos[idx] - delta[idx]) #> target[idx] waitcnt(slot += (2_500 * us001)) ' let slot finish ctra := 0 ' release ctra from pin
Until they FIX that, you can use pre /pre, in HTML mode, thus
- better, but the forum still gives a moronic emoticon when text was intended.,..
Holy freakin' .... I am unworthy!
Where's the Delete Thread button?
/me is banished to the shadows once more
It's always fun to see how different people solve problems. I'm very glad you posted your code.
I just didn't want to to be impressed by an . . . Arduino.
People have done some amazing things with AVR chips. Not long ago @Martin_H posted a link to a hexapod called Stubby.
I'm sure a Propeller can do everything the AVR chip is doing but I haven't been able to replicate all of Stubby's movements yet.
Paul K's Propeller controlled hex does a good job showing what the Propeller can do.
I had wanted to come up with my own code, so I didn't use Paul's code. I think I ought to cry uncle and use Paul's code to improve mine.
I post the 32-servo demo every chance I get so I was surprised someone had escaped seeing it.
http://forums.parallax.com/discussion/161898/high-speed-high-precision-32-output-servo-driver
It uses a single cog, no counters or anything, has 32 outputs with 10 cycle (1/8uS resolution), at up to 500Hz (user selectable). It'll handle pulse lengths from 0.44ms up to however long your update rate is. (at 50hz, the longest pulse could be 20ms). It doesn't have ramping, but I may try to add that in a future version.
I'm currently using a sine-squared velocity formula, so I'm working with the integral of that:
v(t) = sin² t
d(t) = t/2 - 1/4 sin 2t
To make things simple, d(t) is divided by pi / 2, for a range from 0.0 to 1.0, resulting in a static multiplier table.
Example: Angle multiplier in 16 steps
Step: Multiplier, Change
00: 0.000000, 0.000000
01: 0.001933, 0.001933
02: 0.015058, 0.013126
03: 0.048635, 0.033576
04: 0.108384, 0.059749
05: 0.195501, 0.087118
06: 0.306451, 0.110950
07: 0.433576, 0.127125
08: 0.566424, 0.132847 Maximum
09: 0.693549, 0.127125
10: 0.804499, 0.110950
11: 0.891616, 0.087118
12: 0.951365, 0.059749
13: 0.984942, 0.033576
14: 0.998067, 0.013126
15: 1.000000, 0.001933
I've also been using a fixed 0.33 sec / 60° (or 0.5 sec for 90°) as a gross estimate of servo speed under load.
https://en.wikipedia.org/wiki/Smoothstep
I'm not sure of your requirements for servo ballistics or motion smoothing.
I can imagine rocking a servo backwards and forwards with a sinusiodal acceleration. Of course if you only want to move from position A to position B smoothly only a part of the sinusoid would be used.
I'm not sure why you are getting in to sine squared. It all reduces down to sin and cos in the end:
sin²(x) = (1/2) - cos(2x) / 2
You just have to take care of offsetting and scaling things. Also the differential of sin(x) is cos(x). Differential of cos(x) is -sin(x). Nice and easy. Position is sin, velocity is cos, acceleration is -sin
I'm scratching my head a bit about this sinusoidal approach for a couple of reasons:
1) Surely it implies that moving from A to B always takes the same time no matter what the distance between A and B is? That is in the nature of sinusoidal oscillators.
2) For large movements we might be commanding velocities to the servo that are greater than it can achieve. At which point the servo no longer cares about our nice smooth curve but will run as fast as possible to meet the end point, then slam it's brakes on !
Perhaps I'm not looking at this problem correctly.
A look up table is nice and fast but may take too much space. We can generate the required sinusoid on the fly may be smaller and offers flexibility. We can do this with simple addition, subtraction and multiplication. Or even using shifts instead of multiplies!
Edit: What am I thinking, the Prop has trig tables built into it's ROM!
Imagine moving your head from max left position to max right position at max-speed: whiplash!
The method I'm using is better than just using a sine. The integral of sin² is smoother.
I'm not calculating sine, or using floats, as that's slow. I have a fixed pre-calculated table of integers.
The following is off-the-top-of-my-head, so I don't guarantee accuracy:
position := 1_000 target := 2_000 PUB SmoothMove(_channel, _target) | position, change, last_position, next_position, stp last_position := position := GetPWMicro(_channel) change := _target - position repeat stp from 0 to 13 next_position := position + change * table.LONG[stp] / 1_000_000 servo.SetPWMicro(_channel, next_position) servo.WaitMove(last_position, next_position) last_position := next_position servo.SetPWMicro(_channel, _target) servo.WaitMove(last_position, _target)DAT table long 001933 long 015058 long 048635 long 108384 long 195501 long 306451 long 433576 long 566424 long 693549 long 804499 long 891616 long 951365 long 984942 long 998067I made the wrong assumption that the integral of sin² was much the same smoothness as cos (taking appropriate parts of the curves). Turns out you are right sin² is a gentler ride.
The integral of sin² is x/2 - 1/4 * sin(2*x) + C
Plotting them out I see what you mean. Seems to be a little loss in symmetry though but that is no worry.
Now compare the velocity curves (differentiate the distance integrals). The velocity is symmetrical even though the distance is not (counter-intuitive).
Notice that the sine² velocity has a more gentle initial and final slope (slower initial acceleration and final deceleration), but has an overall much steeper slope (overall greater acceleration / deceleration).
Tip: "noreverse" keeps gnuplot from getting inventive and flipping ranges on you.
forums.parallax.com/discussion/comment/1200573/#Comment_1200573
In the same thread as the 32 servo demo has a couple experiments I is with what I called "pseudo sinusoidal" but I don't think it was "pseudo" it was sinusoidal.
forums.parallax.com/discussion/comment/1071858/#Comment_1071858
In one algorithm I used the sine to compute the acceleration and in the other algorithm I squared the time component to generate the motion. In hindsight, I think the two algorithms produced the same motion, I was using different equations to arrive at the same results.
Looking at your plots of velocity curves I see why we disagree a bit here. We are looking at the same thing a bit differently. You have plotted sin(x)**2 and sin(x). One does not have to think about sin(x)**2 because sin(x)**2 and sin(x) are the same shape. Just tweak the scale, phase, and offset and you have the same thing. Or use cos(x) if you like. See attached plot of sin(x)**2 and (sin(2*x - pi / 2) / 2) + 0.5. Or use -(cos(2*x) / 2) + 0.5.
Of course we want to integrate that to get the distance covered for use in commanding the servo. Perhaps thinking of the integral of sin(x)**2 is easier than thinking of the integral of -(cos(2*x) / 2) + 0.5 but it should all come out the same.
Thanks for the "noreverse" tip. I don't use gnuplot often and had never seen it do that, it was puzzling me.
As seen here: http://www.ditutor.com/integrals/integral_sin_squared.html. That intermediate "cos" step is what I plotted above.
So we are indeed talking about the same thing. One does not need sin(x)**2 and it easier not to start there.
1) For large travels the speed commanded at the mid point may exceed the maximum speed of your servo. The servo will lag behind and then run at it's maximum speed until it catches up with the last commanded position. The smoothing is lost and whiplash occurs. One can of course scale the curve so that the maximum velocity is never exceeded in the intended use case. But that leads to...
2) For small travels one is using less acceleration and slower speed. You are taking longer than necessary.
Surely ideally one would want a nice smooth acceleration and deceleration at the start and end of the travel and a straight line in the middle at the maximum speed.
I imagine a constant acceleration until reaching the the mid point of travel or maximum speed would be desired at start up. Then on reaching the mid point position do all that in reverse. Probably a bit tricky to calculate on the fly.
Or is it so that we ignore those issues as it works well enough anyway?
I agree. I think there are applications where one would want to ramp the acceleration but I'd be very surprised if projects which use hobby servos would benefit from this sort of motion. IMO, it's really hard to see the difference between constant acceleration and ramped acceleration.
It's hard to tell from the videos, but my servo monstrosity project did this.
forums.parallax.com/discussion/155937/3-axis-magnetometer-with-3-axis-gimbals-experiment
I used the equations for constant acceleration to compute the servo's acceleration, speed and position at 50Hz based on feedback from the magnetometer.
While the calculations aren't trivial, neither are they very difficult. I'm pretty sure it can all be done with integer math. I cheated and used floats in my program. If I were to do this again (and I probably will do something similar), I'd use integer math for all the calculations.
My acceleration calculations appeared to have worked correctly, unfortunately mounting the magnetometer so close to the servos seriously interfered with the sensors ability to determine its orientation.
I modified the contraption by adding a three inch support for the sensor in order to move the sensor away from the magnetic fields of the servos. This modified version worked much better but I didn't take the time to make a video of it in action.
I'd really like to repeat the experiment with an IMU.
Your constant acceleration scheme is kind of what I had in mind. It occurred to be that it would be nice to have positional feed back from the servo so that you know when to put the brakes on, as it were. I see you have that with a magnetometer, neat.
Sounds like you need some Bosch BNO055 9 axis IMU's with built in sensor fusion. https://www.adafruit.com/products/2472 I'm sure I have seen break out boards for them a lot cheaper elsewhere.
I have one of those. That's what I'm hoping to try with my servo contraption.