Multiple interval timers with encoders T4

[AlborzJ]

Hi all,
I am using rosserial on a teensy 4.0 and need to do a few things: control 3 stepper motors in closed loop feedback for position setting, read imu data, and maybe a control servo. What I did so far was use an IntervalTimer to create the PID loops for the stepper motors, but I am also using Paul’s encoder library for counting ticks of the 3 Quadrature encoders, which I read each time in the ISR’s and send out a pwm to the stepper motor controllers. I also have an interval timer for the IMU which reads data over i2c. The problem I have is in my main loop I eventually have to disable interrupts and copy the global variable data of the IMU to publish over serial as well as take commands given over serial for the pid setpoints. During this interrupt disable phase, I lose ticks and the stepper motor position control drifts off over time and becomes inaccurate. I am considering using the hardware encoder library I found someone had made for the teensy 4.0. Would this ensure no ticks are missed when I disable interrupts? Also, would I benefit by using something like the metro library for reading the imu data (at like 200hz) instead of intervaltimers ?

[defragster]

The T_4.0 with a free running [not blocked] loop() cycles very fast - it starts at ~11 million cycles per second before adding code - but can drop to just a couple million quickly. There is a possible improvement coming to reduce the overhead limiting it to that - but until then in the sketch placing ::

void yield() {}

will reduce some overhead if no serialEvent#() code is used. That link is where those numbers come from.

So with perhaps millisecond cycling in loop() - if the _isr just set a flag - the IMU data could be read polling that flag in loop() and processed without having to disable the interrupts? Reading the IMU data with i2c isn't really a fast and ideal thing to do in an _isr() anyhow.

There are other possibilities - if some simple repro was shown.

[MorganS]

Hi all,
During this interrupt disable phase, I lose ticks and the stepper motor position control drifts off over time and becomes inaccurate.

You must be doing something wrong. Can you show your code?

Interrupts should only be disabled for a few lines of your main loop. Just long enough to grab copies of the volatile variables, optionally resetting them back to zero/false to show that you've "answered" that value. Copying a 32-bit variable is only one instruction on the Teensy 4. That's one six-hundred-millionth of a second. You would have to win the lottery a few times to beat those odds.

I haven't dug into the T4 datasheet too deeply, but assuming it works like every other microcontroller, the interrupts are queued. A pin interrupt from an encoder arrives at a random time but you can be pretty sure that it occurs at a time that the processor is already processing an instruction. It can't abort and undo an instruction already in progress. The interrupt must wait for that instruction to finish. We think of it as diverting to the interrupt instantaneously because a millionth of a second seems instantaneous to a human although it's like a few minutes to a Teensy.

Additionally, even if you disable interrupts for a period, that doesn't stop them entering the queue. They can wait there until you re-enable interrupts. The only way that you can "lose ticks" is when the queue is full and it has to stop new ones entering the queue. On the basic Arduinos, the queue can only hold one of each type of interrupt. So if you disable the interrupt processing for long enough to get TWO clicks on your encoder, one will be lost. For a knob turned by a human, you have to disable the interrupts for an exceptionally long time like a hundred milliseconds to cause it to lose ticks. The T4 is a lot more complex, with interrupt priorities, but you can rest assured that the designers of the chip had a pretty good idea of the things that you are trying to do, so they built the chip to do exactly what you want.

Interference on the encoder wires may also be a problem. A small capacitor like 22pF or 0.1uF can help with some cases of interference. (Note that 1 uF is a million times bigger than 1 pF, so this is a really big range I've given you.)

[jwatte]

During this interrupt disable phase, I lose ticks and the stepper motor position

This means that you're doing too much with interrupts disabled. Copying a few global variables shouldn't take any appreciable time. However, if you try to copy large strings, or worse, do malloc() or use C++ objects with fancy constructors/destructors, you're doing it wrong.

A mechanism I find highly useful is to have two copies of my state, and build the "next state" in the various code, and then when disabling interrupts, just swap the "current state" and "next state" pointers. The rest of the stuff can be done outside the critical section.

[defragster]

What are the interrupt priority levels of the encoder versus the IntervalTimer? If the encoder is the same or lesser - the encoder will also have interrupts queued {like p#4} and not be able to act on them - as if interrupts are disabled while i2c data transfers.

That goes back to prior post #2 - don't do i2c transfers in the _isr() - as it seems is suggested?

In theory __STREXW offers a way to coordinate across interrupts {repeats a code segment on any interrupt} - but that depends on unseen code - it is in use in micros() on T_4.x {two millis() time vars are re-read on any system interrupt before used} - but that use is read only. Any time it needs to write in that time it can still be a rabbit hole as repeating multiple reads until atomic for use is okay, but repeated writes or changes can cause odd things. Just tried it on SD logger lib with _isr() data logging and it seems to yet have some unexpected side effect. { but hard to detect with 120K _isr()/sec to fill a buffer it can take writing GB's of data 16bytes at a time before it fails ... see lottery note in p#3 }

[AlborzJ]

So I figured out the problem, but first, here is my code:

#include <ros.h>
#include <std_msgs/Float32.h>
#include <sensor_msgs/Imu.h>
#include "QuadEncoder.h"
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#define PI 3.1415926535897932384626433832795

/**********Function Prototypes**************/
 void posCb(const std_msgs::Float32& msg);
 void PID();
 void getimu();
 /***********************************/
 
 /************VARIABLES********************/
IntervalTimer steering1Control;
IntervalTimer imuUpdate;
QuadEncoder steering1(1,2,3,0);
ros::NodeHandle nh;
std_msgs::Float32 stepperAngle;
sensor_msgs::Imu imu_dat;
ros::Publisher jointstate("steeringPos", &stepperAngle);
ros::Publisher imupub("/imu/data", &imu_dat);
ros::Subscriber<std_msgs::Float32> angle("SteeringAngle", &posCb);
volatile short int sp = 0;
volatile short int error = 0;
Adafruit_BNO055 bno = Adafruit_BNO055(55,0x28);
volatile sensors_event_t orientationData , angVelocityData , linearAccelData;
volatile sensors_event_t event; 
/************************************************/
void setup()
{
  if(!bno.begin())
  {
   //if not detected, infinite loop
    while(1);
  }
  steering1.setInitConfig();
  //steering1.EncConfig.INDEXTriggerMode = RISING_EDGE;
  steering1.init();
  pinMode(10, OUTPUT);
  pinMode(11, OUTPUT);
  //nh.getHardware()->setBaud(256000);
  nh.initNode();
  nh.advertise(jointstate); //publisher for current angle
  nh.advertise(imupub);
  nh.subscribe(angle); //angle of steering command
  bno.setExtCrystalUse(true);
  imu_dat.header.frame_id = "imu";
  steering1Control.begin(PID,5000);//200 hz timer interrupt loop
  //imuUpdate.begin(getimu,10000);//100hz timer interrupt
  
}

void loop()
{    
    
    stepperAngle.data = steering1.read();
    noInterrupts();
    imu_dat.orientation.x = event.orientation.x;
    imu_dat.orientation.y = event.orientation.y;
    imu_dat.orientation.z = event.orientation.z;
    interrupts();
    jointstate.publish(&stepperAngle);//current positions
    imu_dat.header.stamp = nh.now();
    imupub.publish(&imu_dat);
    nh.spinOnce();
    delay(4);
   
}

void posCb(const std_msgs::Float32& msg){//callback for 250hz subscriber
  noInterrupts();
  sp = map(msg.data,-PI/2,PI/2,-1000,1000);
  interrupts();
}

void PID(){//ISR for PID loop for position of stepper motor
  /*
  if(abs(steering1.read())>1100){
    analogWrite(11, 0);
    return;
  }*/
    error=sp-steering1.read();
    if(error>=0){
      digitalWrite(10, HIGH);
    }else{
      digitalWrite(10, LOW);
      error=error*(-1);
    }
    if(error*100>30000){
      analogWriteFrequency(11, 30000);
    }else{
      analogWriteFrequency(11, error*100);
    }
    analogWrite(11, 200); 
}

void getimu(){
    bno.getEvent(&event);
    bno.getEvent(&angVelocityData, Adafruit_BNO055::VECTOR_GYROSCOPE);
    bno.getEvent(&linearAccelData, Adafruit_BNO055::VECTOR_LINEARACCEL);
  
}

The issue is the stepper motor encoder signals I am getting. I tried this same code with another motor and it doesnt miss ticks. The stepper motor i am using provides differential encoder signals for the a and b channels. I have hooked up 2 wires from the a+ and b+ channel of the stepper motor driver (its a closed loop driver) to the teensys pins as well as ground. I am not sure why this wont produce reliable data. I tried seperating the wires even more but the results are the same. Is there a way to hook up the differential signals for data? or a converter that is to be used?

[defragster]

That encoder sounds like a different problem to solve - good luck.

If the loop() were changed to remove the delay(4) and instead use an elapsedMillis Foo to: if ( Foo>4000) {Foo=0; { rest of the current loop() code } }

Then have the _isr getimu() {Bar++;} and in loop() add :: if (oldBar != Bar) { oldBar=Bar; {read imu event like getimu() does} } - no need to disable interrupts. As long as loop() keeps running at 1000Hz all data will be gathered - if not then the loop() wouldn't read it anyhow before it was updated again.