Diff: main.cpp

Revision:

27:d37b3a0e0f2b

Parent:

26:088e397ec26f

Child:

28:8c90a46b613e

--- a/main.cpp	Tue Oct 29 11:13:15 2019 +0000
+++ b/main.cpp	Tue Oct 29 14:22:05 2019 +0000
@@ -45,7 +45,7 @@
 bool    SubstateChanged = true;
 volatile bool    pressed_1    = false;
 volatile bool    pressed_2    = false;
-HIDScope scope(3);
+HIDScope scope(6);
 
 volatile float theta_1;
 //volatile float  theta_error_1;
@@ -59,9 +59,15 @@
 float Kd;
 
 
-BiQuad Lowpass ( 1.55148e-04, 3.10297e-04, 1.55148e-04, -1.96446e+00, 9.65081e-01 );
-BiQuad Highpass ( 9.40809e-01, -9.40809e-01, 0.00000e+00, -8.81619e-01, 0.00000e+00);
-BiQuad notch (9.98432e-01, -1.89913e+00, 9.98432e-01, -1.89913e+00, 9.96886e-01);
+BiQuad Lowpass_bl ( 1.55148e-04, 3.10297e-04, 1.55148e-04, -1.96446e+00, 9.65081e-01 );
+BiQuad Highpass_bl ( 9.40809e-01, -9.40809e-01, 0.00000e+00, -8.81619e-01, 0.00000e+00);
+BiQuad notch_bl (9.98432e-01, -1.89913e+00, 9.98432e-01, -1.89913e+00, 9.96886e-01);
+BiQuad Lowpass_br ( 1.55148e-04, 3.10297e-04, 1.55148e-04, -1.96446e+00, 9.65081e-01 );
+BiQuad Highpass_br ( 9.40809e-01, -9.40809e-01, 0.00000e+00, -8.81619e-01, 0.00000e+00);
+BiQuad notch_br (9.98432e-01, -1.89913e+00, 9.98432e-01, -1.89913e+00, 9.96886e-01);
+BiQuad Lowpass_leg ( 1.55148e-04, 3.10297e-04, 1.55148e-04, -1.96446e+00, 9.65081e-01 );
+BiQuad Highpass_leg ( 9.40809e-01, -9.40809e-01, 0.00000e+00, -8.81619e-01, 0.00000e+00);
+BiQuad notch_leg (9.98432e-01, -1.89913e+00, 9.98432e-01, -1.89913e+00, 9.96886e-01);
 
 int n = 0;
 
@@ -208,14 +214,25 @@
     pressed_2 = !pressed_2;
 }
 
+bool emg_switch(float treshold, float emg_input) {
+    if(emg_input > treshold){
+        return true;
+    } else {
+        return false;
+    }
+}
+
 float EmgCalibration(float emgFiltered, float mvc_value, float rest_value)
 {
     float emgCalibrated;
     if (emgFiltered <= rest_value) {
-        emgCalibrated = 0;
+        return 0.0f;
+        //emgCalibrated = 0;
     }
+    
     if (emgFiltered >= mvc_value) {
-        emgCalibrated = 1;
+        return 1.1f;
+        //emgCalibrated = 1;
     } else {
         emgCalibrated = (emgFiltered-rest_value)/(mvc_value-rest_value);
     }
@@ -224,20 +241,20 @@
 
 void emgsample()
 {
-    emgFiltered_bl = Highpass.step(emg_bl.read());
-    emgFiltered_bl = notch.step(emgFiltered_bl);
+    emgFiltered_bl = Highpass_bl.step(emg_bl.read());
+    emgFiltered_bl = notch_bl.step(emgFiltered_bl);
     emgFiltered_bl = fabs(emgFiltered_bl);
-    emgFiltered_bl = Lowpass.step(emgFiltered_bl);
+    emgFiltered_bl = Lowpass_bl.step(emgFiltered_bl);
 
-    emgFiltered_br = Highpass.step(emg_br.read());
-    emgFiltered_br = notch.step(emgFiltered_br);
+    emgFiltered_br = Highpass_br.step(emg_br.read());
+    emgFiltered_br = notch_br.step(emgFiltered_br);
     emgFiltered_br = fabs(emgFiltered_br);
-    emgFiltered_br = Lowpass.step(emgFiltered_br);
+    emgFiltered_br = Lowpass_br.step(emgFiltered_br);
 
-    emgFiltered_leg = Highpass.step(emg_leg.read());
-    emgFiltered_leg = notch.step(emgFiltered_leg);
+    emgFiltered_leg = Highpass_leg.step(emg_leg.read());
+    emgFiltered_leg = notch_leg.step(emgFiltered_leg);
     emgFiltered_leg = fabs(emgFiltered_leg);
-    emgFiltered_leg = Lowpass.step(emgFiltered_leg);
+    emgFiltered_leg = Lowpass_leg.step(emgFiltered_leg);
 }
 
 void rest()
@@ -335,52 +352,15 @@
     }
 }
 
-float emgCalibrated_bl;
-float emgCalibrated_br;
-float emgCalibrated_leg;
-
 void WriteScope()
 {
     emgsample();
-    /*
-    if (emgFiltered_bl <= rest_value_bl) {
-        emgCalibrated_bl = 0;
-    }
-    if (emgFiltered_bl >= mvc_value_bl) {
-        emgCalibrated_bl = 1;
-    } else {
-        emgCalibrated_bl = (emgFiltered_bl - rest_value_bl) / (mvc_value_bl - rest_value_bl);
-    }
-    if (emgFiltered_br <= rest_value_br) {
-        emgCalibrated_br = 0;
-    }
-    if (emgFiltered_br >= mvc_value_br) {
-        emgCalibrated_br = 1;
-    } else {
-        emgCalibrated_br = (emgFiltered_br - rest_value_br) / (mvc_value_br - rest_value_br);
-    }
-    if (emgFiltered_leg <= rest_value_leg) {
-        emgCalibrated_leg = 0;
-    }
-    if (emgFiltered_leg >= mvc_value_leg) {
-        emgCalibrated_leg = 1;
-    } else {
-        emgCalibrated_leg = (emgFiltered_leg - rest_value_leg) / (mvc_value_leg - rest_value_leg);
-    }
-    */
-    /*
     scope.set(0, EmgCalibration(emgFiltered_bl, mvc_value_bl, rest_value_bl));
     scope.set(1, EmgCalibration(emgFiltered_br, mvc_value_br, rest_value_br));
     scope.set(2, EmgCalibration(emgFiltered_leg, mvc_value_leg, rest_value_leg));
-    */
-    /*
-    scope.set(0, emgCalibrated_bl);
-    scope.set(1, emgCalibrated_br);
-    scope.set(2, emgCalibrated_leg);
-    */
-    scope.set(0, emg_bl.read());
-    scope.set(1, emgCalibrated_bl);
-    scope.set(2, EmgCalibration(emgFiltered_bl, mvc_value_bl, rest_value_bl));
+    scope.set(3, emgFiltered_bl);
+    scope.set(4, emgFiltered_br);
+    scope.set(5, emgFiltered_leg);
     scope.send();
 }
 
@@ -601,7 +581,7 @@
     //sinus_time.start();
     //PWM_motor_1.period_ms(10);
     //motor_control.attach(&MotorControl, Ts);
-    write_scope.attach(&WriteScope, 0.001);
+    write_scope.attach(&WriteScope, 0.01);
     //TickerStateMachine.attach(StateMachine,1.00f);
     while(true) {
         StateMachine();