Robert Hill / Mbed 2 deprecated FRDM_BIKE_

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

BIKE.cpp@0:30fa229c34d6, 2016-04-10 (annotated)

Committer:
roberthill04
Date:
Sun Apr 10 20:53:46 2016 +0000
Revision:
0:30fa229c34d6
Final Cycle Fit Program

Who changed what in which revision?

UserRevisionLine numberNew contents of line
roberthill04 0:30fa229c34d6 1 #include "mbed.h"
roberthill04 0:30fa229c34d6 2 #include "FXOS8700Q.h" //Needed for Accelerometer
roberthill04 0:30fa229c34d6 3 #define PI 3.14159265
roberthill04 0:30fa229c34d6 4 #include "PulseSensor.h"
roberthill04 0:30fa229c34d6 5 #include "FXAS21000.h" //Needed for Gyroscope (If I add it)
roberthill04 0:30fa229c34d6 6
roberthill04 0:30fa229c34d6 7 DigitalOut gpo(D0);
roberthill04 0:30fa229c34d6 8 DigitalOut led_red(LED_RED);
roberthill04 0:30fa229c34d6 9 DigitalOut led_green(LED_GREEN);
roberthill04 0:30fa229c34d6 10 DigitalOut led_blue(LED_BLUE);
roberthill04 0:30fa229c34d6 11 Serial pc(USBTX, USBRX);
roberthill04 0:30fa229c34d6 12 Serial blue(PTC17,PTC16);
roberthill04 0:30fa229c34d6 13
roberthill04 0:30fa229c34d6 14 int longTimer = 10;
roberthill04 0:30fa229c34d6 15 double shortTimer = 0.5;
roberthill04 0:30fa229c34d6 16
roberthill04 0:30fa229c34d6 17 //instantiating timers
roberthill04 0:30fa229c34d6 18 Timer time01;
roberthill04 0:30fa229c34d6 19 Timer time02;
roberthill04 0:30fa229c34d6 20
roberthill04 0:30fa229c34d6 21 //instantiating summation and counter variables
roberthill04 0:30fa229c34d6 22 int HR_Sum;
roberthill04 0:30fa229c34d6 23 int RPM_Sum;
roberthill04 0:30fa229c34d6 24 int Acc_Sum;
roberthill04 0:30fa229c34d6 25 int Acc_Count;
roberthill04 0:30fa229c34d6 26
roberthill04 0:30fa229c34d6 27 //instantiating averaging variables
roberthill04 0:30fa229c34d6 28 int avgRPM;
roberthill04 0:30fa229c34d6 29 int avgAcc;
roberthill04 0:30fa229c34d6 30 int avgHR;
roberthill04 0:30fa229c34d6 31
roberthill04 0:30fa229c34d6 32 //variables needed for processing
roberthill04 0:30fa229c34d6 33 ////////////////////////////////////////Heart Rate////////////////////////////////////////////////////////////////////////
roberthill04 0:30fa229c34d6 34 AnalogIn Pulse_Signal(A0); //Initialize analog input for pulse signal(Heart Rate sensor)
roberthill04 0:30fa229c34d6 35 int Pulse_Count=0;
roberthill04 0:30fa229c34d6 36 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
roberthill04 0:30fa229c34d6 37
roberthill04 0:30fa229c34d6 38 ///////////////////////////////////////Pedal Speed///////////////////////////////////////////////////////////////////////////////
roberthill04 0:30fa229c34d6 39 DigitalIn HallEffect(D2); //Intializes Digital input into pin D2 (Hall Effect sensor)
roberthill04 0:30fa229c34d6 40 int Hall_Counter = 0; //Initialize Counter for Hall Effect
roberthill04 0:30fa229c34d6 41 bool Low_Hall = true; //Initialize Flag for Hall Effect sensor
roberthill04 0:30fa229c34d6 42 bool Button_Pressed = true; //Initialize flag for output on terminal
roberthill04 0:30fa229c34d6 43 Timer Hall_Timer; //Initialize timer for pedal speed calc
roberthill04 0:30fa229c34d6 44 int Pedal_Time; //Intialize int for Time Passed
roberthill04 0:30fa229c34d6 45 int Pedal_Speed; //Initialize int for Pedal Speed
roberthill04 0:30fa229c34d6 46 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
roberthill04 0:30fa229c34d6 47
roberthill04 0:30fa229c34d6 48 ///////////////////////////////////////Incline////////////////////////////////////////////////////////////////////////////////
roberthill04 0:30fa229c34d6 49 I2C i2c(PTE25, PTE24); //Initialize I2C connection for sensor
roberthill04 0:30fa229c34d6 50 FXOS8700QAccelerometer acc(i2c, FXOS8700CQ_SLAVE_ADDR1); // Configured for the FRDM-K64F with onboard sensors
roberthill04 0:30fa229c34d6 51 FXAS21000 gyro(D14, D15);
roberthill04 0:30fa229c34d6 52 double angleX, denomX_T, denomX_A, denomX_B, denomX_C; //intializing variable to hold the angle calculation
roberthill04 0:30fa229c34d6 53 double angleY, denomY_T, denomY_A, denomY_B, denomY_C; //and denominator pieces of algorithm for both X&Y axis
roberthill04 0:30fa229c34d6 54 float faX, faY, faZ; //intialize float variables for incoming raw sensor values
roberthill04 0:30fa229c34d6 55 double Start_Angle; //Used for intial calibration to find an offset
roberthill04 0:30fa229c34d6 56 double Acc_Angle; //Eq: Angle Experienced - Start_Angle = Actual Angle seen by bike
roberthill04 0:30fa229c34d6 57
roberthill04 0:30fa229c34d6 58 int drift_counter=0;
roberthill04 0:30fa229c34d6 59 double gyro_reset=0;
roberthill04 0:30fa229c34d6 60 double gyro_drift_degree = 0;
roberthill04 0:30fa229c34d6 61 double gyro_drift_angle = 0;
roberthill04 0:30fa229c34d6 62 double gyro_data[3];
roberthill04 0:30fa229c34d6 63 double gyro_angle = 0; //always intilize
roberthill04 0:30fa229c34d6 64 double gyro_degree = 0;
roberthill04 0:30fa229c34d6 65 double gyro_drift =0;
roberthill04 0:30fa229c34d6 66 int gyro_read_time;
roberthill04 0:30fa229c34d6 67
roberthill04 0:30fa229c34d6 68 double Filter_Angle=0;
roberthill04 0:30fa229c34d6 69 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
roberthill04 0:30fa229c34d6 70
roberthill04 0:30fa229c34d6 71 //////////////////////////////////////////Sensor Processing////////////////////////////////////////////////////////////////////////
roberthill04 0:30fa229c34d6 72
roberthill04 0:30fa229c34d6 73 //////////////////////////////////////Heart Rate Processing/////////////////////////////////////////////////////////////////////////
roberthill04 0:30fa229c34d6 74 PulseSensor::PulseSensor(PinName analogPin, void (*printDataCallback)(char,int), int callbackRateMs)
roberthill04 0:30fa229c34d6 75 {
roberthill04 0:30fa229c34d6 76 _started = false;
roberthill04 0:30fa229c34d6 77
roberthill04 0:30fa229c34d6 78 _pAin = new AnalogIn(analogPin);
roberthill04 0:30fa229c34d6 79
roberthill04 0:30fa229c34d6 80 _callbackRateMs = callbackRateMs;
roberthill04 0:30fa229c34d6 81
roberthill04 0:30fa229c34d6 82 _printDataCallback = printDataCallback;
roberthill04 0:30fa229c34d6 83 }
roberthill04 0:30fa229c34d6 84
roberthill04 0:30fa229c34d6 85
roberthill04 0:30fa229c34d6 86 PulseSensor::~PulseSensor()
roberthill04 0:30fa229c34d6 87 {
roberthill04 0:30fa229c34d6 88 delete _pAin;
roberthill04 0:30fa229c34d6 89 }
roberthill04 0:30fa229c34d6 90
roberthill04 0:30fa229c34d6 91
roberthill04 0:30fa229c34d6 92 void PulseSensor::process_data_ticker_callback(void)
roberthill04 0:30fa229c34d6 93 {
roberthill04 0:30fa229c34d6 94 // _printDataCallback('S', Signal); // send Processing the raw Pulse Sensor data ** Commented out so it doens't stream data
roberthill04 0:30fa229c34d6 95 if (QS == true) { // Quantified Self flag is true when a heartbeat is found
roberthill04 0:30fa229c34d6 96
roberthill04 0:30fa229c34d6 97 //_printDataCallback('B',BPM); // send heart rate with a 'B' prefix **Commented out so it doens't stream data
roberthill04 0:30fa229c34d6 98 //_printDataCallback('Q',IBI); // send time between beats with a 'Q' prefix **Commented out so it doens't stream data
roberthill04 0:30fa229c34d6 99 QS = false; // reset the Quantified Self flag for next time
roberthill04 0:30fa229c34d6 100 }
roberthill04 0:30fa229c34d6 101 }
roberthill04 0:30fa229c34d6 102
roberthill04 0:30fa229c34d6 103
roberthill04 0:30fa229c34d6 104 void PulseSensor::sensor_ticker_callback(void)
roberthill04 0:30fa229c34d6 105 {
roberthill04 0:30fa229c34d6 106 Signal = 1023 * _pAin->read(); // read the Pulse Sensor
roberthill04 0:30fa229c34d6 107
roberthill04 0:30fa229c34d6 108
roberthill04 0:30fa229c34d6 109 sampleCounter += 2; // keep track of the time in mS with this variable
roberthill04 0:30fa229c34d6 110 int N = sampleCounter - lastBeatTime; // monitor the time since the last beat to avoid noise
roberthill04 0:30fa229c34d6 111
roberthill04 0:30fa229c34d6 112 // find the peak and trough of the pulse wave
roberthill04 0:30fa229c34d6 113 if(Signal < thresh && N > (IBI/5)*3) { // avoid dichrotic noise by waiting 3/5 of last IBI
roberthill04 0:30fa229c34d6 114 if (Signal < T) { // T is the trough
roberthill04 0:30fa229c34d6 115 T = Signal; // keep track of lowest point in pulse wave
roberthill04 0:30fa229c34d6 116 }
roberthill04 0:30fa229c34d6 117 }
roberthill04 0:30fa229c34d6 118
roberthill04 0:30fa229c34d6 119 if(Signal > thresh && Signal > P) { // thresh condition helps avoid noise
roberthill04 0:30fa229c34d6 120 P = Signal; // P is the peak
roberthill04 0:30fa229c34d6 121 } // keep track of highest point in pulse wave
roberthill04 0:30fa229c34d6 122
roberthill04 0:30fa229c34d6 123 // NOW IT'S TIME TO LOOK FOR THE HEART BEAT
roberthill04 0:30fa229c34d6 124 // signal surges up in value every time there is a pulse
roberthill04 0:30fa229c34d6 125 if (N > 250) { // avoid high frequency noise by waiting
roberthill04 0:30fa229c34d6 126 //this also sets limit to HR sensor to max =240 BPMs
roberthill04 0:30fa229c34d6 127
roberthill04 0:30fa229c34d6 128 if ( (Signal > thresh) && (Pulse == false) && (N > (IBI/5)*3) ) {
roberthill04 0:30fa229c34d6 129 Pulse = true; // set the Pulse flag when we think there is a pulse
roberthill04 0:30fa229c34d6 130 Pulse_Count++; //This should count every time there is a pulse ** value needed for loops below
roberthill04 0:30fa229c34d6 131 IBI = sampleCounter - lastBeatTime; // measure time between beats in mS
roberthill04 0:30fa229c34d6 132 lastBeatTime = sampleCounter; // keep track of time for next pulse
roberthill04 0:30fa229c34d6 133
roberthill04 0:30fa229c34d6 134 if(firstBeat) { // if it's the first time we found a beat, if firstBeat == TRUE
roberthill04 0:30fa229c34d6 135 firstBeat = false; // clear firstBeat flag
roberthill04 0:30fa229c34d6 136 return; // IBI value is unreliable so discard it
roberthill04 0:30fa229c34d6 137 }
roberthill04 0:30fa229c34d6 138 if(secondBeat) { // if this is the second beat, if secondBeat == TRUE
roberthill04 0:30fa229c34d6 139 secondBeat = false; // clear secondBeat flag
roberthill04 0:30fa229c34d6 140 for(int i=0; i<=9; i++) { // seed the running total to get a realisitic BPM at startup
roberthill04 0:30fa229c34d6 141 rate[i] = IBI;
roberthill04 0:30fa229c34d6 142 }
roberthill04 0:30fa229c34d6 143 }
roberthill04 0:30fa229c34d6 144
roberthill04 0:30fa229c34d6 145 // keep a running total of the last 10 IBI values
roberthill04 0:30fa229c34d6 146 long runningTotal = 0; // clear the runningTotal variable
roberthill04 0:30fa229c34d6 147
roberthill04 0:30fa229c34d6 148 for(int i=0; i<=8; i++) { // shift data in the rate array
roberthill04 0:30fa229c34d6 149 rate[i] = rate[i+1]; // and drop the oldest IBI value
roberthill04 0:30fa229c34d6 150 runningTotal += rate[i]; // add up the 9 oldest IBI values
roberthill04 0:30fa229c34d6 151 }
roberthill04 0:30fa229c34d6 152
roberthill04 0:30fa229c34d6 153 rate[9] = IBI; // add the latest IBI to the rate array
roberthill04 0:30fa229c34d6 154 runningTotal += rate[9]; // add the latest IBI to runningTotal
roberthill04 0:30fa229c34d6 155 runningTotal /= 10; // average the last 10 IBI values
roberthill04 0:30fa229c34d6 156 BPM = 60000/runningTotal; // how many beats can fit into a minute? that's BPM!
roberthill04 0:30fa229c34d6 157 QS = true; // set Quantified Self flag
roberthill04 0:30fa229c34d6 158 // QS FLAG IS NOT CLEARED INSIDE THIS ISR
roberthill04 0:30fa229c34d6 159 }
roberthill04 0:30fa229c34d6 160 }
roberthill04 0:30fa229c34d6 161
roberthill04 0:30fa229c34d6 162 if (Signal < thresh && Pulse == true) { // when the values are going down, the beat is over
roberthill04 0:30fa229c34d6 163 Pulse = false; // reset the Pulse flag so we can do it again
roberthill04 0:30fa229c34d6 164 amp = P - T; // get amplitude of the pulse wave
roberthill04 0:30fa229c34d6 165 thresh = amp/2 + T; // set thresh at 50% of the amplitude
roberthill04 0:30fa229c34d6 166 P = thresh; // reset these for next time
roberthill04 0:30fa229c34d6 167 T = thresh;
roberthill04 0:30fa229c34d6 168 }
roberthill04 0:30fa229c34d6 169
roberthill04 0:30fa229c34d6 170 if (N > 2500) { // if 2.5 seconds go by without a beat
roberthill04 0:30fa229c34d6 171 thresh = 512; // set thresh default
roberthill04 0:30fa229c34d6 172 P = 512; // set P default
roberthill04 0:30fa229c34d6 173 T = 512; // set T default
roberthill04 0:30fa229c34d6 174 lastBeatTime = sampleCounter; // bring the lastBeatTime up to date
roberthill04 0:30fa229c34d6 175 firstBeat = true; // set these to avoid noise
roberthill04 0:30fa229c34d6 176 secondBeat = true; // when we get the heartbeat back
roberthill04 0:30fa229c34d6 177 }
roberthill04 0:30fa229c34d6 178 }
roberthill04 0:30fa229c34d6 179
roberthill04 0:30fa229c34d6 180 void sendDataToProcessing(char symbol, int data)
roberthill04 0:30fa229c34d6 181 {
roberthill04 0:30fa229c34d6 182 pc.printf("%c%d\t\r\n", symbol, data);
roberthill04 0:30fa229c34d6 183 }
roberthill04 0:30fa229c34d6 184
roberthill04 0:30fa229c34d6 185 bool PulseSensor::start()
roberthill04 0:30fa229c34d6 186 {
roberthill04 0:30fa229c34d6 187 if (_started == false)
roberthill04 0:30fa229c34d6 188 {
roberthill04 0:30fa229c34d6 189 sampleCounter = 0;
roberthill04 0:30fa229c34d6 190 lastBeatTime = 0;
roberthill04 0:30fa229c34d6 191 P =512;
roberthill04 0:30fa229c34d6 192 T = 512;
roberthill04 0:30fa229c34d6 193 thresh = 512;
roberthill04 0:30fa229c34d6 194 amp = 100;
roberthill04 0:30fa229c34d6 195 firstBeat = true;
roberthill04 0:30fa229c34d6 196 secondBeat = true;
roberthill04 0:30fa229c34d6 197
roberthill04 0:30fa229c34d6 198 BPM=0;
roberthill04 0:30fa229c34d6 199 Signal=0;
roberthill04 0:30fa229c34d6 200 IBI = 600;
roberthill04 0:30fa229c34d6 201 Pulse = false;
roberthill04 0:30fa229c34d6 202 QS = false;
roberthill04 0:30fa229c34d6 203
roberthill04 0:30fa229c34d6 204 _pulseSensorTicker.attach(this, &PulseSensor::sensor_ticker_callback, ((float)_sensorTickRateMs/1000));
roberthill04 0:30fa229c34d6 205 _processDataTicker.attach(this, &PulseSensor::process_data_ticker_callback, ((float)_callbackRateMs/1000));
roberthill04 0:30fa229c34d6 206 _started = true;
roberthill04 0:30fa229c34d6 207 return true;
roberthill04 0:30fa229c34d6 208 }
roberthill04 0:30fa229c34d6 209 else
roberthill04 0:30fa229c34d6 210 {
roberthill04 0:30fa229c34d6 211 return false;
roberthill04 0:30fa229c34d6 212 }
roberthill04 0:30fa229c34d6 213 }
roberthill04 0:30fa229c34d6 214
roberthill04 0:30fa229c34d6 215 bool PulseSensor::stop()
roberthill04 0:30fa229c34d6 216 {
roberthill04 0:30fa229c34d6 217 if(_started == true)
roberthill04 0:30fa229c34d6 218 {
roberthill04 0:30fa229c34d6 219 _pulseSensorTicker.detach();
roberthill04 0:30fa229c34d6 220 _processDataTicker.detach();
roberthill04 0:30fa229c34d6 221 _started = false;
roberthill04 0:30fa229c34d6 222 return true;
roberthill04 0:30fa229c34d6 223 }
roberthill04 0:30fa229c34d6 224 else
roberthill04 0:30fa229c34d6 225 {
roberthill04 0:30fa229c34d6 226 return false;
roberthill04 0:30fa229c34d6 227 }
roberthill04 0:30fa229c34d6 228 }
roberthill04 0:30fa229c34d6 229 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
roberthill04 0:30fa229c34d6 230
roberthill04 0:30fa229c34d6 231
roberthill04 0:30fa229c34d6 232 ///////////////////////////////////Pedal Speed Processing//////////////////////////////////////////////////////////////////////////////////
roberthill04 0:30fa229c34d6 233 void RPM (void)
roberthill04 0:30fa229c34d6 234 {
roberthill04 0:30fa229c34d6 235 Hall_Counter=0; //resets so it adds either a 0 or 1 for the RPM_Sum counter
roberthill04 0:30fa229c34d6 236 Button_Pressed = true;
roberthill04 0:30fa229c34d6 237
roberthill04 0:30fa229c34d6 238 if(HallEffect==0 && Low_Hall==true) { //If Hall Effect Digital Output is low
roberthill04 0:30fa229c34d6 239 Hall_Counter++; //Add one to counter for calc pedal speed
roberthill04 0:30fa229c34d6 240 led_green = 0; //Output Green on LED, simulates wheel rotation "sensed"
roberthill04 0:30fa229c34d6 241 led_red = 1;
roberthill04 0:30fa229c34d6 242 led_blue = 1;
roberthill04 0:30fa229c34d6 243 Low_Hall = false; //flag to avoid errors
roberthill04 0:30fa229c34d6 244 }
roberthill04 0:30fa229c34d6 245
roberthill04 0:30fa229c34d6 246 else if(HallEffect==1 && Low_Hall==true){ //Additional logic for accurate readings
roberthill04 0:30fa229c34d6 247 led_green = 1;
roberthill04 0:30fa229c34d6 248 led_red = 0; //Stays red while hall effect outputs digital high
roberthill04 0:30fa229c34d6 249 led_blue = 1;
roberthill04 0:30fa229c34d6 250 }
roberthill04 0:30fa229c34d6 251 else if(HallEffect==0 && Low_Hall==false){
roberthill04 0:30fa229c34d6 252 led_green = 0;
roberthill04 0:30fa229c34d6 253 led_red = 1;
roberthill04 0:30fa229c34d6 254 led_blue = 1;
roberthill04 0:30fa229c34d6 255
roberthill04 0:30fa229c34d6 256 }
roberthill04 0:30fa229c34d6 257 else if(HallEffect==1 && Low_Hall==false){
roberthill04 0:30fa229c34d6 258 led_green = 1;
roberthill04 0:30fa229c34d6 259 led_red = 0;
roberthill04 0:30fa229c34d6 260 led_blue = 1;
roberthill04 0:30fa229c34d6 261 Low_Hall = true;
roberthill04 0:30fa229c34d6 262 }
roberthill04 0:30fa229c34d6 263 // Hall_Active=0;
roberthill04 0:30fa229c34d6 264 wait(0.05);
roberthill04 0:30fa229c34d6 265 }
roberthill04 0:30fa229c34d6 266 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
roberthill04 0:30fa229c34d6 267
roberthill04 0:30fa229c34d6 268 /////////////////////////////////////Incline Processing/////////////////////////////////////////////////////////////////////////
roberthill04 0:30fa229c34d6 269 void Acc(void)
roberthill04 0:30fa229c34d6 270 {
roberthill04 0:30fa229c34d6 271 acc.getX(faX); //get raw accelerometer data in the X axis
roberthill04 0:30fa229c34d6 272 acc.getY(faY); //"" Y axis
roberthill04 0:30fa229c34d6 273 acc.getZ(faZ); //"" Z axis
roberthill04 0:30fa229c34d6 274 denomY_A = pow(faX, 2);
roberthill04 0:30fa229c34d6 275 denomY_B = pow(faZ, 2);
roberthill04 0:30fa229c34d6 276 denomY_C = denomY_A + denomY_B;
roberthill04 0:30fa229c34d6 277 denomY_T = pow(denomY_C, .5); //pow returns base raised to the power exponent
roberthill04 0:30fa229c34d6 278
roberthill04 0:30fa229c34d6 279 angleY = atan(faY/denomY_T) * 180/PI; //should calculate the angle on the Y axis in degrees based on raw data
roberthill04 0:30fa229c34d6 280 Acc_Angle= angleY - Start_Angle;
roberthill04 0:30fa229c34d6 281 }
roberthill04 0:30fa229c34d6 282
roberthill04 0:30fa229c34d6 283 void Offset_Calc(void) //Function for angle calc upon startup
roberthill04 0:30fa229c34d6 284 {
roberthill04 0:30fa229c34d6 285 acc.getX(faX); //get raw accelerometer data in the X axis
roberthill04 0:30fa229c34d6 286 acc.getY(faY); //"" Y axis
roberthill04 0:30fa229c34d6 287 acc.getZ(faZ);
roberthill04 0:30fa229c34d6 288 denomY_A = pow(faX, 2);
roberthill04 0:30fa229c34d6 289 denomY_B = pow(faZ, 2);
roberthill04 0:30fa229c34d6 290 denomY_C = denomY_A + denomY_B;
roberthill04 0:30fa229c34d6 291 denomY_T = pow(denomY_C, .5); //pow returns base raised to the power exponent
roberthill04 0:30fa229c34d6 292
roberthill04 0:30fa229c34d6 293 Start_Angle = atan(faY/denomY_T) * 180/PI; //Stores intial angle value for offset calculation
roberthill04 0:30fa229c34d6 294
roberthill04 0:30fa229c34d6 295 }
roberthill04 0:30fa229c34d6 296 void Gyro(void)
roberthill04 0:30fa229c34d6 297 {
roberthill04 0:30fa229c34d6 298 gyro.ReadXYZ(gyro_data); //Gyro output data
roberthill04 0:30fa229c34d6 299 gyro_degree= gyro_data[0] / 10; //take y value (dps) and conver it to just degrees
roberthill04 0:30fa229c34d6 300 gyro_angle += gyro_degree; //add new degree value to old value
roberthill04 0:30fa229c34d6 301 //pc.printf("Angle: %f \t\r\n",gyro_angle); //print angle seen by gyro
roberthill04 0:30fa229c34d6 302 wait_ms(10); //wait needed in order to know what to integrate by
roberthill04 0:30fa229c34d6 303
roberthill04 0:30fa229c34d6 304 /*
roberthill04 0:30fa229c34d6 305 while (drift_counter < 30)
roberthill04 0:30fa229c34d6 306 {
roberthill04 0:30fa229c34d6 307 gyro.ReadXYZ(gyro_data);
roberthill04 0:30fa229c34d6 308 gyro_degree= gyro_data[0];
roberthill04 0:30fa229c34d6 309 gyro_angle += (gyro_degree+gyro_drift_angle)/10;
roberthill04 0:30fa229c34d6 310 pc.printf("Angle: %f \t\r\n",gyro_angle);
roberthill04 0:30fa229c34d6 311 wait_ms(10);
roberthill04 0:30fa229c34d6 312 drift_counter++;
roberthill04 0:30fa229c34d6 313 gyro_reset=gyro_angle;
roberthill04 0:30fa229c34d6 314 }
roberthill04 0:30fa229c34d6 315 drift_counter=0;
roberthill04 0:30fa229c34d6 316 */
roberthill04 0:30fa229c34d6 317 }
roberthill04 0:30fa229c34d6 318 void filter(void)
roberthill04 0:30fa229c34d6 319 {
roberthill04 0:30fa229c34d6 320 //Angle = a*(Angle + gyroData *dt) + (1-a)*(accData) filter formula constants subject to change in order to tune filter (must add to 1)
roberthill04 0:30fa229c34d6 321 // a = tau / ( tau * dt)
roberthill04 0:30fa229c34d6 322 //tau = ?
roberthill04 0:30fa229c34d6 323 //dt = 10ms
roberthill04 0:30fa229c34d6 324 /*
roberthill04 0:30fa229c34d6 325 The time constant of a filter is the relative duration of signal it will act on.
roberthill04 0:30fa229c34d6 326 For a low-pass filter, signals much longer than the time constant pass through unaltered
roberthill04 0:30fa229c34d6 327 while signals shorter than the time constant are filtered out. The opposite is true for a highpass
roberthill04 0:30fa229c34d6 328 filter.
roberthill04 0:30fa229c34d6 329 */
roberthill04 0:30fa229c34d6 330
roberthill04 0:30fa229c34d6 331 Filter_Angle = 0.35 * (Filter_Angle + gyro_angle) + 0.65*(Acc_Angle);
roberthill04 0:30fa229c34d6 332 //pc.printf("The Angle after the values are filtered: %f \t\r\n", Filter_Angle);
roberthill04 0:30fa229c34d6 333
roberthill04 0:30fa229c34d6 334 }
roberthill04 0:30fa229c34d6 335 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
roberthill04 0:30fa229c34d6 336
roberthill04 0:30fa229c34d6 337 int main()
roberthill04 0:30fa229c34d6 338 {
roberthill04 0:30fa229c34d6 339 blue.baud(9600);
roberthill04 0:30fa229c34d6 340 acc.enable(); //Turn on Accelerometer
roberthill04 0:30fa229c34d6 341 Offset_Calc(); //Run intial calibration angle
roberthill04 0:30fa229c34d6 342 PulseSensor Pulse_Signal(A0, sendDataToProcessing); //intialize pulse sensor to pin A0 on board
roberthill04 0:30fa229c34d6 343 Pulse_Signal.start(); //Turn on pulse sensor
roberthill04 0:30fa229c34d6 344 //runs data collection continuously
roberthill04 0:30fa229c34d6 345 //blue.printf("Bluetooth Start\t\r");
roberthill04 0:30fa229c34d6 346
roberthill04 0:30fa229c34d6 347 while(1)
roberthill04 0:30fa229c34d6 348 {
roberthill04 0:30fa229c34d6 349
roberthill04 0:30fa229c34d6 350
roberthill04 0:30fa229c34d6 351 //reset HR count and RPM count
roberthill04 0:30fa229c34d6 352 HR_Sum = 0;
roberthill04 0:30fa229c34d6 353 RPM_Sum = 0;
roberthill04 0:30fa229c34d6 354
roberthill04 0:30fa229c34d6 355 //reset timer01 for HR and RPM
roberthill04 0:30fa229c34d6 356 time01.reset();
roberthill04 0:30fa229c34d6 357 time01.start();
roberthill04 0:30fa229c34d6 358
roberthill04 0:30fa229c34d6 359
roberthill04 0:30fa229c34d6 360
roberthill04 0:30fa229c34d6 361
roberthill04 0:30fa229c34d6 362 //Loop includes all four sensors(HR, RPM, & Acc+Gyro)
roberthill04 0:30fa229c34d6 363 while(time01.read() < longTimer) { //runs for 10 seconds
roberthill04 0:30fa229c34d6 364
roberthill04 0:30fa229c34d6 365
roberthill04 0:30fa229c34d6 366 //reset timer02 for Acc
roberthill04 0:30fa229c34d6 367 time02.reset();
roberthill04 0:30fa229c34d6 368 time02.start();
roberthill04 0:30fa229c34d6 369
roberthill04 0:30fa229c34d6 370 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
roberthill04 0:30fa229c34d6 371
roberthill04 0:30fa229c34d6 372
roberthill04 0:30fa229c34d6 373 //loop does not includes acc sensor
roberthill04 0:30fa229c34d6 374 while(time02.read() < shortTimer) //half second loop
roberthill04 0:30fa229c34d6 375 {
roberthill04 0:30fa229c34d6 376 //call Hall sensor funtion to pull # of revolutions
roberthill04 0:30fa229c34d6 377
roberthill04 0:30fa229c34d6 378 RPM(); //Resets every time its run so it sends a 1 or a 0 to add to the RPM_Sum
roberthill04 0:30fa229c34d6 379
roberthill04 0:30fa229c34d6 380 Gyro();
roberthill04 0:30fa229c34d6 381 filter();
roberthill04 0:30fa229c34d6 382
roberthill04 0:30fa229c34d6 383 RPM_Sum = RPM_Sum + Hall_Counter; // + function value
roberthill04 0:30fa229c34d6 384
roberthill04 0:30fa229c34d6 385 }
roberthill04 0:30fa229c34d6 386
roberthill04 0:30fa229c34d6 387 //Every half second calculate angle
roberthill04 0:30fa229c34d6 388
roberthill04 0:30fa229c34d6 389 /*
roberthill04 0:30fa229c34d6 390 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
roberthill04 0:30fa229c34d6 391 blue.printf("This is the current pedal count: %d \t\r\n\n",RPM_Sum);
roberthill04 0:30fa229c34d6 392 blue.printf("This is the current heart rate: %d in BPM \t\r\n\n",Pulse_Signal.BPM);
roberthill04 0:30fa229c34d6 393 */
roberthill04 0:30fa229c34d6 394
roberthill04 0:30fa229c34d6 395 blue.printf("%d,%d,%f\n",Pulse_Signal.BPM, RPM_Sum, Filter_Angle); //prints to bluetooth
roberthill04 0:30fa229c34d6 396 //pc.printf("%d,%d,%f\n",Pulse_Signal.BPM, RPM_Sum, Filter_Angle); //prints to terminal
roberthill04 0:30fa229c34d6 397
roberthill04 0:30fa229c34d6 398
roberthill04 0:30fa229c34d6 399
roberthill04 0:30fa229c34d6 400
roberthill04 0:30fa229c34d6 401 //send data avgAcc, avgHR/BPM variable, avgRPM
roberthill04 0:30fa229c34d6 402
roberthill04 0:30fa229c34d6 403 }
roberthill04 0:30fa229c34d6 404 //reseting average HR and RPM
roberthill04 0:30fa229c34d6 405 avgHR = 0;
roberthill04 0:30fa229c34d6 406 avgRPM = 0;
roberthill04 0:30fa229c34d6 407
roberthill04 0:30fa229c34d6 408
roberthill04 0:30fa229c34d6 409 //converts sum to beats per min
roberthill04 0:30fa229c34d6 410 avgHR = HR_Sum*60/longTimer;
roberthill04 0:30fa229c34d6 411
roberthill04 0:30fa229c34d6 412 //converts sum to revolutions per min
roberthill04 0:30fa229c34d6 413 avgRPM = RPM_Sum*60/longTimer;
roberthill04 0:30fa229c34d6 414
roberthill04 0:30fa229c34d6 415 }
roberthill04 0:30fa229c34d6 416
roberthill04 0:30fa229c34d6 417 }