Diff: main.cpp

Revision:

45:d09040915cfe

Parent:

44:342af9b3c1a0

Child:

46:8a8fa8e602a1

--- a/main.cpp	Thu Oct 31 13:37:25 2019 +0000
+++ b/main.cpp	Thu Oct 31 14:14:23 2019 +0000
@@ -33,6 +33,8 @@
 AnalogIn emg1_in (A0); // Right biceps -> x axis
 AnalogIn emg2_in (A1); // Left biceps  -> y axis
 AnalogIn emg3_in (A2); // Third muscle -> TBD
+AnalogIn emg4_in (A3); // Third muscle -> TBD
+
 
 // Encoder inputs
 DigitalIn encA1(D9);
@@ -197,10 +199,6 @@
 vector<double> emg1_cal;
 int emg1_cal_size; //delete
 double emg1_dir = 1.0;
-double emg1_out_prev;
-double emg1_dt; //delete
-double emg1_dt_prev;
-double emg1_dtdt; //delete
 
 double emg2;
 double emg2_env;
@@ -223,8 +221,16 @@
 double emg3_out;
 double emg3_norm;//delete
 vector<double> emg3_cal;
-int emg3_cal_size;//delete
-int emg3_dir = 1;
+
+double emg4;
+double emg4_env;
+double emg4_MVC;
+double emg4_rest;
+double emg4_factor;//delete
+double emg4_th;
+double emg4_out;
+double emg4_norm;//delete
+vector<double> emg4_cal;
 
 /*
 ------------------------------ EMG FILTERS ------------------------------
@@ -234,9 +240,11 @@
 BiQuad bq1_notch( 0.995636295063941,  -1.89829218816065,   0.995636295063941,  1, -1.89829218816065,   0.991272590127882); // b01 b11 b21 a01 a11 a21
 BiQuad bq2_notch = bq1_notch;
 BiQuad bq3_notch = bq1_notch;
+BiQuad bq4_notch = bq1_notch;
 BiQuadChain bqc1_notch;
 BiQuadChain bqc2_notch;
 BiQuadChain bqc3_notch;
+BiQuadChain bqc4_notch;
 
 // Highpass biquad filter coefficients (butter 4th order @10Hz cutoff) from MATLAB
 BiQuad bq1_H1(0.922946103200875, -1.84589220640175,  0.922946103200875,  1,  -1.88920703055163,  0.892769008131025); // b01 b11 b21 a01 a11 a21
@@ -245,9 +253,12 @@
 BiQuad bq2_H2 = bq1_H2;
 BiQuad bq3_H1 = bq1_H1;
 BiQuad bq3_H2 = bq1_H2;
+BiQuad bq4_H1 = bq1_H1;
+BiQuad bq4_H2 = bq1_H2;
 BiQuadChain bqc1_high;
 BiQuadChain bqc2_high;
 BiQuadChain bqc3_high;
+BiQuadChain bqc4_high;
 
 // Lowpass biquad filter coefficients (butter 4th order @5Hz cutoff) from MATLAB:
 BiQuad bq1_L1(5.32116245737504e-08,  1.06423249147501e-07,   5.32116245737504e-08,   1,  -1.94396715039462,  0.944882378004138); // b01 b11 b21 a01 a11 a21
@@ -256,9 +267,12 @@
 BiQuad bq2_L2 = bq1_L2;
 BiQuad bq3_L1 = bq1_L1;
 BiQuad bq3_L2 = bq1_L2;
+BiQuad bq4_L1 = bq1_L1;
+BiQuad bq4_L2 = bq1_L2;
 BiQuadChain bqc1_low;
 BiQuadChain bqc2_low;
 BiQuadChain bqc3_low;
+BiQuadChain bqc4_low;
 
 // Function to check filter stability
 bool checkBQChainStable()
@@ -283,19 +297,8 @@
     emg1_norm = emg1_env * emg1_factor; // Normalize current EMG signal with calibrated factor
     emg2_norm = emg2_env * emg2_factor; // Idem
     emg3_norm = emg3_env * emg3_factor; // Idem
-
-    emg1_out_prev = emg1_out; // Set previous emg_out signal
-    emg1_dt_prev = emg1_dt; // Set previous emg_out_dt signal
+    emg4_norm = emg4_env * emg4_factor; // Idem
 
-    if (button1_pressed) {
-        button1_pressed = false;
-        emg1_dir = emg1_dir * -1.0;
-    }
-
-    if (button2_pressed) {
-        button2_pressed = false;
-        emg2_dir = emg2_dir * -1.0;
-    }
 
     // Set normalized EMG output signal (CAN BE MOVED TO EXTERNAL FUNCTION BECAUSE IT IS REPEATED 3 TIMES)
     if ( emg1_norm < emg1_th ) { // If below threshold, emg_out = 0 (ignored)
@@ -307,8 +310,6 @@
         // Outputs are scaled to range  [0,      1]
         emg1_out = rescale(emg1_norm, 0, 1, emg1_th, 1);
     }
-    emg1_dt = (emg1_out - emg1_out_prev) / Ts; // Calculate derivative of filtered normalized output signal
-    emg1_dtdt = (emg1_dt - emg1_dt_prev) / Ts; // Calculate acceleration of filtered normalized output signal
 
     // Idem for emg2
     if ( emg2_norm < emg2_th ) {
@@ -321,11 +322,15 @@
 
     // Idem for emg3
     if ( emg3_norm < emg3_th ) {
-        emg3_out = 0.0;
-    } else if ( emg3_norm > 1.0f ) {
-        emg3_out = 1.0;
+        emg1_dir = 1.0;
     } else {
-        emg3_out = rescale(emg3_norm, 0, 1, emg3_th, 1);
+        emg1_dir = -1.0;
+    }
+    
+    if ( emg4_norm < emg4_th ) {
+        emg2_dir = 1.0;
+    } else {
+        emg2_dir = -1.0;
     }
 }
 /*
@@ -384,14 +389,15 @@
         emg1_cal.reserve(Fs * Tcal); // Initialize vector lengths to prevent memory overflow
         emg2_cal.reserve(Fs * Tcal); // Idem
         emg3_cal.reserve(Fs * Tcal); // Idem
+        emg4_cal.reserve(Fs * Tcal); // Idem
     }
 
     // Do stuff until end condition is met
     // Set HIDScope outputs
     scope.set(0, emg1 );
-    scope.set(1, emg1_env );
-    scope.set(2, emg2 );
-    scope.set(3, emg2_env );
+    scope.set(1, emg2 );
+    scope.set(2, emg3 );
+    scope.set(3, emg4 );
     //scope.set(2, emg2_env );
     //scope.set(3, emg3_env );
     scope.send();
@@ -408,19 +414,21 @@
                 emg1_MVC = getMax(emg1_cal); // Store max value of MVC globally
                 emg2_MVC = getMax(emg2_cal);
                 emg3_MVC = getMax(emg3_cal);
-
+                emg4_MVC = getMax(emg4_cal);
                 emg_MVC_cal_done = true; // Set up transition to EMG operation mode
                 break;
             case emg_cal_rest: // In case of rest calibration
                 emg1_rest = getMean(emg1_cal); // Store mean of EMG in rest globally
                 emg2_rest = getMean(emg2_cal);
                 emg3_rest = getMean(emg3_cal);
+                emg4_rest = getMean(emg4_cal);
                 emg_rest_cal_done = true; // Set up transition to EMG operation mode
                 break;
         }
         vector<double>().swap(emg1_cal); // Empty vector to prevent memory overflow
         vector<double>().swap(emg2_cal);
         vector<double>().swap(emg3_cal);
+vector<double>().swap(emg4_cal);
 
         emg_curr_state = emg_wait; // Set next substate
         emg_state_changed = true; // Enable substate entry function
@@ -442,12 +450,8 @@
         emg2_th = emg2_rest * emg2_factor + margin_percentage/100; // Set normalized rest threshold
         emg3_factor = 1 / emg3_MVC; // Factor to normalize MVC
         emg3_th = emg3_rest * emg3_factor + margin_percentage/100; // Set normalized rest threshold
-
-
-        // ------- TO DO: MAKE SURE THESE BUTTONS DO NOT BOUNCE (e.g. with button1.rise() ) ------
-        //button1.fall( &toggleEMG1Dir ); // Change to state MVC calibration on button1 press
-        //button2.fall( &toggleEMG2Dir ); // Change to state rest calibration on button2 press
-
+        emg4_factor = 1 / emg4_MVC; // Factor to normalize MVC
+        emg4_th = emg4_rest * emg4_factor + margin_percentage/100; // Set normalized rest threshold
     }
 
     // This state only runs its entry functions ONCE and then exits the EMG substate machine
@@ -811,10 +815,17 @@
     }
 
     // State transition guard
-    if ( switch2_pressed == true ) {
-        switch2_pressed = false;
+    if ( button1_pressed == true ) {
+        button1_pressed = false;
         operation_curr_state = operation_movement;
         operation_state_changed = true;
+    } else if ( button2_pressed == true ) {
+        button2_pressed = false;
+        //operation_curr_state = operation_finish; // To move to finished operation mode (not used yet)
+        operation_curr_state = operation_wait; // TEMPORARY
+        operation_state_changed = true;
+        global_curr_state = global_wait; // TEMPORARY move directly to global wait state
+        global_state_changed = true; // TEMPORARY move directly to global wait state
     }
 }
 
@@ -843,14 +854,21 @@
     }
 
     // State transition guard
-    if ( switch2_pressed == true ) {
-        switch2_pressed = false;
+    if ( button1_pressed == true ) {
+        button1_pressed = false;
         operation_curr_state = operation_wait;
         operation_state_changed = true;
+    } else if ( button2_pressed == true ) {
+        button2_pressed = false;
+        //operation_curr_state = operation_finish; // To move to finished operation mode (not used yet)
+        operation_curr_state = operation_wait; // TEMPORARY
+        operation_state_changed = true;
+        global_curr_state = global_wait; // TEMPORARY move directly to global wait state
+        global_state_changed = true; // TEMPORARY move directly to global wait state
     }
 }
 
-void do_operation_finish()
+void do_operation_finish() // NOT USED YET
 {
     // Entry function
     if ( operation_state_changed == true ) {
@@ -872,8 +890,8 @@
     // Function to move to starting position
 
     // State transition guard
-    if ( switch2_pressed == true ) {
-        switch2_pressed = false;
+    if ( button2_pressed == true ) {
+        button2_pressed = false;
         operation_curr_state = operation_wait;
         operation_state_changed = true;
 
@@ -1230,6 +1248,7 @@
         emg1 = emg1_in.read();
         emg2 = emg2_in.read();
         emg3 = emg3_in.read();
+        emg4 = emg4_in.read();
 
         double emg1_n = bqc1_notch.step( emg1 );         // Filter notch
         double emg1_hp = bqc1_high.step( emg1_n );       // Filter highpass
@@ -1245,14 +1264,17 @@
         double emg3_hp = bqc3_high.step( emg3_n );       // Filter highpass
         double emg3_rectify = fabs( emg3_hp );           // Rectify
         emg3_env = bqc3_low.step( emg3_rectify ); // Filter lowpass (completes envelope)
+        
+        double emg4_n = bqc4_notch.step( emg4 );         // Filter notch
+        double emg4_hp = bqc4_high.step( emg4_n );       // Filter highpass
+        double emg4_rectify = fabs( emg4_hp );           // Rectify
+        emg4_env = bqc4_low.step( emg4_rectify ); // Filter lowpass (completes envelope)
 
         if (emg_calibrateNow == true) { // Only add values to EMG vectors if calibration flag is true
             emg1_cal.push_back(emg1_env); // Add values to calibration vector
-            // emg1_cal_size = emg1_cal.size(); // Used for debugging
             emg2_cal.push_back(emg2_env); // Add values to calibration vector
-            // emg2_cal_size = emg1_cal.size(); // Used for debugging
             emg3_cal.push_back(emg3_env); // Add values to calibration vector
-            // emg3_cal_size = emg1_cal.size(); // Used for debugging
+            emg4_cal.push_back(emg4_env); // Add values to calibration vector
         }
     }
 
@@ -1299,6 +1321,10 @@
     bqc3_notch.add( &bq3_notch );
     bqc3_high.add( &bq3_H1 ).add( &bq3_H2 );
     bqc3_low.add( &bq3_L1 ).add( &bq3_L2 );
+    
+    bqc4_notch.add( &bq4_notch );
+    bqc4_high.add( &bq4_H1 ).add( &bq4_H2 );
+    bqc4_low.add( &bq4_L1 ).add( &bq4_L2 );
 
     // ---------- Attach buttons ----------
     button1.fall( &button1Press );
@@ -1316,11 +1342,10 @@
     led_r = 1;
 
     while(true) {
-        pc.printf("Global state: %i EMG substate: %i Motor substate: %i\r\n", global_curr_state, emg_curr_state, motor_curr_state);
+        pc.printf("Global state: %i EMG state: %i Motor state: %i Operation state: %i Demo state: %i\r\n", global_curr_state, emg_curr_state, motor_curr_state, operation_curr_state, demo_curr_state);
         pc.printf("EMG1 direction: %f EMG2 direction: %f \r\n", emg1_dir, emg2_dir);
-        pc.printf("Vx: %f Vy: %f \r\n", Vx, Vy);
-        pc.printf("q1: %f q2: %f \r\n",q1*rad2deg,q2*rad2deg);
-        pc.printf("Xe: %f Ye: %f\r\n",xe,ye);
+        //pc.printf("q1: %f q2: %f \r\n",q1*rad2deg,q2*rad2deg);
+        //pc.printf("Xe: %f Ye: %f\r\n",xe,ye);
         wait(0.5f);
     }
 }
\ No newline at end of file