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

Thread: Teensie RTOS (Real time operational system)

  1. #1
    Member
    Join Date
    Nov 2017
    Location
    NEDERLANDS
    Posts
    20

    Teensie RTOS (Real time operational system)

    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 by George1988; 01-08-2018 at 12:57 PM.

  2. #2
    Senior Member+ Theremingenieur's Avatar
    Join Date
    Feb 2014
    Location
    Colmar, France
    Posts
    1,624
    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 by Theremingenieur; 01-08-2018 at 02:02 PM.

  3. #3
    Member
    Join Date
    Nov 2017
    Location
    NEDERLANDS
    Posts
    20
    Thank you for your answer I am a newbie so is FTM capture this: https://www.nxp.com/docs/en/application-note/AN5142.pdf and how do I use it with teensie 3.6? Thank you for your help!!!

  4. #4
    Senior Member+ Theremingenieur's Avatar
    Join Date
    Feb 2014
    Location
    Colmar, France
    Posts
    1,624
    This application note gives a rough overview. Better is to study the ways more detailed FTM chapter of the respective processor reference manuals.

  5. #5
    Member
    Join Date
    Nov 2017
    Location
    NEDERLANDS
    Posts
    20
    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?

  6. #6
    Senior Member+ Theremingenieur's Avatar
    Join Date
    Feb 2014
    Location
    Colmar, France
    Posts
    1,624
    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 by Theremingenieur; 01-09-2018 at 03:11 PM.

  7. #7
    Senior Member PaulStoffregen's Avatar
    Join Date
    Nov 2012
    Posts
    17,595
    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.

  8. #8
    Senior Member+ manitou's Avatar
    Join Date
    Jan 2013
    Posts
    1,466
    Quote Originally Posted by PaulStoffregen View Post
    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.
    tni's DMA input capture https://github.com/tni/teensy-sample...apture_dma.ino

    discussion https://forum.pjrc.com/threads/37148...dth-interrupts

Posting Permissions

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