Welcome to the Parallax Discussion Forums, sign-up to participate.

# QuickStart Driving 32 Servos (Video)

Posts: 10,002
edited 2015-07-21 - 16:33:46
We all knew the Propeller could do it, but I hadn't seen anyone actually drive 32 servos with one Propeller chip.

I received a box of servos in the mail Saturday so I thought I'd give good ol' Servo32v7 a workout.

I've noticed nothing brings out a stammer like making a YouTube video. I find I have a hard time talking while keeping the scene framed in the viewfinder. It's a good thing I didn't try to chew gum at the same time.

My aim was to show each servo being controlled independently from the others. I used an oscillating movement with different servos being out of phase with each other. This "out of phase" aspect shows up as a rippling wave motion (I think it also allows one to walk through walls).

While I have these servos attached to these boards, I'd like to make a better demonstration video that shows off what the Propeller can do. I'm hoping for suggestions of what you all would like to see 32 servos be able to do.

Some things I know I can do with these servos are linear oscillating motion with individually settable end points, period of oscillation, the phase within the oscillation,

I also use an algorithm I call "pseudosinusoidal" to give the servo a bouncing motion where it travels slow near the end points while speeding up through the center area. I'd like to come up with some better sinusoidal motion algorithms that can be computed for 32 servos within the servos' 20ms refresh time.

One such motion I'd like to reproduce is that of a pendulum. I'd be happy with the simplified version of pendulum motion. The equation I'd like to translate to fast Spin code is:

Angle as function of time = theta * cos(squareroot(g/l) * t)
theta is the angle at the pendulum's extreme.
g is acceleration from gravity.
l is the length of the pendulum.
t is time.

Assuming values of g and l aren't going to change, I could pre-compute squareroot(g/l) and use it as a constant. I'd still need to compute a cosine for each servo. Does anyone have a suggestion on the fastest way of computing cosines? I've always used F32 for trig functions but I have a feeling this will likely be too slow for this purpose.

I'll look through the various math objects and time the cosine functions I find. I'm just wondering if someone here as already made these comparisons.

Any other thoughts on what would make a good servo demo?

I figure I should have some sort of follow the leader where all the servos copy the movement of the servo next to it with a bit of time lag to show they aren't moving in unison.

What else should a Propeller servo demo do?

• Posts: 10,002
edited 2012-01-30 - 07:25:33
Edit: The attached code has a severe limit. See post #8 for description of problem.

I'm attaching the code used in this demo. I have not cleaned it up at all! There are large sections commented out. There's a method "DebugLoop" that uses SimpleSerial to display snap shots of servo settings. SimpleSerial doesn't use a separate cog so it is limited to relatively low baud rates. I used 9600bps when I was debugging the code. I had to comment out the cognew statement launching the debug method since I was using pins 30 and 31 with servos.

Besides pins 30 and 31, pins 28 and 29 which are used to load the program from EEPROM need to be disconnected from servos while programming the Propeller. The EEPROM pins need to be disconnected from servos when starting or resetting the program.

I use a set of DIP switches to connected and disconnect the last four pins with the servos.

Here's a list of methods used in this program.
```PUB Setup | localIndex, t1, t2
PUB OscillateLinear(localFirstIndex, localLastIndex) | localIndex
PUB OscillateSinusoidal(localFirstIndex, localLastIndex) | localIndex, pseudoPeriod, pseudoRange, pseudoTime
PUB SetAlternating(firstPosition, secondPosition) | localIndex
PUB SetEndPointLow(localFirstIndex, localFirstEnd, localLastIndex, localLastEnd) | localIndex
PUB SetEndPointHigh(localFirstIndex, localFirstEnd, localLastIndex, localLastEnd) | localIndex
PUB SetPeriod(localFirstIndex, localFirstFrames, localLastIndex, localLastFrames) | localIndex
PUB SetPhase(localFirstIndex, localFirstPhase, localLastIndex, localLastPhase) | localIndex
PUB DebugLoop | localIndex
PUB Dec(value) | i, x
```

Thanks Beau for writting a great servo object!
```*****************************************
* Servo32v7 Driver                   v7 *
* Author: Beau Schwabe                  *
* Copyright (c) 2009 Parallax           *
*****************************************
```

I didn't use the ramping feature at all. I used to dabble in computer animation where I had virtual Lego minifigures walking around a virtual Lego scene. To create the animations, I'd have the computer compute the position of each element of the minifigure for each frame to be rendered. I used 30 frames a second for the videos so I just modified the technique to compute servo positions at 50 frames a second.

Edit(3/11/15): Warning, the code attached is an old version. There are likely better options available.
I plan to upload this program or an improved version to my GitHub account
If there isn't code similar to what is attached here on my on GitHub, send me a message and I'll make and check for any improved versions of the code.
• Posts: 10,002
edited 2012-01-30 - 07:25:55
Reserved for final version.
• Posts: 10,002
edited 2015-08-27 - 01:21:02
Here's the code I used to make the "pseudosinusoidal" motion.
```PUB OscillateSinusoidal(localFirstIndex, localLastIndex) | localIndex, pseudoPeriod, pseudoRange, pseudoTime
waitcnt(nextFrameTime)
nextFrameTime += frameInterval
repeat localIndex from 0 to _ServosInUse - 1
if ++frameCount[localIndex] => period[localIndex]
frameCount[localIndex] := 0

pseudoPeriod := (period[localIndex] * period[localIndex]) / 16  ' fourth period squared
pseudoRange := (maxPosition[localIndex] - minPosition[localIndex]) / 2
if frameCount[localIndex] > period[localIndex] / 2
if frameCount[localIndex] > (period[localIndex] * 3) / 4
pseudoTime := period[localIndex] - frameCount[localIndex]
pseudoTime *= pseudoTime
servoPosition[localIndex] := minPosition[localIndex] + ((pseudoTime * pseudoRange) / pseudoPeriod)
else
pseudoTime := frameCount[localIndex] - (period[localIndex] / 2)
pseudoTime *= pseudoTime
servoPosition[localIndex] := maxPosition[localIndex] - ((pseudoTime * pseudoRange) / pseudoPeriod)

else
if frameCount[localIndex] > period[localIndex] / 4
pseudoTime := (period[localIndex] / 2) - frameCount[localIndex]
pseudoTime *= pseudoTime
servoPosition[localIndex] := maxPosition[localIndex] - ((pseudoTime * pseudoRange) / pseudoPeriod)
else
pseudoTime := frameCount[localIndex] * frameCount[localIndex]
servoPosition[localIndex] := minPosition[localIndex] + ((pseudoTime * pseudoRange) / pseudoPeriod)
Servo.Set(servoPin[localIndex], servoPosition[localIndex])

```

Instead of being based on just the time passed as in linear motion, this uses the square of time from the end points. This gives a faster motion through the center positions of the servo with the servo slowing near the end points. The servo horn kind of looks like it's "bouncing" from one end point to the other.

I think it gives the servo motion a "smoother" look.

The code breaks the servo cycle into four parts. The clockwise and counterclockwise movements are each broken into two sections each. The sections are "toward center" and "away from center". I square the number of frames the current frame is from the closest end point to come up with its current position. This creates an acceleration toward the center position of the servo. (I kind of think it looks cool.)

I'll make a video comparing linear servo motion with this "pseudosinusoidal" motion. I think this "pseudosinusoidal" motion could give a robot a more organic looking motion.

The servo on the right is using the pseudosinusoidal algrithm and the servo on the left a linear algorithm.

• Posts: 1,008
edited 2012-01-30 - 08:08:19
What about the Sine table in ROM? Since you know what range you're working with, you should be able to use integer math just fine.
• Posts: 10,002
edited 2012-01-30 - 08:27:01
What about the Sine table in ROM? Since you know what range you're working with, you should be able to use integer math just fine.

That's what I'm thinking I'll probably end up doing. I just haven't used any of the objects that use the Sine table yet.
• Posts: 2,300
edited 2012-01-30 - 09:20:54
Hello Duane,

That is really a cool demonstration of the Propeller controlling a full set of servos at once. The video definitely helps.
• Posts: 10,002
edited 2012-01-30 - 09:56:09
Hello Duane,

That is really a cool demonstration of the Propeller controlling a full set of servos at once. The video definitely helps.

Thanks Robert.

I just noticed a major flaw in the code I posted above. As the code is now, all the servos that are moving have to use the same method to controll their movements. I was going to make a demo of linear motion side by side with my pseudosinusoidal motion but I realized I have the cycle delay inside each method so I can't call two different methods within the same cycle. This will diffinately need to be changed.
• Posts: 10,002
edited 2015-08-27 - 01:22:15
I think I've figured out how to simulate a pendulum swing with a servo.

Here's the method:
```PUB OscillatePendulum(localFirstIndex, localLastIndex) | localIndex
'' check to see if the period or length has changed.
'' a change in length will change the period but
'' a change in period will not automatically change
'' the length.
repeat localIndex from localFirstIndex to localLastIndex
if pendulumLength[localIndex] <> OldPendulumLength[localIndex]
SetPendulumLength(localIndex) ' this will also set the period
elseif period[localIndex] <> OldPeriod[localIndex]
SetMultiplierFromPeriod(localIndex)

if ++frameCount[localIndex] => period[localIndex]
frameCount[localIndex] := 0

result := F.FFloat(frameCount[localIndex])
result := F.FMul(fMultiplier[localIndex], result)
result := F.Cos(result)
result := F.FNeg(result) ' so this method has the same phase as the other oscillating methods
result := F.FMul(fHalfRange[_ServosInUse], result)

servoPosition[localIndex] := F.FRound(result)

```

It turns out the motion from this pendulum algorithm looks a lot like the motion from my earlier pseudo sinusoidal algorithm. I haven't timed the algorithms to see which method is faster.

I'm not sure if my phase change method works correctly yet. I found a big bug in it that would give bad data if you used any pin except P0 as the first pin to change its phase.

The attached code will drive 28 servos. I left the top four pins free to make it easier to reboot and debug.

Here's a video showing the three different types of oscillation. Linear, pseudo sinusoidal and pendulum.

Edit(3/11/15): Warning, the code attached is an old version. There are likely better options available.
I plan to upload this program or an improved version to my GitHub account
If there isn't code similar to what is attached here on my on GitHub, send me a message and I'll make and check for any improved versions of the code.
• Posts: 19,466
edited 2012-02-04 - 18:59:34
OK, Pal. Now you're just showing off!

Very nice demo. Of course, per Dave James, it would be more mesmerizing with 27 servos, 9 each mode!
"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
• Posts: 9
edited 2013-05-11 - 07:04:45
That is an awesome demo! Did you ever publish the circuit you are using? I'm under the impression that the propeller chip couldn't drive that much.
• Posts: 10,002
edited 2013-05-11 - 13:43:07
That is an awesome demo! Did you ever publish the circuit you are using? I'm under the impression that the propeller chip couldn't drive that much.

There's not much to the circuit. Each servo gets power and a signal line. The signal lines are connected directly to the Propeller's I/O pins. I did add DIP switches to the servo signal connected to P28-P31 to allow the Propeller to boot up properly. The Propeller has 32 I/O pins each I/O pin had a servo connected to it.

I can see in the video, I used a switching regulator to reduce the LiPo voltage down to 5V. I used a separate battery pack to power the QuickStart board (four AA NiMH IIRC). The negative leads of each battery pack were connected to create a common ground connection.

I think that's about it as far as the connections go. As you can see in the video there was a rat's nest of wires making all these connections. I had some carbon fiber rods handy and I used those to hold down the wires. You can see the dark rods between the servos and breadboard holding some of the wires down.
• Posts: 679
edited 2013-05-11 - 19:48:39
erco wrote: »
OK, Pal. Now you're just showing off!

Very nice demo. Of course, per Dave James, it would be more mesmerizing with 27 servos, 9 each mode!

+1 on the showing off.

And well done as well.