George1988
Member
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:
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° 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° 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: