Diff: main.cpp

Revision:

7:105e3ae0fb19

Parent:

6:edd30e11f3c7

Child:

8:24f6744d47b2

--- a/main.cpp	Wed Nov 01 13:42:17 2017 +0000
+++ b/main.cpp	Wed Nov 01 14:11:58 2017 +0000
@@ -76,6 +76,18 @@
 BiQuadChain BiQuad_filter_Right;
 BiQuadChain BiQuad_filter_Mode;
 
+float FilterLeft(float input){
+    float Signal_filtered= BiQuad_filter_Left.step(input);
+    return Signal_filtered;
+}
+float FilterRight(float input){
+    float Signal_filtered= BiQuad_filter_Right.step(input);
+    return Signal_filtered;
+}
+float FilterMode(float input){
+    float Signal_filtered= BiQuad_filter_Mode.step(input);
+    return Signal_filtered;
+}
 
 //Calibration-------------------------------------------------------------------
 void setled(){
@@ -84,18 +96,18 @@
 // Zero calibration
 void mincalibration(){
     pc.printf("start min calibration \r\n");
-    emg_offsetLeft = SignalLeft.calibrate(m,emg0);
-    emg_offsetRight = SignalRight.calibrate(m,emg2);
-    emg_offsetMode = SignalMode.calibrate(m, emg4);
+    emg_offsetLeft = SignalLeft.calibrate(m,FilterLeft(emg0));
+    emg_offsetRight = SignalRight.calibrate(m,FilterRight(emg2));
+    emg_offsetMode = SignalMode.calibrate(m, FilterMode(emg4));
     pc.printf("offsets: %f %f \r\n", emg_offsetLeft, emg_offsetRight);
     Led_green=0; Led_red=0; //Set led to Yellow
 }
 // One calibration
 void maxcalibration(){
     pc.printf("start max calibration \r\n");
-    emg_offsetmaxLeft = SignalLeft.calibrate(m, emg0)-emg_offsetLeft;
-    emg_offsetmaxRight = SignalRight.calibrate(m, emg2)-emg_offsetRight;
-    emg_offsetmaxMode = SignalMode.calibrate(m, emg4)-emg_offsetMode;
+    emg_offsetmaxLeft = SignalLeft.calibrate(m, FilterLeft(emg0))-emg_offsetLeft;
+    emg_offsetmaxRight = SignalRight.calibrate(m, FilterRight(emg2))-emg_offsetRight;
+    emg_offsetmaxMode = SignalMode.calibrate(m, FilterMode(emg4))-emg_offsetMode;
     kLeft = 1/emg_offsetmaxLeft;
     kRight = 1/emg_offsetmaxRight;
     kMode = 1/emg_offsetmaxMode;
@@ -105,6 +117,7 @@
 
 //Filtering the signals---------------------------------------------------------
 //Filter de EMG signals with a BiQuad filter
+/*
 float FilterLeft(float input, float offset){
     float Signal=(input-offset);                    //((input0+input1)/2)
     float Signal_filtered= BiQuad_filter_Left.step(Signal);
@@ -120,16 +133,16 @@
     float Signal_filtered= BiQuad_filter_Mode.step(Signal);
     return Signal_filtered;
 }
-
+*/
 
 //------------------------------------------------------------------------------
 //---------------------------------Servo----------------------------------------
 //------------------------------------------------------------------------------
 void servo(){
-    Signal_filteredLeft = fabs(FilterLeft(emg0, emg_offsetLeft));
-    Signal_filteredRight = fabs(FilterRight(emg2, emg_offsetRight));
-    CaseLeft = SignalLeft.getnumber(n, action, Signal_filteredLeft, kLeft);
-    CaseRight = SignalRight.getnumber(n, action, Signal_filteredRight, kRight);
+    Signal_filteredLeft = fabs(FilterLeft(emg0)-emg_offsetLeft);//*kLeft
+    Signal_filteredRight = fabs(FilterRight(emg2)); //*kRight
+    CaseLeft = SignalLeft.getnumber(n, action, Signal_filteredLeft);
+    CaseRight = SignalRight.getnumber(n, action, Signal_filteredRight);
     if (CaseLeft>=3){
         Up = 0; 
         Up = 1;
@@ -171,17 +184,17 @@
 //Determine the signalnumbers (i.e. speed) based on the strenght of the EMG signals
 void signalnumber(){
     //Left
-    Signal_filteredLeft = fabs(FilterLeft(emg0, emg_offsetLeft));
-    mean_value_left = SignalLeft.getmean(n, Signal_filteredLeft)*kLeft;          
-    CaseLeft = SignalLeft.getnumber(n, action, Signal_filteredLeft, kLeft);
+    Signal_filteredLeft = fabs(FilterLeft(emg0)-emg_offsetLeft);//*kLeft
+    mean_value_left = SignalLeft.getmean(n, Signal_filteredLeft);          
+    CaseLeft = SignalLeft.getnumber(n, action, Signal_filteredLeft);
     //Right
-    Signal_filteredRight = fabs(FilterRight(emg2, emg_offsetRight));
-    mean_value_right = SignalRight.getmean(n, Signal_filteredRight)*kRight; 
-    CaseRight = SignalRight.getnumber(n, action, Signal_filteredRight, kRight);
+    Signal_filteredRight = fabs(FilterRight(emg2)*kRight);
+    mean_value_right = SignalRight.getmean(n, Signal_filteredRight); 
+    CaseRight = SignalRight.getnumber(n, action, Signal_filteredRight);
     //Mode
-    Signal_filteredMode = fabs(FilterMode(emg4, emg_offsetMode));
-    mean_value_mode = SignalMode.getmean(n, Signal_filteredMode)*kMode;
-    CaseMode = SignalMode.getnumber(n, action, Signal_filteredMode, kMode);
+    Signal_filteredMode = fabs(FilterMode(emg4)*kMode);
+    mean_value_mode = SignalMode.getmean(n, Signal_filteredMode);
+    CaseMode = SignalMode.getnumber(n, action, Signal_filteredMode);
     /*if(CaseMode >= 3){
         milli ++;
         if(milli>=150){