analogWriteFrequency issue

Status
Not open for further replies.

cchouard

Member
Hello,
On a Teensy 3.2, I am trying to use the internal hardware timers to drive the stepper motors of a self balancing 2-wheel robot.
I need to update the speed of the stepper motors quite frequently by executing an analogWriteFrequency function every 10ms (at 100Hz). I am indifferent to the duty cycle, so I put it at 50% with a digitalWrite(pin,128) function.
My problem is that analogWriteFrequency seems to reset timers : the output pin goes to low each time the function is called. Therefore, the frequency of steps received by the stepper motors is affected by the call to the analogWriteFrequency function, especially at frequencies below 100hz.

Is there a way to update the frequency without changing the state (high or low) of the output pin ? Alternatively, is there a way to choose the output pin state (high or low) at the beginning of the new cycle triggered by the call to the analogWriteFrequency function ?
 
Hello Paul,
Thank you, but this library handles moves rather than speed. Moreover, I wish the processor to be free to do plenty of other things while the motors are moving, hence the need to use internal timers.
 
analogWriteFrequency() stops the timer to change frequency. You might be able to hack your own frequency change, see
https://forum.pjrc.com/threads/43828-Change-between-2-PWM-frequencies-Teensy-LC

Hello Manitou, I had seen that post, thank you, but I must confess that my hacking capabilities are weak. I need to change frequency without stopping the timer. When I read at this sentence from 36.4.10.2 : «*If the selected mode is not CPWM then MOD register is updated after MOD register was written and the FTM counter changes from MOD to CNTIN*», I think that it should be possible to do so.
What do you think ?
 
Easiest would be to use one of the 4 interval timers for that. Changing the reload value does not restart the timer. There are plenty of examples on using intervaltimer on the forum. Works of course only if you don't need them otherwise.
 
Here a quick snippet showing how to use the IntervalTimer to generate a variable frequency signal without stopping the timer when changing frequencies.

Code:
IntervalTimer timer;

void step()
{
    digitalWriteFast(LED_BUILTIN, !digitalReadFast(LED_BUILTIN));  // generate the pulses (50% duty cycle)
}

void setup()
{
    Serial.begin(0);
    pinMode(LED_BUILTIN, OUTPUT);

    timer.begin(step, 5E4);             // start with a reload value of 50000
}

elapsedMillis stopwatch = 0;

void loop()
{
    if (stopwatch > 1500)               // keep the set frequency for some time
    {
        stopwatch = 0;
        timer.update(random(5E4, 1E6)); // random reload values from 50000 to 1000000 
    }
    // doSomething()
}
 
Luni, thank you very much for your help.
I had seen interval timers briefly, but I rejected them because they are software timers driven by interrupts.
I like hardware timers because they work on a standalone basis, and the only thing the main software has to bother with is to update its frequency and duty cycle from time to time.
The drawback of analogWriteFrequency is that the timer output pin goes to LOW when the function is called.
Then the pin behaves the way it should, namely it goes to HIGH as the counter starts counting, then goes to LOW at the end of the first phase of the duty cycle, then goes to HIGH when the total time period (= 1 / frequency) has ellapsed.
This means that if the output pin is HIGH when the analogWriteFrequency function is called, it will go LOW for a fraction of a second.
I am in the process of engineering my tailormade function by the name of analogWriteFrequencySweet with an objective of avoiding any glitch when called. I will post it when I am happy with it.
 
The FTM timers can be programmed to not reset the counter when you change the reload value (see manitous link). If you really want to go that way be prepared to spend some time reading and understanding the FTM chapters of the datasheet, but getting it going can be a lot of fun of course :)

If you want to use the "hardware toggling" just because it seems to be more efficient I recommend to quickly estimate the additional processor load generated by the code from above.

The setting of the reload value will be the same for the FTM and the PIT timers. The only thing which is additionally done in software is the call of the interrupt function "step()". The rest is done by hardware. For a Teensy 3.2 running at 96MHz I estimate that the pin toggling including the call to the ISR will be done in less than 0.5µs. If you need to generate step pulses with a frequency of say 10kHz the processor must call the ISR every 50µs. That would generate a processor load of 0.5µs / 50µs = 0.01 = 1%. Hardly worth the work with the FTM's (unless you do it for the fun of it).

Just my thoughts of course!
 
Last edited:
Hello Luni,
I see your point.
The reasons why I want to avoid interrupts are :
- for fun (i agree with you, it is fun !)
- I am unconfortable with interrupts : handling them may sometimes be tricky, and I do not precisely know how they cope with I2C or other serial communications. I can see warnings everywhere that ISRs must be as quick as possible.
- I am using an IMU (MPU 6050) with DMP, and the DMP generates interrupts each time (100 times per second) new accelerometer and gyroscope data is available. I want to give utmost priority to it.
 
I have written my own function by the name of changeFrequency as an alternative to the analogWriteFrequency function.

The problem with the analogWriteFrequency function is that its call triggers a "glitch", i.e. a LOW shortly followed by a HIGH (HIGH being the beginning of the duty cycle) on the output pin. This may be a problem for those applications where the function needs to be called frequently to refresh the frequency, for example when the pin is used to generate step pulses for stepper motor drivers when updating their desired speed: it may not be desirable that a step is generated each time the function is called.
The reason why a glitch is generated upon each function call is that the function resets the FTM (Flex Timer Module page 769) counter (FTMx_SC = 0; FTMx_CNT = 0; with x being either equal to 0,1 or 2 on a Teensy 3.2). We need a new changeFrequency function which does not reset the counter each time, but just updates the frequency of the output pin, so that its call does not trigger a LOW followed immediately by a HIGH on the pin.

There are 4 important registers (or parameters) to take into account:
- FTMx_SC_CLKS (page 781), aka clock source : either 1 = F_TIMER (36mHz on a Teensy 3.2, for frequencies above 4.3Hz) or 2 = 31.25kHz, i.e. 1152 times slower (the external 16mHz oscillator divided by 512, for frequencies below 4.29Hz).
- FTMx_SC_PS (page 781), aka prescale factor, or prescale, a 3 bit number which drives the number by which the clock source frequency is divided: divider = 2^prescale, with 0<=prescale<=7 and therefore 1<=divider<=128)
- FTMx_MOD (page 782), aka MOD for modulo, which is the value between 0 and 65535 up to which the FTM counter counts from CNTIN, namely zero, after which the counter drops to zero and counts up again (FTM counts up to MOD+1).
- FTMx_CnV (page 785), aka cval, with the channel number n being equal to 0 or 1 (or more on FTM0 on Teensy 3.2). Cval is the counter value below which the output pin is HIGH and above which the output pin is LOW (in order to drive the PWM). Cval must be greater than zero and smaller than MOD. If cval is larger than MOD, then the output pin is always HIGH, it stalls, there is no longer any frequency ! (duty cycle is exactly 100%). This cval value is neither updated by the analogWriteFrequency function nor by my new changeFrequency function, but by the analogWrite function.

Frequency is updated by updating these registers. But it is important to note that each of them will not be taken into account as soon as registers are written into: clock source, prescale and cval are taken into account immediately, or upon the next clock cycle (example page 781: The new prescaler factor affects the clock source on the next system clock cycle after the new value is updated into the register bits), whereas MOD is only taken into account when the counter reaches the previous MOD value (page 847: MOD register is updated after MOD register was written and the FTM counter changes from MOD to CNTIN, namely 0).
Therefore, updating the parameters may slow down considerably, or even stall, the output pin before the new frequency is taken into account. This is in particular the case when the new frequency is lower than the previous one in such a way that it increases the prescale and/or or decreases the clock source frequency. This is because it would take longer for the counter to reach its previous MOD value. Depending on the application, this may or may not be a serious issue. For example, if all frequencies are above 4.3Hz, the clock source is always 36mHz, and the worst that could happen is an interim period that would be twice as long as the previous one. If frequencies can be both above and below 4.3Hz, then the counter speed may change dramatically (by a factor at least 1152 = 36mHz/31.25KHz) and that may be an issue depending on the current MOD value. The output pin may stall depending on the previous duty cycle, namely on the previous cval value. If duty cycle is not an issue (it is not an issue in applications driving stepper motor drivers), then it is recommended to apply a low duty cycle, namely have a low cval, so that the new MOD cannot be below the old cval.

An then it is always possible to force a FTM counter reset in situations where sharp frequency changes may lead to unwanted results such as output pin temporary freeze. In my changeFrequency function, I have decided to reset FTM if frequency increases dramatically in such a way that prescale or clocksource decreases. But this can of course be adapted to cope with specific situations.

The table below shows which frequency ranges are handled by each combination of prescale and clock source:

Teensy_Frequency_Prescale_MOD_clockSource.jpg

As with the original analogWriteFrequency function, it is recommended to update the duty cycle just after the function is called.
First, the usual analogWriteFrequency function must be called at least once in order to initialize FTM properly.
analogWriteFrequency(pin,frequency); // counter initialized properly
analogWrite(pin, 1); // low duty cycle

Then:
changeFrequency(pin, frequency); // change frequency without resetting the counter
analogWrite(pin, 1); // low duty cycle

Code:
void changeFrequency(uint8_t pin, float frequency) {
// Modification of the original analogWriteFrequency function.
// This function does not reset counter systemically
// in order to avoid glitches in the output pin each time it is called

    uint32_t prescale, mod, ftmClock, ftmClockSource;
    float minfreq;

    if (frequency < (float)(F_TIMER >> 7) / 65536.0f) {
        // frequency is too low for working with F_TIMER:
        ftmClockSource = 2;   // Use alternative 31250Hz clock source
        ftmClock = 31250;     // Set variable for the actual timer clock frequency
    } else {
        ftmClockSource = 1;   // Use default F_TIMER clock source
        ftmClock = F_TIMER;   // Set variable for the actual timer clock frequency
    }
    
    for (prescale = 0; prescale < 7; prescale++) {
        minfreq = (float)(ftmClock >> prescale) / 65536.0f; 
        if (frequency >= minfreq) break;  //prescale is set to the lowest possible prescale value for maximum precision
    }

    mod = (float)(ftmClock >> prescale) / frequency - 0.5f;  // counter will count from zero to MOD+1
    if (mod > 65535) mod = 65535;


if (pin == 3) {   // this is FTM1 on Teensy 3.2
// alternatively, if(pin == FTM1_CH0_PIN || pin == FTM1_CH1_PIN) {

// old prescale is FTM1_SC & (uint32_t)(7)
// old ftmClockSource is (FTM1_SC>>3)&(uint32_t(3))
// old MOD is FTM1_MOD

 if (prescale<(FTM1_SC & (uint32_t)(7))||ftmClockSource<((FTM1_SC>>3)&(uint32_t(3))))  {
// FTM is reset if frequency is increased sharply
            FTM1_SC = 0;
            FTM1_CNT = 0;
        }
        FTM1_MOD = mod; // if FTM had not been reset, MOD actually updated once counter has reached previous MOD
        FTM1_SC = FTM_SC_CLKS(ftmClockSource) | FTM_SC_PS(prescale);  // updating both clocksource and prescale immediately
        FTM1_MOD = mod; // updating MOD again (recommended in certain situations)
    } 

    
if (pin == 25) {  // this is FTM2 on Teensy 3.2
// alternatively, if(pin == FTM2_CH0_PIN || pin == FTM2_CH1_PIN) {

if (prescale<(FTM2_SC & (uint32_t)(7))||ftmClockSource<((FTM2_SC>>3)&(uint32_t(3)))){
            FTM2_SC = 0;
            FTM2_CNT = 0;
        }
        FTM2_MOD = mod;
        FTM2_SC = FTM_SC_CLKS(ftmClockSource) | FTM_SC_PS(prescale);  //Use ftmClockSource instead of 1
        FTM2_MOD = mod;
    }

}
 
Status
Not open for further replies.
Back
Top