Diff: main.cpp

Revision:

5:b4abbd3c513c

Parent:

4:e15fc329b88b

Child:

6:edd30e11f3c7

--- a/main.cpp	Tue Oct 31 14:35:52 2017 +0000
+++ b/main.cpp	Wed Nov 01 11:47:03 2017 +0000
@@ -14,9 +14,8 @@
 //------------------------------------------------------------------------------
 //------------------------------------------------------------------------------
 MODSERIAL pc(USBTX, USBRX);         //Establish connection
-Ticker MyControllerTicker1;         //Declare Ticker Motor 1
-Ticker MyControllerTicker2;         //Declare Ticker Motor 2
-Ticker MySampleTicker;              //Declare Ticker HIDscope
+Ticker MyControllerTicker;          //Declare Ticker Motor 1
+Ticker MyTickerMode;                //Declare Ticker Motor 2
 Ticker MyTickerMean;                //Declare Ticker Signalprocessing
 
 InterruptIn But2(PTA4);             //Declare button for min calibration
@@ -26,16 +25,18 @@
 PIDControl PID;                     //Declare PID Controller
 SignalNumber SignalLeft;            //Declare Signal determiner for Left arm
 SignalNumber SignalRight;           //Declare Signal determiner for Right arm
+SignalNumber SignalMode;
 Movement MoveLeft;                  //Declare Movement determiner
 Movement MoveRight;
 
 AnalogIn    emg0( A0 );             //Set Inputpin for EMG 0 signal Left 
-AnalogIn    emg1( A1 );             //Set Inputpin for EMG 1 signal Left
+//AnalogIn    emg1( A1 );             //Set Inputpin for EMG 1 signal Left
 AnalogIn    emg2( A2 );             //Set Inputpin for EMG 2 signal Right
-AnalogIn    emg3( A3 );             //Set Inputpin for EMG 3 signal Right        
+//AnalogIn    emg3( A3 );             //Set Inputpin for EMG 3 signal Right        
 AnalogIn    emg4( A4 );             //Set Inputpin for EMG 4 signal Mode
-AnalogIn    emg5( A5 );             //Set Inputpin for EMG 5 signal Mode
-DigitalOut     M( D9 );             //Set digital in for mode selection
+//AnalogIn    emg5( A5 );             //Set Inputpin for EMG 5 signal Mode
+DigitalOut     Up( D9 );             //Set digital in for mode selection
+DigitalOut   Down( D8 );
 DigitalOut  Led_red(LED_RED);          
 DigitalOut  Led_green(LED_GREEN);
 DigitalOut  Led_blue(LED_BLUE);  
@@ -43,31 +44,6 @@
 const float CONTROLLER_TS = 0.02;   //Motor frequency
 const float MEAN_TS = 0.002;        //Filter frequency
 
-//Testing methods
-/*
-AnalogIn potmeter1(A5);
-AnalogIn potmeter5(A3);             //Set Inputpin for x axis
-AnalogIn potmeter2(A4);             //Set Inputpin for y axis
-*/
-
-//------------------------------------------------------------------------------
-//---------------------------Mode selection-------------------------------------
-//------------------------------------------------------------------------------
-// From the other Mbed there will be send a signal to determine in which mode the system is in
-int mode =0;
-
-//Recieving mode selection from Mbed 1
-void mode_selection(){
-    if(mode ==6){
-        mode=1;   
-    }
-    else{
-        mode++;
-    }
-    pc.printf("mode = %i\r\n", mode);
-}
-
-
 
 //------------------------------------------------------------------------------
 //-----------------------------EMG Signals--------------------------------------
@@ -76,16 +52,12 @@
 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
-int CaseLeft;                           //Strength of the muscles Left
-int CaseRight;                           //Strength of the muscles Right
-float emg_offsetLeft;                   //Calibtarion value to get zero
-float emg_offsetmaxLeft;                //Calibration value to scale to 1
-float emg_offsetRight;                   //Calibtarion value to get zero
-float emg_offsetmaxRight;                //Calibration value to scale to 1
-float meanxL;                        //Temporary variable for mean value
-float meanxR;
-float kLeft;                      //Scaling factor mean value
-float kRight;                      //Scaling factor mean value
+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
 
 //BiQuad filter variables
 BiQuad LP1( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad
@@ -93,54 +65,110 @@
 BiQuad NO3( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923,  0.41279762014290533); //Notch filter Biquad 
 BiQuadChain BiQuad_filter;
 
+
+//Calibration-------------------------------------------------------------------
 void setled(){
-    Led_red=0;
-    Led_green=1;
-    Led_blue=1;
+    Led_red=0; Led_green=1; Led_blue=1;
+}
+// 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);
+    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;
+    kLeft = 1/emg_offsetmaxLeft;
+    kRight = 1/emg_offsetmaxRight;
+    kMode = 1/emg_offsetmaxMode;
+    pc.printf("offsets: %f %f, scale %f %f \r\n", emg_offsetmaxLeft, emg_offsetmaxRight, kLeft, kRight);
+    Led_red=1;   //Set led to Green
 }
 
-// Calibration function
-void mincalibration(){
-    pc.printf("start cali \r\n");
-    emg_offsetLeft = SignalLeft.calibrate(m,((emg0+emg1)/2));
-    emg_offsetRight = SignalRight.calibrate(m,((emg2+emg3)/2));
-//    pc.printf("calibrated, offset = %f \r\n", emg_offset);
-    Led_green=0;
-    Led_red=0;
-}
-
-void maxcalibration(){
-    pc.printf("start cali max\r\n");
-    emg_offsetmaxLeft = SignalLeft.calibrate(m,((emg0+emg1)/2))-emg_offsetLeft;
-    emg_offsetmaxRight = SignalRight.calibrate(m,((emg2+emg3)/2))-emg_offsetRight;
-    kLeft = 1/emg_offsetmaxLeft;
-    kRight = 1/emg_offsetmaxRight;
-//    pc.printf("calibrated, offset = %f scale = %f \r\n",emg_offsetmax, k);
-    Led_red=1;
-}
-
+//Filtering the signals---------------------------------------------------------
 //Filter de EMG signals with a BiQuad filter
-float Filter(float input0, float input1, float offset){
-    float Signal=input0-offset; //((input0+input1)/2)
+float Filter(float input, float offset){
+    float Signal=input-offset;                    //((input0+input1)/2)
     float Signal_filtered= BiQuad_filter.step(Signal);
     return Signal_filtered;
 }
 
 
-//Determine the signalnumbers (i.e. speed) based on the EMG signals
+//------------------------------------------------------------------------------
+//---------------------------------Servo----------------------------------------
+//------------------------------------------------------------------------------
+void servo(){
+    float Signal_filteredLeft = fabs(Filter(emg0, emg_offsetLeft));
+    float Signal_filteredRight = fabs(Filter(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;
+    }
+    else if (CaseRight >=3){
+        Down = 1;
+        Down = 0;
+    }
+}
+int milli =0;
+
+//------------------------------------------------------------------------------
+//---------------------------Mode selection-------------------------------------
+//------------------------------------------------------------------------------
+int mode =0;
+
+//Recieving mode selection from EMG mode signal
+void mode_selection(){
+    if(mode ==6){
+        mode=1;
+    }
+    else{
+        mode++;
+    }
+    if (mode==3||mode==6){
+        servo();
+    }
+    pc.printf("\r\n mode = %i \r\n", mode); 
+}
+
+// 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, emg1, emg_offsetLeft));
-    meanxL = SignalLeft.getmean(n, Signal_filteredLeft)*kLeft;          //Testing variable
+    float Signal_filteredLeft = fabs(Filter(emg0, emg_offsetLeft));
+    mean_value_left = SignalLeft.getmean(n, Signal_filteredLeft)*kLeft;          
     CaseLeft = SignalLeft.getnumber(n, action, Signal_filteredLeft, kLeft);
-    pc.printf("m %f C %i \r\n",meanxL, CaseLeft);                       //Testing print
     //Right
-    float Signal_filteredRight = fabs(Filter(emg2, emg3, emg_offsetRight));
-    meanxR = SignalRight.getmean(n, Signal_filteredRight)*kRight;          //Testing variable
+    float Signal_filteredRight = fabs(Filter(emg2, emg_offsetRight));
+    mean_value_right = SignalRight.getmean(n, Signal_filteredRight)*kRight; 
     CaseRight = SignalRight.getnumber(n, action, Signal_filteredRight, kRight);
-    pc.printf("m %f C %i \r\n",meanxR, CaseRight);                       //Testing print
+    //Mode
+    float Signal_filteredMode = fabs(Filter(emg4, emg_offsetMode));
+    mean_value_mode = SignalMode.getmean(n, Signal_filteredMode)*kMode;
+    CaseMode = SignalMode.getnumber(n, action, Signal_filteredMode, kMode);
+    if(CaseMode >= 3){
+        milli ++;
+        if(milli>=150){
+            mode_selection();
+            milli=0;
+        }
+    }
+    else{
+        milli=0;
+    }
 }
 
+
+
 //------------------------------------------------------------------------------
 //-------------------------Kinematic Constants----------------------------------
 //------------------------------------------------------------------------------
@@ -156,6 +184,7 @@
 const float L2 = 490.0;                             //Length of the second body
 
 
+
 //------------------------------------------------------------------------------
 //--------------------------------Motor1----------------------------------------
 //------------------------------------------------------------------------------
@@ -167,16 +196,12 @@
 const float MOTOR1_KP = 40.0;
 const float MOTOR1_KI = 0.0;
 const float MOTOR1_KD = 15.0;
-double M1_v1 = 0.0;
-double M1_v2 = 0.0;
+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]={};
 
 void motor1_control(){
-    float *position_math;
-    position_math[0]= MoveLeft.getposition(CaseLeft, mode, 0, max_rangex);
-    position_math[1]= MoveRight.getposition(CaseRight, mode, 1, max_rangey);
     float reference_alpha = Angle.getbeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2, position_math[0], position_math[1]);
     float position_alpha = RAD_PER_PULSE * encoder1.getPosition();
     float error_alpha = reference_alpha-position_alpha;
@@ -202,17 +227,11 @@
 const float MOTOR2_KP = 60.0;
 const float MOTOR2_KI = 0.0;
 const float MOTOR2_KD = 15.0;
-double m2_err_int = 0;
+double M2_v1 = 0.0, M2_v2 = 0.0; //Calculation values
 const double motor2_gain = 2*PI;
 const float M2_N = 0.5;
-double M2_v1 = 0.0;
-double M2_v2 = 0.0;
-
 
 void motor2_control(){
-    float *position_math;
-    position_math[0]= MoveLeft.getposition(CaseLeft, mode, 0, max_rangex);
-    position_math[1]= MoveRight.getposition(CaseRight, mode, 1, max_rangey);
     float reference_beta = Angle.getalpha(max_rangex, max_rangey, x_offset, y_offset, alpha_offset, L1, L2, position_math[0], position_math[1]);
     float position_beta = RAD_PER_PULSE * -encoder2.getPosition();
     float error_beta = reference_beta-position_beta;
@@ -226,6 +245,13 @@
         motor2DirectionPin = 0;
     }
 }
+
+void motor_control(){
+    position_math[0]= MoveLeft.getposition(CaseLeft, mode, 0, max_rangex);
+    position_math[1]= MoveRight.getposition(CaseRight, mode, 1, max_rangey);
+    motor1_control();
+    motor2_control();
+}
 //------------------------------------------------------------------------------
 //------------------------------------------------------------------------------
 //------------------------------------------------------------------------------
@@ -236,14 +262,18 @@
     BiQuad_filter.add( &LP1 ).add( &HP2 ).add( &NO3);
     But2.rise(&mincalibration);
     But1.rise(&maxcalibration);
-//    M.rise(&mode_selection);
-    motor1.period(0.0001f);
-    motor2.period(0.0001f);
-    MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS); 
-    MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS);
+    motor1.period(0.0001f); motor2.period(0.0001f);
+    MyControllerTicker.attach(&motor_control, CONTROLLER_TS); 
     MyTickerMean.attach(&signalnumber, MEAN_TS);
+//    MyTickerMode.attach(&signalmode, MEAN_TS);
+//    MyTickerMean.attach(&signalnumberright, MEAN_TS);
+//    MyTickerMean.attach(&signalmode,MEAN_TS);
 
-    while(1) {}   
+    while(1) {
+        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);
+        wait(0.1f);
+    }
 }