Diff: main.cpp

Revision:

7:7a088536f1c9

Parent:

6:5437cc97e1e6

Child:

8:ea3de43c9e8b

--- a/main.cpp	Sun Oct 20 19:53:00 2019 +0000
+++ b/main.cpp	Mon Oct 21 09:21:17 2019 +0000
@@ -8,6 +8,7 @@
 // #include "Arduino.h" //misschien handig omdat we het EMG arduino board gebruiken (?)
 // #include "EMGFilters.h"
 #include <vector> // For easy array management
+#include <numeric> // For manipulating array data
 
 // PC serial connection
 HIDScope        scope( 2 );
@@ -26,24 +27,19 @@
 double emg1;
 double emg2;
 double emg3;
-double notch1;
-double notch2;
-double notch3;
-double highpass1;
-double highpass2;
-double highpass3;
-double lowpass1;
-double lowpass2;
-double lowpass3;
-double rectify1;
-double rectify2;
-double rectify3;
+
+vector<double> emg1_cal;
+
+int i_cal;
 
 // Initialize tickers
 Ticker tickSample;
+Timeout timeoutCalibrationMVC;
+Ticker tickSampleCalibration;
 
 // Sample rate
 const double Fs = 500; //Hz
+const double Ts = 1/Fs; //sec
 
 // Notch filter coefficients (iirnotch Q factor 35 @50Hz) from MATLAB in the following form:
 // b01 b11 b21 a01 a11 a21
@@ -63,6 +59,11 @@
 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
 
+double stdev(const vector<double> &vect) 
+{
+    
+}
+
 // Check if filters are stable
 bool checkBQChainStable()
 {
@@ -100,12 +101,48 @@
     // Output EMG after filters
     scope.set(1, emg1_env );
     scope.send();
+
+
 }
 
+void sampleCalibration()
+{
+    // Read EMG inputs
+    emg1 = emg1_in.read();
+    emg2 = emg2_in.read();
+    emg3 = emg3_in.read();
+
+    // Output raw EMG input
+    scope.set(0, emg1 );
+
+    double emg1_n_hp = bqc_notch_high.step( emg1 ); // Filter notch and highpass
+    double emg1_rectify = fabs( emg1_n_hp ); // Filter lowpass (completes envelope)
+    double emg1_env = bqc_low.step( emg1_rectify ); // Filter lowpass (completes envelope)
+
+    // Output EMG after filters
+    scope.set(1, emg1_env );
+    scope.send();
+
+    emg1_cal.push_back(emg1_env);
+}
+
+void calibrationMVCFinished()
+{
+    tickSampleCalibration.detach();
+    
+}
+
+void calibrationMVC()
+{
+    timeoutCalibrationMVC.attach( &calibrationMVCFinished, 10.0f);
+    tickSampleCalibration.attach( &sampleCalibration, Ts );
+}
+
+
+
 void main()
 {
     // Initialize sample ticker
-    const double Ts = 1/Fs;
     tickSample.attach(&sample, Ts);
 
     // Create BQ chains to reduce computations
@@ -120,7 +157,7 @@
     }
 
     while(true) {
-        
+
         // Show that system is running
         led_g = !led_g;
         wait(0.5);