Diff: main.cpp

Revision:

11:042170a9b93a

Parent:

10:97a79aa10a56

Child:

12:70f0710400c2

--- a/main.cpp	Mon Oct 21 12:52:47 2019 +0000
+++ b/main.cpp	Mon Oct 21 14:07:30 2019 +0000
@@ -11,7 +11,7 @@
 #include <numeric> // For manipulating array data
 
 // PC serial connection
-HIDScope        scope( 2 );
+HIDScope        scope( 3 );
 MODSERIAL pc(USBTX, USBRX);
 
 //EMG inputs definieren
@@ -37,7 +37,6 @@
 
 vector<double> emg1_cal;
 
-int i_cal;
 
 // Initialize tickers
 Ticker tickSample;
@@ -45,20 +44,25 @@
 Ticker tickSampleCalibration;
 
 // Sample rate
-const double Fs = 500; //Hz
-const double Ts = 1/Fs; //sec
+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;
 
 // 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);
-BiQuad bq_notch ( -1.1978e-16, 0.9561, 0.9780, -1.1978e-16, 0.9780 );
+BiQuad bq_notch( 0.995636295063941,  -1.89829218816065,   0.995636295063941,  1, -1.89829218816065,   0.991272590127882);
+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_notch_high; // Used to chain two 2nd other filters into a 4th order filter
+BiQuadChain bqc_high; // Used to chain two 2nd other filters into a 4th order filter
 
 // Lowpass filter coefficients (butter 4th order @5Hz cutoff) from MATLAB in the following form:
 // b01 b11 b21 a01 a11 a21
@@ -93,10 +97,11 @@
 // Check if filters are stable
 bool checkBQChainStable()
 {
-    bool n_hp_stable =  bqc_notch_high.stable();
+    bool n_stable = bqc_notch.stable();
+    bool hp_stable =  bqc_high.stable();
     bool l_stable = bqc_low.stable();
 
-    if (n_hp_stable && l_stable) {
+    if (n_stable && hp_stable && l_stable) {
         return true;
     } else {
         return false;
@@ -104,6 +109,7 @@
 }
 
 
+/*
 // Read samples, filter samples and output to HIDScope
 void sample()
 {
@@ -127,9 +133,8 @@
     // Output EMG after filters
     scope.set(1, emg1_env );
     scope.send();
-
-
 }
+*/
 
 void sampleCalibration()
 {
@@ -142,12 +147,13 @@
     scope.set(0, emg1 );
     
     double emg1_n = bqc_notch.step( emg1 ); // Filter notch
+    scope.set(1, emg1_n);
     double emg1_hp = bqc_high.step( emg1_n ); // Filter highpass
-    double emg1_rectify = fabs( emg1_n_hp ); // Rectify
+    double emg1_rectify = fabs( emg1_hp ); // Rectify
     double emg1_env = bqc_low.step( emg1_rectify ); // Filter lowpass (completes envelope)
 
     // Output EMG after filters
-    scope.set(1, emg1_env );
+    scope.set(2, emg1_env );
     scope.send();
 
     emg1_cal.push_back(emg1_env);
@@ -156,6 +162,7 @@
 void calibrationMVCFinished()
 {
     tickSampleCalibration.detach();
+    
     emg1_mean = getMean(emg1_cal);
     emg1_stdev = getStdev(emg1_cal, emg1_mean);
     
@@ -166,7 +173,7 @@
 
 void calibrationMVC()
 {
-    timeoutCalibrationMVC.attach( &calibrationMVCFinished, 10.0f);
+    timeoutCalibrationMVC.attach( &calibrationMVCFinished, Tcal);
     tickSampleCalibration.attach( &sampleCalibration, Ts );
     led_b = 0;
 }