Diff: main.cpp

Revision:

6:edd30e11f3c7

Parent:

5:b4abbd3c513c

Child:

7:105e3ae0fb19

diff -r b4abbd3c513c -r edd30e11f3c7 main.cpp
--- a/main.cpp	Wed Nov 01 11:47:03 2017 +0000
+++ b/main.cpp	Wed Nov 01 13:42:17 2017 +0000
@@ -20,6 +20,7 @@
 
 InterruptIn But2(PTA4);             //Declare button for min calibration
 InterruptIn But1(PTC6);             //Declare button for max calibration
+InterruptIn Mode(D7);
 
 AnglePosition Angle;                //Declare Angle calculater
 PIDControl PID;                     //Declare PID Controller
@@ -40,6 +41,7 @@
 DigitalOut  Led_red(LED_RED);          
 DigitalOut  Led_green(LED_GREEN);
 DigitalOut  Led_blue(LED_BLUE);  
+DigitalOut  test(D2);
 
 const float CONTROLLER_TS = 0.02;   //Motor frequency
 const float MEAN_TS = 0.002;        //Filter frequency
@@ -51,19 +53,28 @@
 // Filtering the signal and getting the usefull information out of it.
 const int n = 400;                  //Window size for the mean value, also adjust in signalnumber.cpp
 const int action =50;               //Number of same mean values to change the signalnumber
-const int m = 300;                  //Number of samples for calibration
+const int m = 400;                  //Number of samples for calibration
 int CaseLeft, CaseRight, CaseMode;           //Strength of the muscles 
 float emg_offsetLeft, emg_offsetmaxLeft;     //Calibration offsets from zero to one for the left arm
 float emg_offsetRight, emg_offsetmaxRight;   //Calibration offsets from zero to one for the right arm
 float emg_offsetMode, emg_offsetmaxMode;
 float mean_value_left, mean_value_right, mean_value_mode;   //Mean value of the filtered system
 float kLeft, kRight, kMode;            //Scaling factors
+float Signal_filteredLeft, Signal_filteredRight, Signal_filteredMode;
 
 //BiQuad filter variables
-BiQuad LP1( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad
-BiQuad HP2( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad
-BiQuad NO3( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923,  0.41279762014290533); //Notch filter Biquad 
-BiQuadChain BiQuad_filter;
+BiQuad LP1L( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad
+BiQuad HP2L( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad
+BiQuad NO3L( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923,  0.41279762014290533); //Notch filter Biquad 
+BiQuad LP1R( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad
+BiQuad HP2R( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad
+BiQuad NO3R( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923,  0.41279762014290533); //Notch filter Biquad 
+BiQuad LP1M( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad
+BiQuad HP2M( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad
+BiQuad NO3M( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923,  0.41279762014290533); //Notch filter Biquad 
+BiQuadChain BiQuad_filter_Left;
+BiQuadChain BiQuad_filter_Right;
+BiQuadChain BiQuad_filter_Mode;
 
 
 //Calibration-------------------------------------------------------------------
@@ -82,9 +93,9 @@
 // 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, emg0)-emg_offsetLeft;
+    emg_offsetmaxRight = SignalRight.calibrate(m, emg2)-emg_offsetRight;
+    emg_offsetmaxMode = SignalMode.calibrate(m, emg4)-emg_offsetMode;
     kLeft = 1/emg_offsetmaxLeft;
     kRight = 1/emg_offsetmaxRight;
     kMode = 1/emg_offsetmaxMode;
@@ -94,9 +105,19 @@
 
 //Filtering the signals---------------------------------------------------------
 //Filter de EMG signals with a BiQuad filter
-float Filter(float input, float offset){
+float FilterLeft(float input, float offset){
+    float Signal=(input-offset);                    //((input0+input1)/2)
+    float Signal_filtered= BiQuad_filter_Left.step(Signal);
+    return Signal_filtered;
+}
+float FilterRight(float input, float offset){
     float Signal=input-offset;                    //((input0+input1)/2)
-    float Signal_filtered= BiQuad_filter.step(Signal);
+    float Signal_filtered= BiQuad_filter_Right.step(Signal);
+    return Signal_filtered;
+}
+float FilterMode(float input, float offset){
+    float Signal=input-offset;                    //((input0+input1)/2)
+    float Signal_filtered= BiQuad_filter_Mode.step(Signal);
     return Signal_filtered;
 }
 
@@ -105,17 +126,17 @@
 //---------------------------------Servo----------------------------------------
 //------------------------------------------------------------------------------
 void servo(){
-    float Signal_filteredLeft = fabs(Filter(emg0, emg_offsetLeft));
-    float Signal_filteredRight = fabs(Filter(emg2, emg_offsetRight));
+    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);
     if (CaseLeft>=3){
-        Up = 1; 
-        Up = 0;
+        Up = 0; 
+        Up = 1;
     }
     else if (CaseRight >=3){
+        Down = 0;
         Down = 1;
-        Down = 0;
     }
 }
 int milli =0;
@@ -141,21 +162,27 @@
 
 // Control mode selection-------------------------------------------------------
 
+
+
+
+
+
+
 //Determine the signalnumbers (i.e. speed) based on the strenght of the EMG signals
 void signalnumber(){
     //Left
-    float Signal_filteredLeft = fabs(Filter(emg0, emg_offsetLeft));
+    Signal_filteredLeft = fabs(FilterLeft(emg0, emg_offsetLeft));
     mean_value_left = SignalLeft.getmean(n, Signal_filteredLeft)*kLeft;          
     CaseLeft = SignalLeft.getnumber(n, action, Signal_filteredLeft, kLeft);
     //Right
-    float Signal_filteredRight = fabs(Filter(emg2, emg_offsetRight));
+    Signal_filteredRight = fabs(FilterRight(emg2, emg_offsetRight));
     mean_value_right = SignalRight.getmean(n, Signal_filteredRight)*kRight; 
     CaseRight = SignalRight.getnumber(n, action, Signal_filteredRight, kRight);
     //Mode
-    float Signal_filteredMode = fabs(Filter(emg4, emg_offsetMode));
+    Signal_filteredMode = fabs(FilterMode(emg4, emg_offsetMode));
     mean_value_mode = SignalMode.getmean(n, Signal_filteredMode)*kMode;
     CaseMode = SignalMode.getnumber(n, action, Signal_filteredMode, kMode);
-    if(CaseMode >= 3){
+    /*if(CaseMode >= 3){
         milli ++;
         if(milli>=150){
             mode_selection();
@@ -164,11 +191,16 @@
     }
     else{
         milli=0;
-    }
+    }*/
 }
 
 
 
+
+
+
+
+
 //------------------------------------------------------------------------------
 //-------------------------Kinematic Constants----------------------------------
 //------------------------------------------------------------------------------
@@ -199,7 +231,7 @@
 double M1_v1 = 0.0, M1_v2 = 0.0; //Calculation values
 const double motor1_gain = 2*PI;
 const float M1_N = 0.5;
-static float position_math[2]={};
+float position_math[2]={};
 
 void motor1_control(){
     float reference_alpha = Angle.getbeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2, position_math[0], position_math[1]);
@@ -258,10 +290,14 @@
 
 int main(){
     pc.baud(115200);
+    test=0;
     setled(); 
-    BiQuad_filter.add( &LP1 ).add( &HP2 ).add( &NO3);
+    BiQuad_filter_Left.add( &LP1L ).add( &HP2L ).add( &NO3L);
+    BiQuad_filter_Right.add( &LP1R ).add( &HP2R ).add( &NO3R);
+    BiQuad_filter_Mode.add( &LP1M ).add( &HP2M ).add( &NO3M);
     But2.rise(&mincalibration);
     But1.rise(&maxcalibration);
+    Mode.rise(&mode_selection);
     motor1.period(0.0001f); motor2.period(0.0001f);
     MyControllerTicker.attach(&motor_control, CONTROLLER_TS); 
     MyTickerMean.attach(&signalnumber, MEAN_TS);
@@ -270,8 +306,9 @@
 //    MyTickerMean.attach(&signalmode,MEAN_TS);
 
     while(1) {
-        pc.printf("Mean %f %f %f \r\n", mean_value_left, mean_value_right, mean_value_mode);
+//        pc.printf("Mean %f %f %f \r\n", mean_value_left, mean_value_right, mean_value_mode);
 //        pc.printf("Case %i %i %i, mode = %i \r\n", CaseLeft, CaseRight, CaseMode, mode);
+        pc.printf("mean %f , case = %i \r\n", mean_value_left, CaseLeft);
         wait(0.1f);
     }
 }