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

Thread: Multiple interval timers with encoders T4

  1. #1
    Junior Member
    Join Date
    May 2020
    Posts
    2

    Multiple interval timers with encoders T4

    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 ?

  2. #2
    Senior Member+ defragster's Avatar
    Join Date
    Feb 2015
    Posts
    11,571
    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 ::
    Code:
    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.

  3. #3
    Member MorganS's Avatar
    Join Date
    Apr 2015
    Location
    Northern Nevada
    Posts
    49
    Quote Originally Posted by AlborzJ View Post
    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.)

  4. #4
    Senior Member
    Join Date
    Dec 2014
    Posts
    304
    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.

  5. #5
    Senior Member+ defragster's Avatar
    Join Date
    Feb 2015
    Posts
    11,571
    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 }

  6. #6
    Junior Member
    Join Date
    May 2020
    Posts
    2
    So I figured out the problem, but first, here is my code:
    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?

  7. #7
    Senior Member+ defragster's Avatar
    Join Date
    Feb 2015
    Posts
    11,571
    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.

Tags for this Thread

Posting Permissions

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