Properly handling timer overflow ?

Post here all questions related to LibMaple core if you can't find a relevant section!
Post Reply
Koep
Posts: 10
Joined: Sat Sep 12, 2020 10:24 am

Properly handling timer overflow ?

Post by Koep »

Hi everyone!

I am/have been building a small router with dc servomotors based on blue pills and cheap h-bridges,
using arduino 1.8.18 and Roger's core
I already have excessive hair thinning caused by head scratching trying to find the cause for infrequent movement errors.

Last week I finally noticed that I have removed most electrical/RFI/EMI causes thus
leaving what has probably been the major cause whole time.

Something in my timer overflow isr is wrong but I'm somehow blind as I can't pinpoint the cause
I added debounce to my PID routine but to no avail

My somewhat poorly commented/structured code below

Would be grateful of any tips

Rgds
Koep

Code: Select all

volatile long turns = 0; //this can count the number of turns the encoder gives.
volatile long steps = 0; //this can count the number of turns the encoder gives.
volatile double error = 0; //error of speed = set_speed - pv_speed
volatile double error_pre = 0;  //last error of speed
volatile double error_sum = 0;  //sum error of speed
volatile double Output = 0;  //sum error of speed
unsigned long tprint = 0;
int EncDebounce = 0;



HardwareTimer Power = HardwareTimer(1);
HardwareTimer enc = HardwareTimer(2);
HardwareTimer PidTime = HardwareTimer(3);
HardwareTimer pulse = HardwareTimer(4);

void countTurns() {
  PidTime.pause();
  if (enc.getDirection())
    turns -= 65449;
    //turns -= settings.PPR;
  //turns ++;
  else
    turns += 65449;
    //turns += settings.PPR;
  //turns--;
  PidTime.resume();
}

void countSteps() {
  PidTime.pause();
  if (pulse.getDirection())
    steps -= 65029;
   // steps -= (settings.PPR - 3333);
  //turns ++;
  else
    steps += 65029;
    //steps -= (settings.PPR - 3333);
  //turns--;
  PidTime.resume();
}

void DoPid() {
  error = (double)(steps + pulse.getCount() - (turns + enc.getCount()));
  if (Lost){
  }else if (error < -settings.MaxErr || error > settings.MaxErr){
   EncDebounce++;
   if (EncDebounce > 900) Lost = true;  
  }else{  
  error_sum += (error * settings.Ki * settings.Gain); //sum of error
  if (error_sum > settings.ZeroS + settings.MaxKi) error_sum = settings.ZeroS + settings.MaxKi;
  if (error_sum < -settings.ZeroS - settings.MaxKi) error_sum = -settings.ZeroS - settings.MaxKi;
  Output = (error * settings.Kp * settings.Gain);
  Output += error_sum - (((error - error_pre) * settings.Kd) * settings.Gain);
  //max Ki and output;
  if (Output > (settings.ZeroS-300)) Output = (settings.ZeroS-300);
  if (Output < (-settings.ZeroS+300)) Output = (-settings.ZeroS+300);
  error_pre = error;  //save last (previous) error

  //pwmWrite(PWM_OUT, (long)settings.ZeroS+Output);
  bitSet(TIMER1->regs.adv->CR1, 1); // update disable
  if (Output < 0) {
    TIMER1->regs.adv->CCR1 = (int)((settings.ZeroS - settings.FFw) + Output);
  } else if (Output > 0) {
    TIMER1->regs.adv->CCR1 = (int)((settings.ZeroS + settings.FFw) + Output);
  } else {
    TIMER1->regs.adv->CCR1 = (int)(settings.ZeroS + Output);
  }
  bitClear(TIMER1->regs.adv->CR1, 1); // update enable
  EncDebounce = 0;
}
}
SerialCommand sCmd;     // The demo SerialCommand object

void setup() {

  Power.pause();
  Power.setPrescaleFactor(1);
  //Power.setOverflow(((long)settings.ZeroS) * 2); //12,5kHz
  Power.setOverflow(((long)settings.ZeroS + settings.FFw + 80) * 2); //12,5kHz
  Power.refresh();
  Power.resume();

  pinMode(PWM_OUT, PWM); //PA8
  pinMode(PWM_OUT_COMP, PWM);//PB13

  bitClear(TIMER1->regs.adv->BDTR, 8); //
  bitClear(TIMER1->regs.adv->BDTR, 9); //
  bitClear(TIMER1->regs.adv->CCER, 0); //this should enable complimentary outputs
  bitClear(TIMER1->regs.adv->CCER, 2);
  bitSet(TIMER1->regs.adv->BDTR, 0); //dead time 1*125ns
  //bitSet(TIMER1->regs.adv->BDTR, 1); //dead time 1*125ns
  //bitSet(TIMER1->regs.adv->BDTR, 2); //dead time 2*125ns = tot 5*125ns
  //bitSet(TIMER1->regs.adv->BDTR, 3); //dead time 1*125ns

  afio_cfg_debug_ports(AFIO_DEBUG_NONE); // remap ENCODER pins TIM2
  afio_remap(AFIO_REMAP_TIM2_PARTIAL_1);

  enc.pause(); //stop...
  enc.setMode(1, TIMER_ENCODER); //set mode, the channel is not used when in this mode.
  enc.setOverflow(65449);    //use this to match the number of pulse per revolution of the encoder. Most industrial use 1024 single channel steps.
  //enc.setEdgeCounting(TIMER_SMCR_SMS_ENCODER3);
  enc.refresh();
  enc.attachInterrupt(0, countTurns); //channel doesn't mean much here either.
  enc.resume();                 //start the encoder...
  
  bitSet(TIMER2->regs.adv->CCMR1, 4);
  bitSet(TIMER2->regs.adv->CCMR1, 5);
  bitSet(TIMER2->regs.adv->CCMR1, 6);
  bitSet(TIMER2->regs.adv->CCMR1, 7);
  bitSet(TIMER2->regs.adv->CCMR1, 12);
  bitSet(TIMER2->regs.adv->CCMR1, 13);
  bitSet(TIMER2->regs.adv->CCMR1, 14);
  bitSet(TIMER2->regs.adv->CCMR1, 15);
/* */
  pulse.pause(); //stop...
  pulse.setMode(2, TIMER_ENCODER); //set mode, the channel is not used when in this mode.
  pulse.setOverflow(65029);  //use this to match the number of pulse per revolution of the encoder. Most industrial use 1024 single channel steps.
  pulse.setEdgeCounting(TIMER_SMCR_SMS_ENCODER1); //or TIMER_SMCR_SMS_ENCODER1 or TIMER_SMCR_SMS_ENCODER2. This uses both channels to count and ascertain direction.
  pulse.refresh();
  pulse.attachInterrupt(0, countSteps); //channel doesn't mean much here either.
  pulse.resume();                 //start the encoder...
  //bitClear(TIMER4->regs.adv->CR1, 4);
  
  bitSet(TIMER4->regs.adv->CCMR1, 4);
  bitSet(TIMER4->regs.adv->CCMR1, 5);
  bitSet(TIMER4->regs.adv->CCMR1, 6);
  bitSet(TIMER4->regs.adv->CCMR1, 7);
  bitSet(TIMER4->regs.adv->CCMR1, 12);
  bitSet(TIMER4->regs.adv->CCMR1, 13);
  bitSet(TIMER4->regs.adv->CCMR1, 14);
  bitSet(TIMER4->regs.adv->CCMR1, 15);
/* */
  PidTime.pause();
  PidTime.setPrescaleFactor(1);
  PidTime.setOverflow(3049);
  //PidTime.setOverflow(3000);
  PidTime.setMode(TIMER_CH1, TIMER_OUTPUTCOMPARE);
  PidTime.setCompare(TIMER_CH1, 1);
  PidTime.attachInterrupt(TIMER_CH1,  DoPid);
  PidTime.refresh();
  PidTime.resume();

ag123
Posts: 1653
Joined: Thu Dec 19, 2019 5:30 am
Answers: 24

Re: Properly handling timer overflow ?

Post by ag123 »

assuming that you are using stm32f103.

I think in stm32f103 the timers are 16 bits. but that it has a prescaler which is 16 bits

I've not reviewed the codes really. But that keep rm0008 handy as it tells you what is ultimately available in the hardware.
https://www.st.com/resource/en/referenc ... ronics.pdf

if you are referring to setOverflow() function, I think that can more appropriately be deemed as the period, e.g. that 65535 (0xffff) is probably the longest period on the clocks. if you need to 'extend' that beyond 65535, i'd guess one way is to configure it to 65535 cycles for the period and handle the rest of the counting in your ISR (e.g. your attachInterrupt() callback, but that means in 'attachinterrupt' you are literally counting the number of full 65535 cycles each time the interrupt triggers.

however, if your confusion is about setOverflow() itself, the simplest way is to think of that as the number of ( pheriperial clocks / prescaler ) cycles that is counted.

this 'overflow' value is in the ARR (auto reload register), so that it counts the number of ( pheriperial clocks / prescaler ) cycles before resetting and triggering the 'update' interrupt for the next period. for the exact/precise definition review rm0008 again.
dannyf
Posts: 446
Joined: Sat Jul 04, 2020 7:46 pm

Re: Properly handling timer overflow ?

Post by dannyf »

My somewhat poorly commented/structured code below
Impossible to follow.

I think you are trying to configure TIM1 to over flow at 8049, and then set output compare to trigger at 1 - not sure why.

maybe you can help more by stating what you are trying to do - invoking DoPid() periodically?

your code could use some modularity. and definitely comments.
Koep
Posts: 10
Joined: Sat Sep 12, 2020 10:24 am

Re: Properly handling timer overflow ?

Post by Koep »

Hi and thank you for taking time to reply.

Following is what I at least think I'm doing:

Timer1 runs PWM in complementary mode with dead time enabled.
Timer1 configured to overflow at 5560 for app. 13kHz PWM freq.

Timer2 (enc) is hardware quadrature encoder connected to HEDS optical encoder (TIMER_SMCR_SMS_ENCODER3)
Timer4 (pulse) is hardware encoder in pulse+dir mode connected to step/dir output (TIMER_SMCR_SMS_ENCODER1)

Timer3 (PidTime) is set to overflow at 3049 to periodically run doPid() function ie PID loop.
The //PidTime.setOverflow(3000) is commented out.

The somewhat odd looking overflow values for timers 2,3 and 4 are an attempt to avoid timers overflowing at the same time.

After posting I came across this viewtopic.php?t=1080
which to my understanding is the root cause of my problem. My case is just 32/16bit instead of 64/32.

It also looks that it is more likely to cause error when counting downwards

I hope the above makes better sense

Rgds,
Tapio
dannyf
Posts: 446
Joined: Sat Jul 04, 2020 7:46 pm

Re: Properly handling timer overflow ?

Post by dannyf »

maybe you can simplify it so you can isolate the problem.

1) write a piece of code that generates the pwm on TIM1:

Code: Select all

//initialize pwm on TIM1, with user-specified period
void pwmInit(uint16_t pr) {
  ...
}
2) write code for your encoders:

Code: Select all

//initialize optical encoder on TIM2
//I actually don't know why you need to specify the overflow here - setting the prescaler makes more sense to me but that could just be my ignorance
void enc2Init(uint16_t pr) {
  ...
}

//initialize encoder on TIM4
//I actually don't know why you need to specify the overflow here - setting the prescaler makes more sense to me but that could just be my ignorance
void enc4Init(uint16_t pr) {
  ...
}

3. then write your pid code - all this does is to periodically call the pid routine

Code: Select all

//set up pid routine
void pidInit(uint16_t pr, void (*isrptr)(void)) {
  //set up the timer with pr;
  ...
  //install the isrptr
  ___.attachInterrupt(TIMER_CH1,  isrptr);
}
you can then test the code individually.

for the pid routine, you can start by simply flipping a pin and you can measure that pin's frequency to see if the code is executing correctly.

After that, put in the real pid routines.

the issue here is likely your time constant: with everything else going on, and the use of floating point math, you just don't have enough time go execute your isr/DoPid() routine.

again, start simple by testing individual pieces of code first, make sure that they work before integrating them.
which to my understanding is the root cause of my problem.
unlikely.
dannyf
Posts: 446
Joined: Sat Jul 04, 2020 7:46 pm

Re: Properly handling timer overflow ?

Post by dannyf »

the way to integrate them together is simple:

Code: Select all

//I just used some random numbers
#define PR_PWM		10000	//pwm period - you can tie it to F_CPU to make the pwm frequency invariant to the chip's speed
#define PR_ENC2		2000		//encoder 2 frequency / period
#define PR_ENC4		4000		//encoder 4 frequency / period
#define PR_PID			10000	//pid period - again, tie it to F_CPU to make it invariabnt

//set up my application
void appInit(void) {
  pwmInit(PR_PWM);				//initialize the pwm
  enc2Init(PR_ENC2);				//initialize enc2
  enc4Init(PR_ENC4);				//initialize enc4
  pidInit(PR_PID, DoPid);			//initialize pid and install user pid routine as isr
  //...
}
/code]

with this approach, if you want to change the frequencies, you can simply change the PR_xyz macros.
Koep
Posts: 10
Joined: Sat Sep 12, 2020 10:24 am

Re: Properly handling timer overflow ? - SOLVED

Post by Koep »

Hello everyone!

This indeed turned out to be this exact issue viewtopic.php?t=1080

I removed the overflow interrupts from timers 2 and 4 (ie encoder and pulse counter)
and moved the checking of overflow to the beginning of the pid loop.

All errors are now gone :D

Rgds,
Tapio
Post Reply

Return to “General discussion”