Best way of driving 18-24 servos with Teensy 3.1?

Status
Not open for further replies.

joecarb89

Member
Hi all,

I am currently working on a hexapod with 24 servos. It is a project I have been working on a little bit here and there for a few years. Originally I was going to use the Arduino Duemilanove + Pololu 24 Mini Maestro then the Arduino Mega and now the Teensy 3.1 + TLC5940 (using slow PWM mode). (I started the project as a complete newbie to electronics, now I am about to graduate from EE school :rolleyes:)

Unfortunately I found the 3.1 doesn't support the TLC5940 servo.h file and I do not have enough ARM experience to make it work.

I considered using the PulsePosition library (with a shift register) but I am not sure if it would work well for my application (I also only have 74HC595 shift registers on hand). I also considered PWMservo but I do not see references to the 3.1 and it does not compile under Arduino 1.0.6.

The adafruit Servo Driver and its library look promising and it compiles but the compatibility isn't explicitly stated in github. I just never really liked their libraries and I don't want to drop $30 + $10 in shipping (college budget..)

At this rate, I am unsure of which would be the best (and cheapest!) route to take. The CPU will be doing a lot of math so it would probably be best to go the adafruit route but I think PulsePosition would be a bit cheaper. Does anyone have a recommendation?
 
Why do you think that ?

The teensy will be performing a lot of math, I am unsure of how CPU intensive pulse position is. I also I do not have any 164 shift registers on hand but I could give it a shot though. The 164's are nonlatching I believe. Could I hold the 595's latch open and use that as a test?
 
If you are doing a lot of math, I recommend fixed point over floating point (on the teensy). It's not hard, even if you have numbers with fractional parts.
 
Oh, looks like I never ported PWMServo.

I've made a port to 32 bit Teensy just now, just for you! :) Well, also for anyone else who ever needs this...

https://github.com/PaulStoffregen/PWMServo

This new port of PWMServo should be able to work with *all* the PWM pins. Since Teensy 3.1 has 12 PWM, you can get 12 servos with this, and 12 more on the non-PWM pins with the regular Servo library.

This is brand new. I tested with an oscilloscope and verifying internal numerical results, but no actual motors yet. Please let me know how it works for you?
 
Woah, awesome support! I just did a quick test and it works! The servos bind at one extreme but I believe that will be an easy fix. I will test it with higher quality servos and see how it performs in a few hours.

I also like the idea of fixed point math, I will do some research on it tomorrow. Thanks!
 
I've updated the Servo page with info for Teensy LC & 3.x

http://www.pjrc.com/teensy/td_libs_Servo.html

The servos bind at one extreme

Try using myservo.attach(pin, 1000, 2000). Does that fix the problem?

...recommend fixed point over floating point (on the teensy).

I'd recommend nearly the opposite. Write your code the simplest, most convenient way and don't be shy about using float. These motors move so slowly relative to the speed of Teensy (even with floats), and their control pulses only communicate new position data at 20 ms intervals.

If you're concerned about CPU usage, use an elapsedMicros variable and Serial.print (or a pair of digitalWriteFast and an oscilloscope) to measure the actual time your code takes. There's little point optimizing if it's well under the 20 ms pulse interval.

If you do end up optimizing with fixed point, a known-good floating point version is incredibly handy for comparison.
 
myservo.attach(700,2400); works and gives very close to 180 degrees of movement. I am running my micro servo off the 3v output from the teensy but there is no jitter, which is very important for my application.

And thanks for the tip about the fixed point vs floating point CPU usage. I will build the robot using floating point math but still look into fixed point stuff for other projects. I tested a few parts of the code and the teensy performed way better than the Uno.

By the way, under Arduino IDE 1.0.6, there is an option to overclock the 3.1 to 96MHz. Would I run into any potential timing issues with it overclocked?
 
By the way, under Arduino IDE 1.0.6, there is an option to overclock the 3.1 to 96MHz.

Yes, it's under Tools > USB Type.

In newer versions of Arduino, this menu also gives the option have the compiler optimize for speed vs code size. On ancient Arduino 1.0.6, you're stuck with only optimizing for size, which costs some performance.

When you use the newer Arduino versions, you can also edit hardware/teensy/avr/boards.txt to uncomment faster overclocking options.
 
This seems like it might be interesting to try out. For the most part I have gotten away from using RC servos, but still have a few Lynxmotion Hexapods sitting around with them (Phoenix, T-Hex, Round 4dof ) sitting around, which in the past I have done Teensy to SSC-32 to drive them. I have the Phoenix code base which uses fixed point math to drive them.

Wish list: PWMServo - Wish it supported the writeMicroseconds/readMicroseconds. method, like the Servo library does. Maybe likewise if you do a write and the value > 180 assume microseconds (and not 180).

Actually I wish they shared a common base class. Might be fun to setup a new Teensy3Servo class that incorporates both libraries, such that if the servo is in the PWM range it uses PWM code, else it uses the servo library like code.

Maybe I will hack up a Phoenix Servo driver for something like that. (Have one for AX servos, SSC-32, Orion controller and I know some other have one for Maestro.
 
Yes, it's under Tools > USB Type.

In newer versions of Arduino, this menu also gives the option have the compiler optimize for speed vs code size. On ancient Arduino 1.0.6, you're stuck with only optimizing for size, which costs some performance.

When you use the newer Arduino versions, you can also edit hardware/teensy/avr/boards.txt to uncomment faster overclocking options.


Thanks! I just installed a separate copy of 1.6.9 (but not the Teensyduino beta yet). I get a lot of errors for my older attiny cores and a lot of warning about invalid libraries but it seems most of my Arduino stuff compiles. I will look into it some more since I just bought a 101.
 
I'm currently using a Teensy 3.1 using a serial port to control a Mini Maestro 18-Channel USB Servo Controller (from Pololu) for a quadruped robot. I'm running a lot of floating point for the inverse kinematics and just purchased a couple of the new Teensy 3.6 boards to take advantage of the hardware floating point. I'd like to use the Teensy 3.6 to control the servo's directly with the additional pins and the PWM hardware of the Kinetis K66 ARM.
Will the existing PWMServo library support that?

BTW, here's a link to blog I started on the QuadPod if you want to check it out: http://www.neo-bots.com/
 
I forgot to mention that I'd like to run the PWM outputs faster than the typical 50Hz used for most servos. I'm currently running them at 300Hz to get a smoother response.
 
Yes, PWMServo is your best option. In only works on PWM pins, so with Teensy 3.2 you can control up to 12 servos. With Teensy 3.6 you get 22 PWM pins. Hopefully that's enough?

PWMServo is written for the standard 50 Hz control. To change to 300 Hz, first find and edit this line:

Code:
        analogWriteFrequency(pin, 50);

You'll also have to edit the duty cycle computation. Look for this:

Code:
        uint32_t us = (((max16 - min16) * 46603 * angle) >> 11) + (min16 << 12); // us*256
        uint32_t duty = (us * 3355) >> 22;
        //float usec = (float)((max16 - min16)<<4) * ((float)angle / 180.0f) + (float)(min16<<4);
        //uint32_t duty = (int)(usec / 20000.0f * 4096.0f);

Since Teensy 3.5 & 3.6 have hardware floating point, the simplest thing you can do is delete the first 2 lines using interger-only math, and uncomment the much simpler floating point computation. Then edit 20000.0 (the number of microseconds for 50 Hz) to 3333.333 for your 300 Hz waveform.

You might also wish to edit the usec computation. The microseconds corresponding to 0 and 180 degrees are stored with 16 us resolution, which is done to precisely match the Arduino Servo library. My guess is that'll be pretty limiting for 300 Hz control. As long as you're editing the code, the simplest thing would be to just hard code the number of actual microseconds for min and max pulse width. Since they're float, you can use precision finer than 1 us, but of course the hardware is only capable of 20.8 ns steps on Teensy 3.2 or 16.7 ns steps on Teensy 3.6.
 
Paul, Thanks for the quick and helpful response! I'll try to test this out this weekend.
BTW, I just signed up on this forum but have been referring to it often since I discovered your great Teensy products just over a year ago. I've been very impressed by your quick and thorough responses! I hope you don't mind but I mentioned you and the Teensy 3.2 in my blog as my Arduino board of choice.
 
Paul, How do I get a 16.7 nsec step with the Teensy 3.6? If I set the analogWriteFrequency to 300 Hz, the period is 3.333 milliseconds. If that's the period and I can change the duty cycle by 1/4096 steps that seems like it gives 0.814 usec (814 nsec) steps. What am I missing?
 
OK, I just realized that the analog write resolution isn't limited to 12 bits, it can go up to 16 bits. That looks like it would give a step size of 1/65536 of the 3333 usec which is 0.051 usec (51 nsec). Is that correct?
In https://www.pjrc.com/teensy/td_pulse.html you mention that "The actual resolution available depends on the PWM frequency, where slower frequencies have higher resolution. For example, if you set the PWM frequency to 375 kHz and the resolution to 10 bits, analogWrite will automatically map the 0-1023 values to the available 0-127 range. Your code can write values from 0 to 1023, but groups of 8 consecutive values will produce the same output."
When and why does this mapping of values occur? You have a table that provides "Ideal" frequencies for different boards and clock rates but I'm not sure what that means or how to calculate those for the Teensy 3.6.

Rather than modifying the PWMServo library I think I'll modify my existing servo code (that was controlling the Pololu Maestro Servo controller) as I'm using floating point radians rather than degrees and my existing servo code also has min, mid and max microsecond values for each servo as they don't seem to have a perfectly linear response. I'll just replace the Maetro's serial commands with the appropriate analogWrites to the PWM pins once they've been set up at the correct resolution and frequency.
 
Last edited:
I've got some test code running an array of all PMW pins at 200 Hz with 16 bits of resolution and the scope shows all 22 PWM outputs working!
For some reason the PWM from digital pins 16 and 17 show a frequency of 400 Hz but the correct duty cycle.

There are a couple of other strange things I'm seeing with this setup (200 Hz with 16 bits of resolution).

1. The duty cycle I see on the scope doesn't change when I go from 0/65536 to 1/65536, nothing shows up in either case.
When changing from 1/65536 to 2/65536 the scope now shows a pulse width of about 135 nsec which is roughly what I'd expect.
When changing from 2/65536 to 3/65536 the scope still shows a pulse width of about 135 nsec which is the same as I saw for 2/65536.
At 4/65536 the scope now shows a pulse width of about 266 nsec which is roughly what I'd expect again.
In other words, it seems to be ignoring the LSB of the duty cycle value.​

2. A lot of the PWM output pins have the pulse occurring at the same time, but not all of them.
If I trigger the scope from PWM pin 2 the other pulses start at approximately the following times shown below.
I don't think it's an issue, but I'm curious why it would be.​

  • pin 02: 0 usec
  • pin 03: -27 usec
  • pin 04: -27 usec
  • pin 05: -9 usec
  • pin 06: -9 usec
  • pin 07: 0 usec
  • pin 08: 0 usec
  • pin 09: -9 usec
  • pin 10: -9 usec
  • pin 14: 0 usec
  • pin 16: -14 usec
  • pin 17: -14 usec
  • pin 20: -9 usec
  • pin 21: -9 usec
  • pin 22: -9 usec
  • pin 23: -9 usec
  • pin 29: -7 usec
  • pin 30: -7 usec
  • pin 35: 0 usec
  • pin 36: 0 usec
  • pin 37: 0 usec
  • pin 38: 0 usec
 
Last edited:
In other words, it seems to be ignoring the LSB of the duty cycle value.

Perhaps the carrier frequency you've chosen doesn't map to exactly a multiple of 1/65536th of F_BUS.

The timers are 16 bits, but not all 16 bits are truly used when the timer must be configured with a reload value that uses less than 65536 counts to achieve the carrier frequency.

In other words, if the timer is counting from 0 to 49151 to achieve your carrier frequency, the 16 bit number you give to analogWrite gets automatically scaled (by code within analogWrite) onto the 0 to 49151 range the hardware is actually using.

2. A lot of the PWM output pins have the pulse occurring at the same time, but not all of them .... but I'm curious why it would be.

The 22 PWM pins on Teensy 3.6 correspond to 5 actual hardware timers. You can expect all the pins controlled by the same timer to be in sync. But unless you've gone to extreme measures when configuring the timers (with analogWriteFrequency), the odds are very slim the independent timers running at the same frequency will be in phase sync with each other.
 
Last edited:
Thanks Paul, I now recall the write-up you have on Pulsed Output: PWM & Tone (https://www.pjrc.com/teensy/td_pulse.html) and what you said in your response above makes sense.

There's a large table near the end of that write-up with the Ideal Frequency (PWM) for various boards and clock rates (but it doesn't include the 3.5 or 3.6 yet).
Could you describe briefly in that write-up how the ideal frequency is calculated based on the CPU clock?
That way we could figure it out ourselves for various boards and CPU clock frequencies.
I don't even know how the F_BUS clock relates to the CPU clock, is it the same for every board/processor?

Assuming I can calculate the F_BUS frequency, then is the maximum PWM frequency given by F_BUS/(2^bits of resolution)?
Then can I pick any lower multiple of that maximum PWM frequency, for example: F_BUS/(2^bits of resolution) / integer_divisor as a PWM frequency that will still give me that resolution?
 
the PWMServo seems to give a very gorse control. Servo-tester I use seems to give much better control.

I have modified a servo, disconnected the potentiometer, and controlling the servo speed. this works well, but the problem is that PWMServo gives only about 6 different speed. The current consumption is measured so when the speed jumps is well visible and also from sound one can tell. I tested also using directly the PWM control analogWriteFrequency(pin, 50); and it works, but somewhat erratic and can not tel about the accuracy at this time.

Previously I have used the Pulse library for PPM with very good results (for 16 channels) but could it be used just for one servo without additional HW? Maybe sending the same value to all channels? Need to do some soldering as it does not seem to accept pin 3 where the PWM is now connected.
 
Status
Not open for further replies.
Back
Top