George1988
Member
Code:
#define ENCODER_OPTIMIZE_INTERRUPTS
#include <Encoder.h> // library used to measure encoder counts of handlebars and fork https://www.pjrc.com/teensy/td_libs_Encoder.html
#define rearencoderA 0
#define pedalencoderA 2
unsigned long duration;
unsigned long duration1;
unsigned long freq;
unsigned long freq1;
unsigned long rpm;
unsigned long rpm1;
unsigned long U;
unsigned long rear;
long Handcounts, Forkcounts;
Encoder Handlebars(4,5);
Encoder Fork(6,7);
void setup() {
Serial.begin(19200);
pinMode (rearencoderA,INPUT);
pinMode (pedalencoderA,INPUT);
}
void loop() {
Handcounts = Handlebars.read();
Forkcounts = Fork.read();
// 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/
Serial.print("Handlebar(deg) = ");
Serial.print(Handcounts*360/294908);
Serial.print(", Fork(deg) = ");
Serial.print(Forkcounts*360/294908);
Serial.print(", Speed(km/h) = ");
Serial.print(U);
Serial.print(",Pedal(rpm)= ");
Serial.print(rpm1);
Serial.println();
}I would like to combine both codes and instead of printing the following data (U,rpm1,handlebarangle and forkangle) log the following data to an SD card.
I am a newbie and I do not know how to do it exactly? Can someone please guide me
The code below reads absolute encoders and commands two motors:
Code:
#include <SPI.h> // include the SPI library to initiate the clock signal
//pin alocation on teensie 3.6 board
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
void setup() {
// set the slaveSelectpins and power pin of encoders as outputs:
pinMode (ssimu, OUTPUT);
pinMode (sshand, OUTPUT);
pinMode (ssfork, OUTPUT);
pinMode (pwrencoders, OUTPUT);
// initialize SPI:
SPI.begin();
Serial.begin(500000); // begin serial communication;
}
void loop() {
// 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; //// 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);
//-------------------------------------------------------------------- This program calculates the bicycle forward velocity in km/h---------------------------
// -----------------------------------------------Printing encoder counts° handlebar------------------------------------------------------------------------
Serial.print("Hand(deg)=");
Serial.print(handlebarangle);
// Serial.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();
}