Propeller controlling 16 Servos using one COG

Andrew E MileskiAndrew E Mileski Posts: 77
edited August 2015 in Propeller 1 Vote Up0Vote Down
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.


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)
 

Comments

  • 28 Comments sorted by Date Added Votes
  • Have you looked at the Servo32 object in the OBEX?  It can handle up to 32 servos (or 28 if you want to reserve pins 28-31 for their boot functions) with a pulse width resolution of 1us using only one cog.  It was the basis for the Parallax Servo Controller which would control 16 servos each and could be cascaded for a total of 32 servos controlled by a serial input.
    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.
  • And there isn't 31 bits of precision, there are only 200000 clock cycles in 2.5ms, which is 17.5 bits
  • Andrew E MileskiAndrew E Mileski Posts: 77
    edited July 2015 Vote Up0Vote Down
    See, I knew somebody must have done it before!  Thanks for the pointer!

    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.)
  • JonnyMacJonnyMac Posts: 5,739
    edited July 2015 Vote Up0Vote Down
    My own driver (Spin only, running in its own cog) has fractional microsecond resolution as well and only updates one servo at a time (two if using the 16-servo variant). I divide the 20ms update window into eight slots of 2.5ms (I allow pulses from 0.6 to 2.4ms). At the beginning of each slot I program a counter to create a pulse on the current servo pin, and while that's running I do the math for servo ramping if that has been enabled for the servo.
    Jon McPhalen
      *It's "Jon" or "JonnyMac" -- please don't call me "Jonny"
  • Andrew E MileskiAndrew E Mileski Posts: 77
    edited July 2015 Vote Up0Vote Down
    JohnnyMac,

    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.
  • Andrew E MileskiAndrew E Mileski Posts: 77
    edited July 2015 Vote Up0Vote Down
    Mark_T,

    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.
  • Andrew,
    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.
  • JonnyMacJonnyMac Posts: 5,739
    edited August 2015 Vote Up0Vote Down
    JohnnyMac,

    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  
                                                                                                                                        
    Jon McPhalen
      *It's "Jon" or "JonnyMac" -- please don't call me "Jonny"
  • jmgjmg Posts: 10,345
    edited August 2015 Vote Up0Vote Down
    (Sorry, I haven't yet figured out nice formatting in these fouled-up newly-improved forums....)

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

    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
    
  • SapphireSapphire Posts: 470
    edited August 2015 Vote Up0Vote Down
    Or use Phil's code box:


    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
    
    Sapphire
  • Duane Degn,

    Holy freakin' .... I am unworthy!

    Where's the Delete Thread button?

    /me is banished to the shadows once more


  • Andrew, There's no such thing as too many servo objects.
    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.
  • I just posted an object that might be of interest to some of you:

    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.
  • Has anyone dealt with servo ballistics? (movement smoothing)

    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.
  • You might want to look at SmoothStep. It's similar in shape, but will evaluate a lot faster than Sin.

    https://en.wikipedia.org/wiki/Smoothstep
  • Heater.Heater. Posts: 19,528
    edited August 2015 Vote Up0Vote Down
    Andrew E Mileski,

    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!






  • Andrew E MileskiAndrew E Mileski Posts: 77
    edited August 2015 Vote Up0Vote Down
    Acceleration starts slow, and peaks mid-transit, then decelerates to a stop at the new position. This produces a more natural movement, also compensates for inertia.

    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 998067
    
  • Heater.Heater. Posts: 19,528
    edited August 2015 Vote Up0Vote Down
    OK, I'm totally with you removing the "whiplash" from the movements.

    I 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.
    679 x 615 - 22K
  • Andrew E MileskiAndrew E Mileski Posts: 77
    edited August 2015 Vote Up0Vote Down
    I should have thought of plotting them. Thanks for the effort.

    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.
    640 x 480 - 4K
  • There was some discussion about non-constant acceleration in this thread.

    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.
  • Heater.Heater. Posts: 19,528
    edited August 2015 Vote Up0Vote Down
    Andrew E Mileski,
    The velocity is symmetrical even though the distance is not (counter-intuitive).
    You are right there. That does seem a bit odd.

    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.
    820 x 613 - 25K
  • Of course when taking the integral of sin(x) squared the first step is to reduce it to the integral of (1 - cos(2x)) / 2 from which we get x/2 - sin(2x) / 4 plus a constant.
    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.
  • There is something I'm not getting about this interpolation idea. Surely if you do that same interpolation scaled for different distances of travel in a single movement you have a few issues:

    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?









  • Heater. wrote: »
    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 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.

    Heater. wrote: »
    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.

    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.

    ConstantAccelerationEquations150827.PNG

    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.
  • Heater.Heater. Posts: 19,528
    edited August 2015 Vote Up0Vote Down
    I imagine you are right, with hobby servos and small travels it probably does not make a lot of difference.

    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.
  • Heater. wrote: »
    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 sen 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.

  • Duane: How many servos can you run now, Bro?

    "When you make a thing, a thing that is new, it is so complicated making it that it is bound to be ugly. But those that make it after you, they don’t have to worry about making it. And they can make it pretty, and so everybody can like it when others make it after you."

    - Pablo Picasso
  • Great new Pac Man wall!
    Infernal Machine
Sign In or Register to comment.