Robert Hill
Cycle Fit - All Sensors
Diff: All_Sensors.cpp
- Revision:
- 0:ef02694deaa8
diff -r 000000000000 -r ef02694deaa8 All_Sensors.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/All_Sensors.cpp Wed Feb 24 16:53:45 2016 +0000
@@ -0,0 +1,334 @@
+#include "mbed.h"
+
+#include "PulseSensor.h" //Needed for HeartBeat
+//#include "AnalogIn.h"
+
+#include "DigitalIn.h" //Needed for Hall Effect
+
+#include "FXOS8700Q.h" //Needed for Accelerometer
+#define PI 3.14159265
+
+
+DigitalOut led_red(LED_RED);
+DigitalOut led_green(LED_GREEN);
+DigitalOut led_blue(LED_BLUE);
+DigitalIn sw2(SW2);
+DigitalIn sw3(SW3);
+Serial pc(USBTX, USBRX);
+bool Button_Pressed = true; //Initialize flag for output on terminal
+///////////////////////////////////////Variables//////////////////////////////////////////////////
+
+///////////////////////////////////////Heart Rate/////////////////////////////////////////////////
+
+AnalogIn Pulse_Signal(A0); //Initialize analog input for pulse signal(Heart Rate sensor)
+
+///////////////////////////////////////Hall Effect/////////////////////////////////////////////////
+
+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
+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
+
+////////////////////////////////////////Accelerometer////////////////////////////////////////////////
+
+I2C i2c(PTE25, PTE24); //Initialize I2C connection for sensor
+FXOS8700QAccelerometer acc(i2c, FXOS8700CQ_SLAVE_ADDR1); // Configured for the FRDM-K64F with onboard sensors
+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
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////
+
+/////////////////////////////////////////////Heart Rate Processing Section////////////////////////////////////////////////////////
+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
+ if (QS == true) { // Quantified Self flag is true when a heartbeat is found
+ //fadeRate = 255; // Set 'fadeRate' Variable to 255 to fade LED with pulse
+ _printDataCallback('B',BPM); // send heart rate with a 'B' prefix
+ _printDataCallback('Q',IBI); // send time between beats with a 'Q' prefix
+ 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
+ //digitalWrite(blinkPin,HIGH); // turn on pin 13 LED
+ 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;
+ }
+}
+////////////////////////////////////////////////////////End of Heart Rate Processing section//////////////////////////////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////////////Hall Effect Processing section/////////////////////////////////////////////////////
+
+void Hall_Effect_Count(void)
+{
+ Hall_Timer.stop(); //stop the timer
+ Pedal_Time=Hall_Timer.read()/60; //Divides Time in seconds by 60 so we have minutes for Pedal_Speed (RPM)
+ Pedal_Speed=Hall_Counter/(Hall_Timer.read()/60); //Calculates pedal speed in units of RPM
+
+ //Hall_Timer.reset(); //idea here is the timer is reset after the program outputs the pedal speed so the demo can be reran
+
+}
+
+void RPM (void)
+{
+ 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;
+ }
+
+
+}
+////////////////////////////////////////////////////////End of Hall Effect Processing section//////////////////////////////////////////////////////////////////////////////////
+
+////////////////////////////////////////////////////////Inclinometer Processing section//////////////////////////////////////////////////////////////////////////////////
+ /*
+ In order to calculate angle from accel data use the following algorithm:
+ Ax = arctan( rawX/sqrt(rawY^2 + rawZ^2))
+ Ay = arctan( rawY/sqrt(rawX^2 + rawZ^2))
+ using Ax as an example:
+ we have denom_T = the total denominator = (sqrt(rawY^2 + rawZ^2 )
+ denom_A = rawY^2 && denom_B = rawZ^2 && denom_C = denom_A + denom_B
+ we may only be concerned with one of these angles for our application
+ also note value is output in radians will need to convert to degrees using: 180/PI
+
+ example of how to use inverse tangent function :
+
+ int main ()
+ {
+ double param, result;
+ param = 1.0;
+ result = atan (param) * 180 / PI;
+ printf ("The arc tangent of %f is %f degrees\n", param, result );
+ return 0;
+ }
+ */
+
+ /*
+ Example of pow: 7 ^ 3 would be written as pow(7.0, 3.0);
+ we can use this find squareroots by making the power exponent = 0.5
+ */
+ void Y_AXIS(void)
+ {
+
+ float faX, faY, faZ; //intialize float variables for incoming raw sensor values
+ 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
+ }
+////////////////////////////////////////////////////////End of Inclinometer Processing section//////////////////////////////////////////////////////////////////////////////////
+
+int main()
+{
+ led_blue = 1; //LED Off
+ led_green = 1;
+ led_red = 1;
+ pc.baud(9600);
+ pc.printf("Hello World from FRDM-K64F board. This is the Cycle Fit Sensor Demo Program. \t \r\n");
+ pc.printf("This program calcualtes and outputs: Heart Rate(in BPM), Pedal Speed(in RPM), and the Angle of incline(in degrees) Press Button SW2 to see fitness metrics.\t \r\n");
+
+
+ PulseSensor Pulse_Signal(A0, sendDataToProcessing); //Intializes Pulse_Signal to A0 for HR
+ Pulse_Signal.start(); //Start collecting data from sensor
+
+ Hall_Timer.start(); //Starts Timer for Pedal Speed Calculation
+
+ acc.enable(); //enables Accel sensor so it can collect data
+
+
+
+ while(1) //NEED TO RUN THOUGH ALL BUTTON PRESS LOGIC FOR COMBINED PROGRAM************************
+ {
+ Button_Pressed = true;
+ RPM(); //function that finds pedal speed through H.E. sensor
+ Y_AXIS(); //function that finds incline angle through Accel sensor
+ if (sw2==0 && Button_Pressed == true)
+ {
+ Pulse_Signal.stop(); //stops the continuous signal (do i need to stop the signal?)
+ pc.printf("Current Heart Rate is: %d BPM\t \r\n", Pulse_Signal.BPM); //Outputs Heart Rate
+
+ Hall_Effect_Count(); //function for calculating pedal speed
+ //Hall_Timer.start(); //restart Hall Effect Timer
+ pc.printf("Approximate pedal speed: %d RPM\t \r\n", Pedal_Speed); //Outputs Pedal Speed
+
+ printf("Approximate angle in the Y-Axis =%f degrees\t \r\n", angleY); //Outputs Inclination Angle in Y axis
+ puts(""); //clears a line under output
+
+ //Pulse_Signal.start();
+ //Button_Pressed= false;
+ //wait(0.5);
+ }
+ else if (sw3==0 && Button_Pressed == true)
+ {
+ Pulse_Signal.start();
+ Hall_Timer.start();
+ }
+ }
+}
