Robert Hill
Final Program for Cycle Fit Project, Program is able to collect, process and output via bluetooth values for: Pedal Speed (RPM) Heart Rate (BPM) and incline (degrees)
Dependencies: FXAS21000 FXOS8700Q mbed
Diff: BIKE.cpp
- Revision:
- 0:30fa229c34d6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/BIKE.cpp Sun Apr 10 20:53:46 2016 +0000
@@ -0,0 +1,417 @@
+#include "mbed.h"
+#include "FXOS8700Q.h" //Needed for Accelerometer
+#define PI 3.14159265
+#include "PulseSensor.h"
+#include "FXAS21000.h" //Needed for Gyroscope (If I add it)
+
+DigitalOut gpo(D0);
+DigitalOut led_red(LED_RED);
+DigitalOut led_green(LED_GREEN);
+DigitalOut led_blue(LED_BLUE);
+Serial pc(USBTX, USBRX);
+Serial blue(PTC17,PTC16);
+
+int longTimer = 10;
+double shortTimer = 0.5;
+
+//instantiating timers
+Timer time01;
+Timer time02;
+
+//instantiating summation and counter variables
+int HR_Sum;
+int RPM_Sum;
+int Acc_Sum;
+int Acc_Count;
+
+//instantiating averaging variables
+int avgRPM;
+int avgAcc;
+int avgHR;
+
+//variables needed for processing
+////////////////////////////////////////Heart Rate////////////////////////////////////////////////////////////////////////
+AnalogIn Pulse_Signal(A0); //Initialize analog input for pulse signal(Heart Rate sensor)
+int Pulse_Count=0;
+/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////Pedal Speed///////////////////////////////////////////////////////////////////////////////
+DigitalIn HallEffect(D2); //Intializes Digital input into pin D2 (Hall Effect sensor)
+int Hall_Counter = 0; //Initialize Counter for Hall Effect
+bool Low_Hall = true; //Initialize Flag for Hall Effect sensor
+bool Button_Pressed = true; //Initialize flag for output on terminal
+Timer Hall_Timer; //Initialize timer for pedal speed calc
+int Pedal_Time; //Intialize int for Time Passed
+int Pedal_Speed; //Initialize int for Pedal Speed
+/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////Incline////////////////////////////////////////////////////////////////////////////////
+I2C i2c(PTE25, PTE24); //Initialize I2C connection for sensor
+FXOS8700QAccelerometer acc(i2c, FXOS8700CQ_SLAVE_ADDR1); // Configured for the FRDM-K64F with onboard sensors
+FXAS21000 gyro(D14, D15);
+double angleX, denomX_T, denomX_A, denomX_B, denomX_C; //intializing variable to hold the angle calculation
+double angleY, denomY_T, denomY_A, denomY_B, denomY_C; //and denominator pieces of algorithm for both X&Y axis
+float faX, faY, faZ; //intialize float variables for incoming raw sensor values
+double Start_Angle; //Used for intial calibration to find an offset
+double Acc_Angle; //Eq: Angle Experienced - Start_Angle = Actual Angle seen by bike
+
+int drift_counter=0;
+double gyro_reset=0;
+double gyro_drift_degree = 0;
+double gyro_drift_angle = 0;
+double gyro_data[3];
+double gyro_angle = 0; //always intilize
+double gyro_degree = 0;
+double gyro_drift =0;
+int gyro_read_time;
+
+double Filter_Angle=0;
+/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+//////////////////////////////////////////Sensor Processing////////////////////////////////////////////////////////////////////////
+
+//////////////////////////////////////Heart Rate Processing/////////////////////////////////////////////////////////////////////////
+PulseSensor::PulseSensor(PinName analogPin, void (*printDataCallback)(char,int), int callbackRateMs)
+{
+ _started = false;
+
+ _pAin = new AnalogIn(analogPin);
+
+ _callbackRateMs = callbackRateMs;
+
+ _printDataCallback = printDataCallback;
+}
+
+
+PulseSensor::~PulseSensor()
+{
+ delete _pAin;
+}
+
+
+void PulseSensor::process_data_ticker_callback(void)
+{
+ // _printDataCallback('S', Signal); // send Processing the raw Pulse Sensor data ** Commented out so it doens't stream data
+ if (QS == true) { // Quantified Self flag is true when a heartbeat is found
+
+ //_printDataCallback('B',BPM); // send heart rate with a 'B' prefix **Commented out so it doens't stream data
+ //_printDataCallback('Q',IBI); // send time between beats with a 'Q' prefix **Commented out so it doens't stream data
+ QS = false; // reset the Quantified Self flag for next time
+ }
+}
+
+
+void PulseSensor::sensor_ticker_callback(void)
+{
+ Signal = 1023 * _pAin->read(); // read the Pulse Sensor
+
+
+ sampleCounter += 2; // keep track of the time in mS with this variable
+ int N = sampleCounter - lastBeatTime; // monitor the time since the last beat to avoid noise
+
+ // find the peak and trough of the pulse wave
+ if(Signal < thresh && N > (IBI/5)*3) { // avoid dichrotic noise by waiting 3/5 of last IBI
+ if (Signal < T) { // T is the trough
+ T = Signal; // keep track of lowest point in pulse wave
+ }
+ }
+
+ if(Signal > thresh && Signal > P) { // thresh condition helps avoid noise
+ P = Signal; // P is the peak
+ } // keep track of highest point in pulse wave
+
+ // NOW IT'S TIME TO LOOK FOR THE HEART BEAT
+ // signal surges up in value every time there is a pulse
+ if (N > 250) { // avoid high frequency noise by waiting
+ //this also sets limit to HR sensor to max =240 BPMs
+
+ if ( (Signal > thresh) && (Pulse == false) && (N > (IBI/5)*3) ) {
+ Pulse = true; // set the Pulse flag when we think there is a pulse
+ Pulse_Count++; //This should count every time there is a pulse ** value needed for loops below
+ IBI = sampleCounter - lastBeatTime; // measure time between beats in mS
+ lastBeatTime = sampleCounter; // keep track of time for next pulse
+
+ if(firstBeat) { // if it's the first time we found a beat, if firstBeat == TRUE
+ firstBeat = false; // clear firstBeat flag
+ return; // IBI value is unreliable so discard it
+ }
+ if(secondBeat) { // if this is the second beat, if secondBeat == TRUE
+ secondBeat = false; // clear secondBeat flag
+ for(int i=0; i<=9; i++) { // seed the running total to get a realisitic BPM at startup
+ rate[i] = IBI;
+ }
+ }
+
+ // keep a running total of the last 10 IBI values
+ long runningTotal = 0; // clear the runningTotal variable
+
+ for(int i=0; i<=8; i++) { // shift data in the rate array
+ rate[i] = rate[i+1]; // and drop the oldest IBI value
+ runningTotal += rate[i]; // add up the 9 oldest IBI values
+ }
+
+ rate[9] = IBI; // add the latest IBI to the rate array
+ runningTotal += rate[9]; // add the latest IBI to runningTotal
+ runningTotal /= 10; // average the last 10 IBI values
+ BPM = 60000/runningTotal; // how many beats can fit into a minute? that's BPM!
+ QS = true; // set Quantified Self flag
+ // QS FLAG IS NOT CLEARED INSIDE THIS ISR
+ }
+ }
+
+ if (Signal < thresh && Pulse == true) { // when the values are going down, the beat is over
+ Pulse = false; // reset the Pulse flag so we can do it again
+ amp = P - T; // get amplitude of the pulse wave
+ thresh = amp/2 + T; // set thresh at 50% of the amplitude
+ P = thresh; // reset these for next time
+ T = thresh;
+ }
+
+ if (N > 2500) { // if 2.5 seconds go by without a beat
+ thresh = 512; // set thresh default
+ P = 512; // set P default
+ T = 512; // set T default
+ lastBeatTime = sampleCounter; // bring the lastBeatTime up to date
+ firstBeat = true; // set these to avoid noise
+ secondBeat = true; // when we get the heartbeat back
+ }
+}
+
+void sendDataToProcessing(char symbol, int data)
+{
+ pc.printf("%c%d\t\r\n", symbol, data);
+}
+
+bool PulseSensor::start()
+{
+ if (_started == false)
+ {
+ sampleCounter = 0;
+ lastBeatTime = 0;
+ P =512;
+ T = 512;
+ thresh = 512;
+ amp = 100;
+ firstBeat = true;
+ secondBeat = true;
+
+ BPM=0;
+ Signal=0;
+ IBI = 600;
+ Pulse = false;
+ QS = false;
+
+ _pulseSensorTicker.attach(this, &PulseSensor::sensor_ticker_callback, ((float)_sensorTickRateMs/1000));
+ _processDataTicker.attach(this, &PulseSensor::process_data_ticker_callback, ((float)_callbackRateMs/1000));
+ _started = true;
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+}
+
+bool PulseSensor::stop()
+{
+ if(_started == true)
+ {
+ _pulseSensorTicker.detach();
+ _processDataTicker.detach();
+ _started = false;
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+}
+/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+///////////////////////////////////Pedal Speed Processing//////////////////////////////////////////////////////////////////////////////////
+void RPM (void)
+{
+ Hall_Counter=0; //resets so it adds either a 0 or 1 for the RPM_Sum counter
+ Button_Pressed = true;
+
+ if(HallEffect==0 && Low_Hall==true) { //If Hall Effect Digital Output is low
+ Hall_Counter++; //Add one to counter for calc pedal speed
+ led_green = 0; //Output Green on LED, simulates wheel rotation "sensed"
+ led_red = 1;
+ led_blue = 1;
+ Low_Hall = false; //flag to avoid errors
+ }
+
+ else if(HallEffect==1 && Low_Hall==true){ //Additional logic for accurate readings
+ led_green = 1;
+ led_red = 0; //Stays red while hall effect outputs digital high
+ led_blue = 1;
+ }
+ else if(HallEffect==0 && Low_Hall==false){
+ led_green = 0;
+ led_red = 1;
+ led_blue = 1;
+
+ }
+ else if(HallEffect==1 && Low_Hall==false){
+ led_green = 1;
+ led_red = 0;
+ led_blue = 1;
+ Low_Hall = true;
+ }
+ // Hall_Active=0;
+ wait(0.05);
+}
+/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/////////////////////////////////////Incline Processing/////////////////////////////////////////////////////////////////////////
+void Acc(void)
+ {
+ acc.getX(faX); //get raw accelerometer data in the X axis
+ acc.getY(faY); //"" Y axis
+ acc.getZ(faZ); //"" Z axis
+ denomY_A = pow(faX, 2);
+ denomY_B = pow(faZ, 2);
+ denomY_C = denomY_A + denomY_B;
+ denomY_T = pow(denomY_C, .5); //pow returns base raised to the power exponent
+
+ angleY = atan(faY/denomY_T) * 180/PI; //should calculate the angle on the Y axis in degrees based on raw data
+ Acc_Angle= angleY - Start_Angle;
+ }
+
+void Offset_Calc(void) //Function for angle calc upon startup
+ {
+ acc.getX(faX); //get raw accelerometer data in the X axis
+ acc.getY(faY); //"" Y axis
+ acc.getZ(faZ);
+ denomY_A = pow(faX, 2);
+ denomY_B = pow(faZ, 2);
+ denomY_C = denomY_A + denomY_B;
+ denomY_T = pow(denomY_C, .5); //pow returns base raised to the power exponent
+
+ Start_Angle = atan(faY/denomY_T) * 180/PI; //Stores intial angle value for offset calculation
+
+ }
+void Gyro(void)
+ {
+ gyro.ReadXYZ(gyro_data); //Gyro output data
+ gyro_degree= gyro_data[0] / 10; //take y value (dps) and conver it to just degrees
+ gyro_angle += gyro_degree; //add new degree value to old value
+ //pc.printf("Angle: %f \t\r\n",gyro_angle); //print angle seen by gyro
+ wait_ms(10); //wait needed in order to know what to integrate by
+
+ /*
+ while (drift_counter < 30)
+ {
+ gyro.ReadXYZ(gyro_data);
+ gyro_degree= gyro_data[0];
+ gyro_angle += (gyro_degree+gyro_drift_angle)/10;
+ pc.printf("Angle: %f \t\r\n",gyro_angle);
+ wait_ms(10);
+ drift_counter++;
+ gyro_reset=gyro_angle;
+ }
+ drift_counter=0;
+ */
+ }
+void filter(void)
+ {
+ //Angle = a*(Angle + gyroData *dt) + (1-a)*(accData) filter formula constants subject to change in order to tune filter (must add to 1)
+ // a = tau / ( tau * dt)
+ //tau = ?
+ //dt = 10ms
+ /*
+ The time constant of a filter is the relative duration of signal it will act on.
+ For a low-pass filter, signals much longer than the time constant pass through unaltered
+ while signals shorter than the time constant are filtered out. The opposite is true for a highpass
+ filter.
+ */
+
+ Filter_Angle = 0.35 * (Filter_Angle + gyro_angle) + 0.65*(Acc_Angle);
+ //pc.printf("The Angle after the values are filtered: %f \t\r\n", Filter_Angle);
+
+ }
+/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+int main()
+{
+ blue.baud(9600);
+ acc.enable(); //Turn on Accelerometer
+ Offset_Calc(); //Run intial calibration angle
+ PulseSensor Pulse_Signal(A0, sendDataToProcessing); //intialize pulse sensor to pin A0 on board
+ Pulse_Signal.start(); //Turn on pulse sensor
+ //runs data collection continuously
+ //blue.printf("Bluetooth Start\t\r");
+
+ while(1)
+ {
+
+
+ //reset HR count and RPM count
+ HR_Sum = 0;
+ RPM_Sum = 0;
+
+ //reset timer01 for HR and RPM
+ time01.reset();
+ time01.start();
+
+
+
+
+ //Loop includes all four sensors(HR, RPM, & Acc+Gyro)
+ while(time01.read() < longTimer) { //runs for 10 seconds
+
+
+ //reset timer02 for Acc
+ time02.reset();
+ time02.start();
+
+ Acc(); //might need to move this into the half second loop so that the Acc value the gyro takes to avoid drift is more accuarte
+
+
+ //loop does not includes acc sensor
+ while(time02.read() < shortTimer) //half second loop
+ {
+ //call Hall sensor funtion to pull # of revolutions
+
+ RPM(); //Resets every time its run so it sends a 1 or a 0 to add to the RPM_Sum
+
+ Gyro();
+ filter();
+
+ RPM_Sum = RPM_Sum + Hall_Counter; // + function value
+
+ }
+
+ //Every half second calculate angle
+
+ /*
+ blue.printf("This is the current angle: %f in degrees\t\r\n\n", Actual_Angle); //this is here to check values being found before sending for debugging
+ blue.printf("This is the current pedal count: %d \t\r\n\n",RPM_Sum);
+ blue.printf("This is the current heart rate: %d in BPM \t\r\n\n",Pulse_Signal.BPM);
+ */
+
+ blue.printf("%d,%d,%f\n",Pulse_Signal.BPM, RPM_Sum, Filter_Angle); //prints to bluetooth
+ //pc.printf("%d,%d,%f\n",Pulse_Signal.BPM, RPM_Sum, Filter_Angle); //prints to terminal
+
+
+
+
+ //send data avgAcc, avgHR/BPM variable, avgRPM
+
+ }
+ //reseting average HR and RPM
+ avgHR = 0;
+ avgRPM = 0;
+
+
+ //converts sum to beats per min
+ avgHR = HR_Sum*60/longTimer;
+
+ //converts sum to revolutions per min
+ avgRPM = RPM_Sum*60/longTimer;
+
+ }
+
+}