Revision 0:30fa229c34d6, committed 2016-04-10

Comitter:

roberthill04

Date:

Sun Apr 10 20:53:46 2016 +0000

Commit message:

Final Cycle Fit Program

Changed in this revision

diff -r 000000000000 -r 30fa229c34d6 BIKE.cpp
--- /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;                  
+        
+    }   
+        
+}

diff -r 000000000000 -r 30fa229c34d6 FXAS21000.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FXAS21000.lib	Sun Apr 10 20:53:46 2016 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/roberthill04/code/FXAS21000/#79bf9c15d4d7

diff -r 000000000000 -r 30fa229c34d6 FXOS8700Q.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FXOS8700Q.lib	Sun Apr 10 20:53:46 2016 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/teams/Freescale/code/FXOS8700Q/#aee7dea904e2

diff -r 000000000000 -r 30fa229c34d6 PulseSensor.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PulseSensor.h	Sun Apr 10 20:53:46 2016 +0000
@@ -0,0 +1,75 @@
+#ifndef PULSE_SENSOR_H
+#define PULSE_SENSOR_H
+ 
+#include "mbed.h"
+ 
+ 
+/**
+ * Class for interfacing with a http://pulsesensor.myshopify.com/ 'Pulse Sensor Amped'.
+ * The contents of this class are based on the "Pulse Sensor Amped 1.1" Arduino Sketch.
+ *
+ * When using this class for the first time, it is recommended that you use the Processing 
+ * GUI app available http://pulsesensor.myshopify.com/pages/code-and-guide. Using this, you
+ * will easily be able to verify the operating of your sensor, and the integration of this
+ * class into your mbed project. 
+ * WEBSITE is incorrect^^^
+ **/
+ 
+class PulseSensor
+{
+    private:
+        volatile int rate[10];                          // used to hold last ten IBI values
+        volatile unsigned long sampleCounter;          // used to determine pulse timing
+        volatile unsigned long lastBeatTime;           // used to find the inter beat interval
+        volatile int P;                                // used to find peak in pulse wave
+        volatile int T;                     // used to find trough in pulse wave
+        volatile int thresh;                // used to find instant moment of heart beat
+        volatile int amp;                   // used to hold amplitude of pulse waveform
+        volatile bool firstBeat;        // used to seed rate array so we startup with reasonable BPM
+        volatile bool secondBeat;       // used to seed rate array so we startup with reasonable BPM
+        
+        // these variables are volatile because they are used during the interrupt service routine!
+        
+        volatile int Signal;                // holds the incoming raw data
+        volatile int IBI;             // holds the time between beats, the Inter-Beat Interval
+        volatile bool Pulse;        // true when pulse wave is high, false when it's low
+        volatile bool QS;           // becomes true when a beat is found
+    
+    
+        void (*_printDataCallback)(char,int);
+        static const int _sensorTickRateMs = 2;
+        int       _callbackRateMs;
+        bool      _started;
+        
+        AnalogIn *_pAin;
+        Ticker    _pulseSensorTicker;
+        Ticker    _processDataTicker;
+        
+        void sensor_ticker_callback(void);
+        void process_data_ticker_callback(void);
+    
+    public:
+    
+         volatile int BPM;                   // used to hold the pulse rate
+        /** PulseSensor Constructor - Note this does not start the reading of the sensor.
+         * @param   analogPin Name of the analog pin that the sensor is connected to.
+         * @param   printDataCallback Pointer to function which will be called to print the latest data. Output format available here: http://pulsesensor.myshopify.com/pages/code-and-guide
+         * @param   callbackRateMs Rate at which the printDataCallback is to be called, recommended is 20ms for graphing of pulse signal.
+         */
+        PulseSensor(PinName analogPin, void (*printDataCallback)(char,int), int callbackRateMs=20);
+        
+        /** Destructor */
+        ~PulseSensor();
+        
+        /** Start reading the Pulse Sensor, and sending current readings to the print data callback.
+         * @returns true if reading of the sensor is started, false if reading was aleady in progress.
+         */
+        bool start();
+        
+        /** Stops the current reading of the Pulse Senson.
+         * @return true if reading is stopped, false if reading was already stopped.
+         */
+        bool stop();
+};
+ 
+#endif
\ No newline at end of file

diff -r 000000000000 -r 30fa229c34d6 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sun Apr 10 20:53:46 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/165afa46840b
\ No newline at end of file