sdfa

Dependencies:   AnglePosition2 Encoder FastPWM MODSERIAL Movement PIDController Servo SignalNumber2 biquadFilter mbed

Fork of kinematics_controlv2 by Peter Knoben

Files at this revision

API Documentation at this revision

Comitter:
peterknoben
Date:
Wed Nov 01 11:47:03 2017 +0000
Parent:
4:e15fc329b88b
Commit message:
fsd

Changed in this revision

Movement.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r e15fc329b88b -r b4abbd3c513c Movement.lib
--- a/Movement.lib	Tue Oct 31 14:35:52 2017 +0000
+++ b/Movement.lib	Wed Nov 01 11:47:03 2017 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/users/peterknoben/code/Movement/#9c61094ffaac
+https://os.mbed.com/users/peterknoben/code/Movement/#03e2651b4a24
diff -r e15fc329b88b -r b4abbd3c513c main.cpp
--- 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);
+    }
 }