Diff: main.cpp

Revision:

15:421d3d9c563b

Parent:

12:70f0710400c2

Child:

16:7acbcc4aa35c

--- a/main.cpp	Mon Oct 21 14:36:24 2019 +0000
+++ b/main.cpp	Tue Oct 22 09:07:29 2019 +0000
@@ -10,14 +10,13 @@
 #include <vector> // For easy array management
 #include <numeric> // For manipulating array data
 
-// PC serial connection
-HIDScope        scope( 3 );
-MODSERIAL pc(USBTX, USBRX);
+/*
+------ DEFINE MBED CONNECTIONS ------
+*/
 
-//EMG inputs definieren
-AnalogIn emg1_in (A1); //emg van rechterbicep, voor de x-richting
-AnalogIn emg2_in (A2); //emg van linkerbicep, voor de y-richting
-AnalogIn emg3_in (A3); //emg van een derde (nog te bepalen) spier, voor het vernaderen van de richting
+// PC serial connection
+HIDScope        scope( 5 );
+MODSERIAL pc(USBTX, USBRX);
 
 // LED
 DigitalOut      led_g(LED_GREEN);
@@ -29,52 +28,68 @@
 InterruptIn button2(D10);
 InterruptIn button3(SW3);
 
-//variablen voor EMG
+// Global variables for EMG reading
+AnalogIn emg1_in (A1); // Right biceps, x axis
+AnalogIn emg2_in (A2); // Left biceps, y axis
+AnalogIn emg3_in (A3); // Third muscle, TBD
+
 double emg1;
-double emg2;
-double emg3;
 double emg1_MVC;
 double emg1_MVC_stdev;
 double emg1_rest;
 double emg1_rest_stdev;
-
 vector<double> emg1_cal;
 
+double emg2;
+double emg2_MVC;
+double emg2_MVC_stdev;
+double emg2_rest;
+double emg2_rest_stdev;
+vector<double> emg2_cal;
 
-// Initialize tickers
+double emg3;
+double emg3_MVC;
+double emg3_MVC_stdev;
+double emg3_rest;
+double emg3_rest_stdev;
+vector<double> emg3_cal;
+
+// Initialize tickers and timeouts
 Ticker tickSample;
+Ticker tickSampleCalibration;
 Timeout timeoutCalibrationMVC;
 Timeout timeoutCalibrationRest;
-Ticker tickSampleCalibration;
 
-// Sample rate
+/*
+------ GLOBAL VARIABLES ------
+*/
 const double Fs = 500; // Sampling frequency (s)
-const double Ts = 1/Fs; // Sampling time (s)
-
 const double Tcal = 10.0f; // Calibration duration (s)
-
-int trim_cal = 1; // Trim the beginning of the calibration vector to reduce transient behaviour by X seconds
-int trim_cal_i = trim_cal * Fs - 1;
+int trim_cal = 1; // Trim transient behaviour of calibration (s)
 
-// Notch filter coefficients (iirnotch Q factor 35 @50Hz) from MATLAB in the following form:
-// b01 b11 b21 a01 a11 a21
-BiQuad bq_notch( 0.995636295063941,  -1.89829218816065,   0.995636295063941,  1, -1.89829218816065,   0.991272590127882);
+// Calculate global variables
+const double Ts = 1/Fs; // Sampling time (s)
+int trim_cal_i = trim_cal * Fs - 1; // Determine iterator of transient behaviour trim
+
+// Notch biquad filter coefficients (iirnotch Q factor 35 @50Hz) from MATLAB:
+BiQuad bq_notch( 0.995636295063941,  -1.89829218816065,   0.995636295063941,  1, -1.89829218816065,   0.991272590127882); // b01 b11 b21 a01 a11 a21
 BiQuadChain bqc_notch;
 
-// Highpass filter coefficients (butter 4th order @10Hz cutoff) from MATLAB in the following form:
-// b01 b11 b21 a01 a11 a21
-// b02 b12 b22 a02 a12 a22
-BiQuad bq_H1(0.922946103200875, -1.84589220640175,  0.922946103200875,  1,  -1.88920703055163,  0.892769008131025);
-BiQuad bq_H2(1,                 -2,                 1,                  1,  -1.95046575793011,  0.954143234875078);
-BiQuadChain bqc_high; // Used to chain two 2nd other filters into a 4th order filter
+// Highpass biquad filter coefficients (butter 4th order @10Hz cutoff) from MATLAB
+BiQuad bq_H1(0.922946103200875, -1.84589220640175,  0.922946103200875,  1,  -1.88920703055163,  0.892769008131025); // b01 b11 b21 a01 a11 a21
+BiQuad bq_H2(1,                 -2,                 1,                  1,  -1.95046575793011,  0.954143234875078); // b02 b12 b22 a02 a12 a22
+BiQuadChain bqc_high;
 
-// Lowpass filter coefficients (butter 4th order @5Hz cutoff) from MATLAB in the following form:
-// b01 b11 b21 a01 a11 a21
-// b02 b12 b22 a02 a12 a22
-BiQuad bq_L1(5.32116245737504e-08,  1.06423249147501e-07,   5.32116245737504e-08,   1,  -1.94396715039462,  0.944882378004138);
-BiQuad bq_L2(1,                     2,                      1,                      1,  -1.97586467534468,  0.976794920438162);
-BiQuadChain bqc_low; // Used to chain two 2nd other filters into a 4th order filter
+// Lowpass biquad filter coefficients (butter 4th order @5Hz cutoff) from MATLAB:
+BiQuad bq_L1(5.32116245737504e-08,  1.06423249147501e-07,   5.32116245737504e-08,   1,  -1.94396715039462,  0.944882378004138); // b01 b11 b21 a01 a11 a21
+BiQuad bq_L2(1,                     2,                      1,                      1,  -1.97586467534468,  0.976794920438162); // b02 b12 b22 a02 a12 a22
+BiQuadChain bqc_low;
 
+/*
+------ HELPER FUNCTIONS ------
+*/
+
+// Return mean of vector
 double getMean(const vector<double> &vect)
 {
     double sum = 0.0;
@@ -86,6 +101,7 @@
     return sum/vect_n;
 }
 
+// Return standard deviation of vector
 double getStdev(const vector<double> &vect, const double vect_mean)
 {
     double sum2 = 0.0;
@@ -98,7 +114,7 @@
     return output;
 }
 
-// Check if filters are stable
+// Check filter stability
 bool checkBQChainStable()
 {
     bool n_stable = bqc_notch.stable();
@@ -112,7 +128,9 @@
     }
 }
 
-
+/*
+------ TICKER FUNCTIONS ------
+*/
 /*
 // Read samples, filter samples and output to HIDScope
 void sample()
@@ -153,77 +171,82 @@
     double emg1_n = bqc_notch.step( emg1 ); // Filter notch
     scope.set(1, emg1_n);
     double emg1_hp = bqc_high.step( emg1_n ); // Filter highpass
+    scope.set(2, emg1_hp);
     double emg1_rectify = fabs( emg1_hp ); // Rectify
+    scope.set(3, emg1_rectify);
     double emg1_env = bqc_low.step( emg1_rectify ); // Filter lowpass (completes envelope)
 
     // Output EMG after filters
-    scope.set(2, emg1_env );
+    scope.set(4, emg1_env );
     scope.send();
 
     emg1_cal.push_back(emg1_env);
 }
 
+/*
+------ EMG CALIBRATION FUNCTIONS ------
+*/
+
+// Finish up calibration of MVC
 void calibrationMVCFinished()
 {
-    tickSampleCalibration.detach();
-    
-    emg1_MVC = getMean(emg1_cal);
-    emg1_MVC_stdev = getStdev(emg1_cal, emg1_MVC);
-    
-    emg1_cal.clear();
-
-    led_b = 1;
+    tickSampleCalibration.detach(); // Stop calibration ticker to remove interrupt
+    emg1_MVC = getMean(emg1_cal); // Store MVC globally
+    emg1_MVC_stdev = getStdev(emg1_cal, emg1_MVC); // Store MVC stde globally
+    emg1_cal.clear(); // Empty vector to prevent memory overflow
+    led_b = 1; // Turn off calibration led
 }
 
+// Run calibration of MVC
 void calibrationMVC()
 {
-    timeoutCalibrationMVC.attach( &calibrationMVCFinished, Tcal);
-    tickSampleCalibration.attach( &sampleCalibration, Ts );
-    led_b = 0;
+    timeoutCalibrationMVC.attach( &calibrationMVCFinished, Tcal); // Stop MVC calibration after interval
+    tickSampleCalibration.attach( &sampleCalibration, Ts ); // Start sample ticker
+    led_b = 0; // Turn on calibration led
 }
 
+// Finish up calibration in rest
 void calibrationRestFinished()
 {
-    tickSampleCalibration.detach();
-    
-    emg1_rest = getMean(emg1_cal);
-    emg1_rest_stdev = getStdev(emg1_cal, emg1_rest);
-    
-    emg1_cal.clear();
-
-    led_b = 1;
+    tickSampleCalibration.detach(); // Stop calibration ticker to remove interrupt
+    emg1_rest = getMean(emg1_cal); // Store rest globally
+    emg1_rest_stdev = getStdev(emg1_cal, emg1_rest);// Store rest stdev globally
+    emg1_cal.clear(); // Empty vector to prevent memory overflow
+    led_b = 1; // Turn off calibration led
 }
 
 void calibrationRest()
 {
-    timeoutCalibrationRest.attach( &calibrationRestFinished, Tcal);
-    tickSampleCalibration.attach( &sampleCalibration, Ts );
-    led_b = 0;
+    timeoutCalibrationRest.attach( &calibrationRestFinished, Tcal); // Stop rest calibration after interval
+    tickSampleCalibration.attach( &sampleCalibration, Ts ); // Start sample ticker
+    led_b = 0; // Turn on calibration led
 }
 
+// Determine scale factors for operation mode
 void makeScale()
 {
-    double margin_percentage = 10;
-    double factor1 = 1 / emg1_MVC;
-    double emg1_th = emg1_rest * factor1 + margin_percentage/100;
+    double margin_percentage = 10; // Set up % margin for rest
+    double factor1 = 1 / emg1_MVC; // Factor to normalize MVC
+    double emg1_th = emg1_rest * factor1 + margin_percentage/100; // Set normalized rest threshold
     
     pc.printf("Factor: %f   TH: %f\r\n", factor1, emg1_th); 
 }
 
 void main()
 {
-    pc.baud(115200);  
+    pc.baud(115200); // MODSERIAL rate
     pc.printf("Starting\r\n");
-    // Initialize sample ticker
-    // tickSample.attach(&sample, Ts);
+    
+    // tickSample.attach(&sample, Ts); // Initialize sample ticker
 
     // Create BQ chains to reduce computations
     bqc_notch.add( &bq_notch );
     bqc_high.add( &bq_H1 ).add( &bq_H2 );
     bqc_low.add( &bq_L1 ).add( &bq_L2 );
 
-    led_b = 1; // Turn led off at startup
-    led_g = 1;
+    led_b = 1; // Turn blue led off at startup
+    led_g = 1; // Turn green led off at startup
+    led_r = 1; // Turn red led off at startup
 
     // If any filter chain is unstable, red led will light up
     if (checkBQChainStable) {
@@ -232,9 +255,9 @@
         led_r = 0; // LED on
     }
 
-    button1.fall( &calibrationMVC );
-    button2.fall( &calibrationRest );
-    button3.fall( &makeScale );
+    button1.fall( &calibrationMVC ); // Run MVC calibration on button press
+    button2.fall( &calibrationRest ); // Run rest calibration on button press
+    button3.fall( &makeScale ); // Create scale factors and close calibration at button press
 
     while(true) {