Forum Rule: Always post complete source code & details to reproduce any issue!
Results 1 to 8 of 8

Thread: got the Teensy 4.1! Can I drive servos at 125 Hz?

  1. #1
    Senior Member
    Join Date
    Dec 2016
    Buena Park ca

    got the Teensy 4.1! Can I drive servos at 125 Hz?

    Using right now the ol <Servo.h> library and servos are ran at 50hz.
    But new fancy servos will also work at 125hz making them more responsive. Can we do that?
    I plan to use 5 servos at the same time.

    The pins assigned are:
    Ft_R.attach(37); //foot tilt Right
    Ft_L.attach(36); //index
    Ant_R.attach(23); //antenna Right
    Ant_L.attach(22); //Antenna Left

    Second question:
    Am I restricted to PWM pins for output?

    I pulled my hair for about an hour to discover why pin 40 and 41 would not work...



  2. #2
    Senior Member PaulS's Avatar
    Join Date
    Apr 2015
    You may want to have a look at this library: TeensyStep.
    Forum member Luni will definitely step in if you have further questions.


    [edit] sorry for missuggesting: TeensyStep is not yet implemented for Teensy 4.x
    Last edited by PaulS; 01-10-2021 at 07:05 PM.

  3. #3
    Senior Member
    Join Date
    Dec 2016
    Buena Park ca
    Te above library applies to steppers not hobby servos.

    Servo control is achieved by sending a servo a PWM (pulse-width modulation) signal, a series of repeating pulses of variable width where either the width of the pulse (most common modern hobby servos) o determines the position to be achieved by the servo.

    a width of 1500 micro seconds = 90 degrees on the servo arm for example.
    Steppers don't work like that.

  4. #4
    Senior Member+ KurtE's Avatar
    Join Date
    Jan 2014
    There are probably many different ways to answer this question.

    can a T4.1 run 5 servos at 125hz. The answer is probably yes, not sure to what end or what means. That is I have my doubts that most hobby servos will change much of anything 125 times per second...

    There are also several different Servo libraries out there, example with Teensyduino there is: the Servo library and then there is the PWMServo library.

    Servo Library
    It has been a LONG long time since I have done much with RC servos. But I remember correctly, the servo library services one servo per time period, that is it startups up the first servos pulse, when that pulse time ends it sets the pin low and sets the next servos pin high...

    So how many cycles you can run per second, may depend on how many servos it is configured for.
    That is if you allow for the full 3000us (or 3ms) per servo pulse times 12 servos (how many servos the library configures per timer),
    each cycle can take up to 12*3 or 36ms you end up at about 28 per second.
    I believe the library assumes max pulse of 2400 so again that only gets you up to about 35 cycles per second.
    Now it may assume that not all will be max as it is configured for about 50 cycles per second.

    I have played with different strategies for servo libraries in the past, where instead of doing one after another, you instead you do groups of servos that either all start at a specific time and then schedule interrupts and the like to turn them off at appropriate times or the other way of start them up at an appropriate time and they all turn off at the same time.

    Some hardware I have used in past like SSC-32 (atmega board), had an 8 pin output mux on 4 different hardware timer controlled pin, where you used hardware timers to turn on and off each pin using built in timer... Again which you could do something like that with Teensy.

    PWMServo library
    It uses analogWrite to do the timing.

    Note: it sets the analogWriteFrequency to 50 on those pins and it's math to calculate the duty value, Not sure how much of the math would need to change. Also as you raise the frequency it will probably reduce the resolution. That is how many distinct values you may be able to compute. Again I have not gone through it in a long time. Probably last time I played with it was during T4 beta.

    Good luck.


    Side note - these days I mostly use Dynamixel and now playing with some new Lynxmotion Servos (LSS)

  5. #5
    Senior Member
    Join Date
    Dec 2016
    Buena Park ca
    I will work on it and let you know. Thanks

  6. #6
    Senior Member PaulStoffregen's Avatar
    Join Date
    Nov 2012
    I'd recommend using PWMServo, and of course use only the PWM capable pins.

    To use 125 Hz, you'll probably need 2 edits. First, the easy one. Find this line:
            analogWriteFrequency(pin, 50);
    and change to this:
            analogWriteFrequency(pin, 125);
    Now, the more complicated part. Find this code which computes "duty". Look for this:
            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);
    This is actually 2 implementations, one using only integers and the other commented out for floating point.

    Since Teensy 4.1 has a FPU, there's no need to use the more complicated integer code. I'd recommend you comment it out. The simpler float code has "20000.0f", which is the period of 50 Hz. So I believe all you should need to make this work correctly is editing that number, like this:
            //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 / 8000.0f * 4096.0f);
    You'll also see a commented Serial.printf() which shows all the info. Maybe uncomment that and confirm the numbers are as you expected.

    I'd also suggest checking the specs for min and max pulse width for the motors you're using when they run at 125 Hz input rate. If you're calling PWMServo begin() without the optional 2nd and 3rd inputs for min and max, make sure your motor is close to the defaults the library offers. Those optional inputs exist so you can best tune the library to meet the specs of whatever motor you're using.

  7. #7
    I am just using analogWrite(6, ServoPWM);

    It takes a bit experimenting to find the proper ServoPWM range.

    the servo seems to work fine 400 Hz, so it is set up

    analogWriteFrequency(6, 400);
    digitalWrite(6, LOW);
    pinMode(6, OUTPUT);

    I have used this directly to control the servo and on this one modified so that ServoPWM controls speed, i.e the potentiometer is removed from the axis and there is a different encoder for reading the position.

  8. #8
    Senior Member
    Join Date
    Dec 2016
    Buena Park ca
    Great, thanks everyone,
    I got some other bugs at the moment, but I will try this soon and let you know.

Posting Permissions

  • You may not post new threads
  • You may not post replies
  • You may not post attachments
  • You may not edit your posts