Teensie RTOS (Real time operational system)

Status
Not open for further replies.
For a university project I use this real time operational system developed by Fernando Trias : https://github.com/ftrias/TeensyThreads. I use two rotary encoders one two estimate bicycle velocity and one to estimate rpm of the pedal cadence sytem. For the estimation of velocity I use the pulseIn function and then multiple this value by 2 to get the period etc. It seems that the estimation of velocity and rpm seen in the second thread ( void read_data() {) gives me false values when using the RTOS. However, the estimation is fine whenever I run the code without the (RTOS).
I am struggling with this issue a couple of days so any help is more than welcome.



Below is my code:
Code:
#include <SD.h>
#include <TeensyThreads.h> // https://github.com/ftrias/TeensyThreads
#include <SPI.h>  // include the SPI library to initiate the clock signal for absolute encoders and IMU

//-----------------------------------------------Define pins needed for wheel and pedal cadence encoders------------------------------------------------------------------------------------------------------

#define rearencoderA 0 // define pin where rear wheel encoder chanel A is connected
#define pedalencoderA 2 // define pin where the pedal encoder chanel A is connected

//--------------------------------------------SD Card logging-----------------------------------------------------------------------------------------------------------------------------------
File myFile; // create a file object
const int CS =BUILTIN_SDCARD; // define CS pin of SD card

//-----------------------------------------------Define pins needed for absolude encoders------------------------------------------------------------------------------------------------------------------

const int ssimu = 10; // slave selection enable is low
const int sshand = 24; 
const int ssfork = 25; 
const int pwrencoders = 26; // making high enable power to encoders
float handlebaranglelast; // check estimation of angular velocity section

//-----------------------------------------------Define time as a variable for SD card----------------------------------------------------------------------------------------------------------------

unsigned long time;

//-----------------------------------------------Define variables needed for calculation of forward velocity and pedal cadence--------------------------------------------------------------------------------
unsigned long duration;
unsigned long duration1;
unsigned long freq;
unsigned long freq1;
unsigned long rpm;
unsigned long rpm1;
unsigned long U;
unsigned long rear;

void run_motor() { // name thread function
  
// set the slaveSelectpins and power pin of absolute encoders and IMU as outputs:
  
  pinMode (ssimu, OUTPUT);
  pinMode (sshand, OUTPUT);
  pinMode (ssfork, OUTPUT);
  pinMode (pwrencoders, OUTPUT);

  while(1) { // this  is basically the control loop of the two motors
    
    // enable slave device
    
    digitalWrite(ssimu,HIGH); // setting HIGH slave selection pin to disable IMU
    digitalWrite(sshand,LOW);  // setting LOW slave selection pin to enable handlebar encoder
    digitalWrite(pwrencoders,HIGH); // setting HIGH PA10 pin of external pcb connected to pin 26 of teensie to enable power to my encoders
    digitalWrite(ssfork,HIGH); // setting HIGH slave selection pin to disable fork encoder
    
    // set clock signal characteristics
    
    SPI.beginTransaction(SPISettings(125000, MSBFIRST, SPI_MODE3));  // set frequency to 125 Khz-4 Mhz, encoder transmit first the MSB, Clock Idles High Latch on the initial clock edge, sample on the subsequent edge
    
    uint16_t mybyte; // declaring variable to unsigned 16-bit integer
    uint16_t mybyte1; //declaring variable to unsigned 16-bit integer
    mybyte= SPI.transfer16(0); // transfering 16 bits to MOSI and reading what comes back to MISO
    
    //From my byte1 you need to mask out 3 bits the first MSB and the two LSB;
    
    mybyte1 = (mybyte &= 0x7fff)>>2; // First AND bit operator is used bit mask 0111 1111 1111 1111 the 16bit is set to zero. 
    //afterwards the bits are shifted two positions to the left. mybyte is now a 14bit with the MSB=0 and the upcoming 13 bit coming from the encoder signal.
    
    digitalWrite(sshand,HIGH); // disable handlebar encoder
    SPI.endTransaction(); // ending transaction
    delayMicroseconds(20); // delay between 12.5 μs ≤ tm ≤ 20.5 μs for SSI timeout
    
    //---------------------------------------------------Reading data from fork encoder----------------------------------------------------------------------------------------
    
    digitalWrite(ssfork,LOW); // enable fork encoder
    uint16_t mybytefork; // declaring variable to unsigned 16-bit integer
    mybytefork= SPI.transfer16(0); // transfering 16 bits to MOSI and reading what comes back to MISO
    uint16_t mybyte1fork; //declaring variable to unsigned 16-bit integer
    mybyte1fork = (mybytefork &= 0x7fff)>>2; // First AND bit operator is used bit mask 0111 1111 1111 1111 the 16bit is set to zero. 
    //afterwards the bits are shifted two positions to the left. mybyte is now a 14bit with the MSB=0 and the upcoming 13 bit coming from the encoder signal.
    digitalWrite(ssfork,HIGH); // disable fork encoder
    SPI.endTransaction(); // ending transaction
    delayMicroseconds(20); // delay between 12.5 μs ≤ tm ≤ 20.5 μs for SSI timeout
    
    // -----------------------------------------------Processing data output of encoders------------------------------------------------------------------------
    
    //Note: The two encoders are mounted oppose to each other for this reason we have different counts directions. 
    // More specific, Anticlockwise rotation of handlebar encoder gives 360 deg-> 310 deg whereas, anticlockwise rotation gives 0-55 degrees.
    // The rotational direction must be the same for both encoders, the encoders must give an output 0+- 180 degrees. Two if statements are used for these reasons.
    
    float handlebarangle;
    handlebarangle=(float)mybyte1*360.0/8191.0; // encoder counts to deg
    
    if (handlebarangle >180) // Anticlockwise rotation of handlebar encoder gives 360 deg-> 310 deg.
    handlebarangle = handlebarangle -360; //substracting -360 we get 0-180 deg anticlockwise.
    // ---------------------------------------------Set software limits------------------------------------------------------------------------------------------
    // The fork mechanical range is +- 38 degrees since handlebars do bot have a mechanical limit software limits are set below if you do not set this limits the motor foldsback.
    if (handlebarangle >38) 
    handlebarangle = 38;
    if (handlebarangle <-38) // seting angle limit
    handlebarangle = -38;
    // end of software limits
    float forkangle;
    forkangle=-(float)mybyte1fork*360.0/8191.0-41.5; //// encoder counts to deg, substracting 8 deg to align fork with handlebars and - to get minus values from fork encoder
    if (forkangle <-180) // Clockwise rotation of fork encoder gives -360 deg-> -310 deg 
    forkangle = forkangle+360; //by adding +360 we get 0-180 deg anticlockwise
    {
    }
    
    // -----------------------------------------------Fork feedback tracking controller------------------------------------------------------------------------
    
    
    //declare pin alocation on teensie 3.6 board
    const int forkmotordrive = 30; // making High enables fork motor drive
    const int pwmforkmotor = 9;
    //declare pin as outputs
    pinMode (forkmotordrive, OUTPUT);
    pinMode (pwmforkmotor, OUTPUT);
    // define variables
    float Kp= 0.5;//0.139626 ; // Nick recommends 8 Nm/rad=0.139626 Nm/deg
    float Kd=0.010472;// Nick recommends 0.6 Nm/rad=0.010472 Nm/deg
    float Tc= 0.0369;// Torque constant  36.9 mNm/A = 0.0369 Nm/A
    const int Tdac=1;
    float Torque;
    signed int Torquedac;
    
    //----------------------------------------Estimation of angular velocity---------------------------------------------------------------------------------
    float velocity;
    velocity=(handlebaranglelast-handlebarangle)/0.000352; // where dt=352 (us)// it is two fast i need to add a counter and a while loop and print at lower frequencies
    if (handlebarangle!=handlebaranglelast)
    {
    handlebaranglelast=handlebarangle;
    }
    //--------------------------------------------------------------------calculation of Torque fork motor--------------------------------------------------------
    Torque=Kp*(-handlebarangle+forkangle);
    Torquedac=(Torque*1334+32768)+412; // divide with reduction ratio* torque constant to give a command in Amperes
    // Scale torque amp with a constant to obtain a dac number
    if (Torquedac>65536) // Clockwise rotation of fork encoder gives -360 deg-> -310 deg
    Torquedac=65536; //by adding +360 we get 0-180 deg anticlockwise
    if (Torquedac<-65536) // Clockwise rotation of fork encoder gives -360 deg-> -310 deg 
    Torquedac=-65536; //by adding +360 we get 0-180 deg anticlockwise
    analogWriteResolution(16);
    analogWrite(pwmforkmotor,abs(Torquedac)); 
    digitalWrite(forkmotordrive,LOW);
    
    // -----------------------------------------------Handlebar feedback tracking controller------------------------------------------------------------------------
    
    
    //declare pin alocation on teensie 3.6 board
    const int handmotordrive = 29; // making high enables handlebar motor drive
    const int pwmhandmotor = 8;
    //declare pin as outputs
    pinMode (handmotordrive, OUTPUT);
    pinMode (pwmhandmotor, OUTPUT);
    // define variables
    float Kp1= 0.3;//0.139626 ; // Nick recommends 8 Nm/rad=0.139626 Nm/deg
    float Kd1=0.010472;// Nick recommends 0.6 Nm/rad=0.010472 Nm/deg
    float Tc1= 0.0369;// Torque constant  36.9 mNm/A = 0.0369 Nm/A
    const int Tdac1=1;
    float Torque1;
    signed int Torquedac1;
    
    //--------------------------------------------------------------------calculation of Torque handlebar motor--------------------------------------------------------
    Torque1=Kp1*(-handlebarangle+forkangle);
    Torquedac1=(Torque1*1334+32768)+412; // divide with reduction ratio* torque constant to give a command in Amperes
    // Scale torque amp with a constant to obtain a dac number
    if (Torquedac1>65536) // Clockwise rotation of fork encoder gives -360 deg-> -310 deg
    Torquedac1=65536; //by adding +360 we get 0-180 deg anticlockwise
    if (Torquedac1<-65536) // Clockwise rotation of fork encoder gives -360 deg-> -310 deg 
    Torquedac1=-65536; //by adding +360 we get 0-180 deg anticlockwise
    analogWriteResolution(16);
    analogWrite(pwmhandmotor,abs(Torquedac1)); 
    digitalWrite(handmotordrive,LOW);
    
    // -----------------------------------------------Printing encoder counts&deg handlebar------------------------------------------------------------------------
myFile = SD.open("abs.txt", FILE_WRITE);
 // if the file opened okay, write to it:
  if (myFile) {
  
//Serial.print("Time = "); // printing variables when needed for debugging 
//Serial.print(time);
//Serial.print("Hand(deg)=");
//Serial.print(handlebarangle);
//erial.print(",Hand(Counts)= ");
//Serial.print(mybyte1);
//Printing encoder counts&deg Fork
//Serial.print(",Fork(deg)=");
//Serial.print(forkangle);
//Serial.print(",Fork(Counts)= ");
//Serial.print(mybyte1fork);
//Serial.println(); 
//Serial.print(",Torque(Nm)=");
//Serial.print(Torque);
//Serial.print(",Torque(dac)=");
 //Serial.print(Torquedac);
//Serial.println(); 

time = millis();
myFile.print(time); 
myFile.print(',');
myFile.print(handlebarangle); 
myFile.print(',');
myFile.println(forkangle); 
      // Write to file
myFile.close(); // close the file 
  }
    // if the file didn't open, print an error:
else {
      Serial.println("error opening file or card not present");
   }
  }
}
 
void read_data() {
  
  pinMode (rearencoderA,INPUT);
  pinMode (pedalencoderA,INPUT);

   while (1) {
    // This program calculates the bicycle forward velocity in km/h
    duration = pulseIn(rearencoderA,HIGH,250000); //  Returns the length of the pulse in microseconds when signal output A is high
    freq=1200000/(duration*2); // The period of the is 2 times the duration of the pulse of output A https://tushev.org/articles/arduino/9/measuring-frequency-with-arduino
    rpm = (freq*60)/96; // 96 are the line counts of the encoder http://www.quantumdev.com/finding-the-rpm-of-an-optical-encoder-using-an-oscilloscope/
    U=6.28*0.3355*rpm*60/1000; // http://answers.tutorvista.com/489133/what-is-the-formula-for-converting-rpm-to-kph.html#
    // This program calculates the pedal cadence speed in rpm
    duration1 = pulseIn(pedalencoderA,HIGH,250000); //  Returns the length of the pulse in microseconds when signal output A is high
    freq1=1200000/(duration1*2); // The period is 2 times the duration of the pulse of output A. 12e6 is set based on speedometer output comparison https://tushev.org/articles/arduino/9/measuring-frequency-with-arduino
    rpm1 = (freq1*60)/96; // 96 are the line counts of the encoder http://www.quantumdev.com/finding-the-rpm-of-an-optical-encoder-using-an-oscilloscope/
  
  // -----------------------------------------------Data logging forward velocity U and pedal cadence rpm1------------------------------------------------------------------------------

 // Create/Open file 
//myFile = SD.open("gear1.txt", FILE_WRITE);
    // if the file opened okay, write to it:
  //  if (myFile) {
      
 Serial.print("Time = "); // printing variables when needed for debugging
   Serial.print(time);
     Serial.print(", Speed(km/h) = ");
     Serial.print(U);
     Serial.print(",Pedal(rpm)= ");
      Serial.println(rpm1);
      
     
     //  time = millis();
     // myFile.print(time); 
    //  myFile.print(',');
    //  myFile.print(U); 
   //  myFile.print(',');
   //   myFile.println(rpm1); 
      // Write to file
   //  myFile.close(); // close the file 
  //  }
    // if the file didn't open, print an error:
   // else {
   //   Serial.println("error opening file or card not present");
  //  }
  }
}

void setup() {
  SPI.begin(); // intializing SPI
  SD.begin(CS); // Initialize SD Library
  Serial.begin(500000); // begin serial communication;
  threads.addThread(run_motor); // creating a thread call run motor fuction
  threads.addThread(read_data);// creating a thread call read data fuction
}

void loop() {
 // threads.delay(10);
}

}
 
Last edited:
I don't know how this RTOS works exactly but if it switches threads by fixed time slices without waiting for time sensitive tasks like pulseIn to accomplish, you'll naturally get wrong results. I doubt that this RTOS does context switching, I think that the micros() counter (on which the pulseIn() function depends) does just continue, even while other threads are being serviced, which will for sure go into the pants.

I think, you should rather use FTM capture to measure the pulse widths in a hardware counter without dependency from whatever task timing.

Edit: I see now that this RTOS does preemptive multitasking, thus you should make sure that the read_data() thread is not interrupted or will remain locked while the (non thread safe) pulseIn() function is active.
 
Last edited:
This application note gives a rough overview. Better is to study the ways more detailed FTM chapter of the respective processor reference manuals.
 
Thank you for your valuable help. I was thinking using FreqMeasureMulti library instead of pulseIn to estimate the frequency.Since FreqMeasureMulti library uses CPU clock cycle to estimate frequency and is not task timing related is there an issue by using this library while using the teensiethreads (RTOS)? Otherwise I will take a look at the FTM chapter of the processor manual. Is there an example using FTM capture on teensie board?
 
The FreqMeasureMulti library uses the internal system bus clock for the FTM timer. It - should be - thread safe, since the timer continues to count independently of the context switching but under the condition that this RTOS allows hardware interrupts to be serviced without delay.

But basically, now that you can do the pulse capture on dedicated FTM hardware in the background, perhaps the RTOS (with all its overhead) is not longer needed?

Edit: ...and the FreqMeasureMulti library is one of the best examples for FTM capture on the Teensy.
 
Last edited:
The other libs using input capture, aside from FreqMeasure & FreqMeasureMulti, are AltSoftSerial and PulsePosition. I believe tni may have posted a DMA-based example on the forum several months ago, but I don't have a link handy.
 
Status
Not open for further replies.
Back
Top