Diff: main.cpp

Revision:

1:078e96685ed3

Parent:

0:e03285f8a410

Child:

2:6523e21391e5

--- a/main.cpp	Fri Oct 28 10:36:03 2016 +0000
+++ b/main.cpp	Mon Oct 31 14:21:31 2016 +0000
@@ -18,7 +18,7 @@
 AnalogIn emgB(A0);
 AnalogIn emgT(A1);
 AnalogIn emgS(A2);
-HIDScope scope(3);
+HIDScope scope(4);
 
 DigitalOut ledg (LED_GREEN);
 DigitalOut ledr (LED_RED);
@@ -26,7 +26,7 @@
 InterruptIn knop_biceps(SW2);
 InterruptIn knop_triceps(SW3);
 InterruptIn knop_switch(D9);
-
+InterruptIn knop_calibrate(PTC12);
 BiQuadChain filter1b;
 BiQuadChain filter2b;
 BiQuadChain filter1t;
@@ -49,39 +49,49 @@
 BiQuad bq3s(1.0000e+00, -1.6182e+00, 1.0000e+00, -1.6143e+00 , 9.8260e-01);   // Notch + HP
 BiQuad bq4s(3.4604e-04,  6.9208e-04, 3.4604e-04, -1.9467e+00,  9.4808e-01);   // LP
 
-const float threshold_biceps = 0.07;
-const float threshold_triceps = 0.07;
-const float threshold_switch = 0.05;
+volatile double bEMG_max = 0;
+volatile double tEMG_max = 0;
+volatile double sEMG_max = 0;
+const double percentage_threshold_biceps = 0.09/0.171536;           // 0.171536 is max aanspanning
+const double percentage_threshold_triceps = 0.07/0.203654;          // 0.203654 is max aanspanning
+const double percentage_threshold_switch = 0.09/0.171536;           // gekopieerd van andere biceps
+volatile double threshold_biceps = 0;
+volatile double threshold_triceps = 0;
+volatile double threshold_switch = 0;
+volatile bool calibrate_biceps = false;
+volatile bool calibrate_triceps = false;
+volatile bool calibrate_switch = false;
+volatile bool calibration_finished = false;
 
-volatile float q1 = 0;
-volatile float q2 = 0;
-volatile float q1_begin;
-volatile float q2_begin;
-const float l1 = 0.3626;
-const float l2 = 0.420;
-volatile float q1_v;
-volatile float q2_v;
-volatile float q1_ref = 0;
-volatile float q2_ref = 0;
-volatile float q1_error = 0;
-volatile float q2_error = 0;
-volatile float q1_error_prev = 0;
-volatile float q2_error_prev = 0;
-volatile float q1DerivativeError = 0;
-volatile float q2DerivativeError = 0;
-volatile float q1IntError = 0;
-volatile float q2IntError = 0;
-volatile float TotalError1= 0;
-volatile float TotalError2= 0;
-float ctrlOutput_M1 = 0;
-float ctrlOutput_M2 = 0;
-volatile float vx;
-volatile float vy;
+volatile double q1 = 0;
+volatile double q2 = 0;
+const double l1 = 0.3626;
+const double l2 = 0.420;
+volatile double q1_v;
+volatile double q2_v;
+volatile double q1_ref = 0;
+volatile double q2_ref = 0;
+volatile double q1_ref_prev = 0;
+volatile double q2_ref_prev = 0;
+volatile double q1_error = 0;
+volatile double q2_error = 0;
+volatile double q1_error_prev = 0;
+volatile double q2_error_prev = 0;
+volatile double q1DerivativeError = 0;
+volatile double q2DerivativeError = 0;
+volatile double q1IntError = 0;
+volatile double q2IntError = 0;
+volatile double q1_total_error= 0;
+volatile double q2_total_error= 0;
+double ctrlOutput_M1 = 0;
+double ctrlOutput_M2 = 0;
+volatile double vx;
+volatile double vy;
 volatile bool translatie_richting = true;  //true is verticaal, false is horizontaal
 
-const float TS = 0.02;
-const float MotorGain_M1 = 4.3;         // bij pwm = 1 draait (losse) motor met 4.3 rad/s -> gemeten
-const float MotorGain_M2 = 4.7;         // gemeten
+const double TS = 0.02;
+const double MotorGain_M1 = 4.3;         // bij pwm = 1 draait (losse) motor met 4.3 rad/s -> gemeten
+const double MotorGain_M2 = 4.7;         // gemeten
 
 Ticker update_encoder_ticker;
 volatile bool go_flag_update_encoder = false;
@@ -94,11 +104,63 @@
 {
     //q1 = wheel_M1.getPulses()/(1334.355/2);
     //q2 = wheel_M2.getPulses()/(1334.355/2);
-    //pc.printf("q1 = %f \tq1_ref = %f \tq2 = %f \tq2_ref = %f \ttotalerr1 = %f \ttotalerr2 = %f\n\r",q1, q1_ref,q2,q2_ref,TotalError1,TotalError2);
+    //pc.printf("q1 = %f \tq1_ref = %f \tq2 = %f \tq2_ref = %f \ttotalerr1 = %f \ttotalerr2 = %f\n\r",q1, q1_ref,q2,q2_ref,q1_total_error,q2_total_error);
     pc.printf("vx = %f \tvy = %f \tq1_r = %f \tq2_r = %f \tq1 = %f \tq2 = %f \tpwm_M1 = %f \tpwm_M2 = %f\n\r",vx,vy,q1_ref,q2_ref,q1,q2,pwm_M1.read(),pwm_M2.read());
-    //pc.printf("q1_err = %0.9f \tq2_err = %0.9f \tq1IntErr = %0.9f \tq2IntErr = %0.9f \tTotErr1 = %0.9f \tTotErr2 = %0.9f\n\r",q1_error,q2_error,q1IntError,q2IntError,TotalError1,TotalError2);
+    //pc.printf("q1_err = %0.9f \tq2_err = %0.9f \tq1IntErr = %0.9f \tq2IntErr = %0.9f \tTotErr1 = %0.9f \tTotErr2 = %0.9f\n\r",q1_error,q2_error,q1IntError,q2IntError,q1_total_error,q2_total_error);
+}
+
+Ticker end_calibration_biceps_ticker;
+void end_calibration_biceps()
+{
+    ledr = 1;
+    calibrate_biceps = false;
+    end_calibration_biceps_ticker.detach();
+}
+
+Ticker end_calibration_triceps_ticker;
+void end_calibration_triceps()
+{
+    ledg = 1;
+    calibrate_triceps = false;
+    end_calibration_triceps_ticker.detach();
+}
+
+Ticker end_calibration_switch_ticker;
+void end_calibration_switch()
+{
+    ledb = 1;
+    calibrate_switch = false;
+    end_calibration_switch_ticker.detach();
+    calibration_finished = true;
 }
 
+volatile int n = 0;
+void start_calibration()
+{
+    calibration_finished = false;
+    n++;
+    if (n == 1) {
+        ledr = 0;
+        bEMG_max = 0;
+        calibrate_biceps = true;
+        end_calibration_biceps_ticker.attach(&end_calibration_biceps, 10);
+    }
+    if (n == 2) {
+        ledg = 0;
+        tEMG_max = 0;
+        calibrate_triceps = true;
+        end_calibration_triceps_ticker.attach(&end_calibration_triceps, 10);
+    }
+    if (n == 3) {
+        ledb = 0;
+        sEMG_max = 0;
+        calibrate_switch = true;
+        end_calibration_switch_ticker.attach(&end_calibration_switch, 10);
+        n = 0;
+    }    
+}
+
+
 Ticker PIDcontrol;
 volatile bool go_flag_controller = false;
 
@@ -134,12 +196,13 @@
         wait(0.005f);
     }
     pwm_M2 = 0;
-    ledg = !ledg;
     begin_hoeken();
 }
 
 void biceps()
 {
+    q1_ref_prev = 0;
+    q2_ref_prev = 0;
     q1IntError = 0;
     q2IntError = 0;
     q1_error_prev = 0;
@@ -155,6 +218,8 @@
 
 void triceps()
 {
+    q1_ref_prev = 0;
+    q2_ref_prev = 0;
     q1IntError = 0;
     q2IntError = 0;
     q1_error_prev = 0;
@@ -178,10 +243,16 @@
     } else {
         vx = 0;
         vy = 0;
+        q1_ref = q1;
+        q2_ref = q2;
+        q1_error = 0;
+        q2_error = 0;
         q1IntError = 0;
         q2IntError = 0;
         q1_error_prev = 0;
         q2_error_prev = 0;
+        q1_total_error = 0;
+        q2_total_error = 0;
     }
 
     if (translatie_richting == 1) {
@@ -210,45 +281,63 @@
 
 void emgsample()
 {
-    float bEMG_raw = emgB.read();
-    float bEMG_HPfilt = filter1b.step( bEMG_raw );
-    float bEMG_rect = abs(bEMG_HPfilt);
-    float bEMG_filt = filter2b.step(bEMG_rect);
+    double bEMG_raw = emgB.read();
+    double bEMG_HPfilt = filter1b.step( bEMG_raw );
+    double bEMG_rect = abs(bEMG_HPfilt);
+    double bEMG_filt = filter2b.step(bEMG_rect);
+
+    double tEMG_raw = emgT.read();
+    double tEMG_HPfilt = filter1t.step( tEMG_raw );
+    double tEMG_rect = abs(tEMG_HPfilt);
+    double tEMG_filt = filter2t.step(tEMG_rect);
+
+    double sEMG_raw = emgS.read();
+    double sEMG_HPfilt = filter1s.step( sEMG_raw );
+    double sEMG_rect = abs(sEMG_HPfilt);
+    double sEMG_filt = filter2s.step(sEMG_rect);
     
-    float tEMG_raw = emgT.read();
-    float tEMG_HPfilt = filter1t.step( tEMG_raw );
-    float tEMG_rect = abs(tEMG_HPfilt);
-    float tEMG_filt = filter2t.step(tEMG_rect);
+    if ((bEMG_filt > bEMG_max) && (calibrate_biceps == true) ) {
+        bEMG_max = bEMG_filt;
+        threshold_biceps = bEMG_max*percentage_threshold_biceps;
+        }
     
-    float sEMG_raw = emgS.read();
-    float sEMG_HPfilt = filter1s.step( sEMG_raw );
-    float sEMG_rect = abs(sEMG_HPfilt);
-    float sEMG_filt = filter2s.step(sEMG_rect);
+    if ((tEMG_filt > tEMG_max) && (calibrate_triceps == true) ) {
+        tEMG_max = tEMG_filt;
+        threshold_triceps = tEMG_max*percentage_threshold_triceps;
+        }
     
+    if ((sEMG_filt > sEMG_max) && (calibrate_switch == true) ) {
+        sEMG_max = sEMG_filt;
+        threshold_switch = sEMG_max*percentage_threshold_switch;
+        }
+        
     scope.set(0, bEMG_filt);
     scope.set(1, tEMG_filt);
-    scope.set(2, sEMG_filt);
+    scope.set(2, threshold_biceps);
+    scope.set(3, threshold_triceps);
     scope.send();
     
-    // motor aansturing
-    if (sEMG_filt > threshold_switch) {
-        if (switch_active == true) {
-        switcher();
-        switch_active = false;
-        switch_activate_ticker.attach(&switch_activate, 0.5f);
+    // motor aansturing, pas uitvoeren wanneer kalibratie klaar is
+    if ( calibration_finished == true ) {
+        if (sEMG_filt > threshold_switch) {
+            if (switch_active == true) {
+                switcher();
+                switch_active = false;
+                switch_activate_ticker.attach(&switch_activate, 0.5f);
+            }
+        } else if (tEMG_filt > threshold_triceps) {
+            triceps();
+        } else if (bEMG_filt > threshold_biceps) {
+            biceps();
         }
-    } else if (tEMG_filt > threshold_triceps) {
-        triceps();
-    } else if (bEMG_filt > threshold_biceps) {
-        biceps();
     }
 }
 
 Ticker update_ref_ticker;
-volatile float J_1;
-volatile float J_2;
-volatile float J_3;
-volatile float J_4;
+volatile double J_1;
+volatile double J_2;
+volatile double J_3;
+volatile double J_4;
 volatile bool go_flag_update_ref = false;
 void flag_update_ref()
 {
@@ -271,41 +360,68 @@
     if ( (q1 > (90*2*3.1415/360)) && (q1_v > 0 ) ) {                // WAARDES VINDEN 0.8726 (50 graden)
         q1_v = 0;
         q2_v = 0;
+        q1_ref = q1;
+        q2_ref = q2;
+        q1IntError = 0;
+        q2IntError = 0;
+        q1_error_prev = 0;
+        q2_error_prev = 0;
     } else if ( (q1 < -(90*2*3.1415/360)) && (q1_v < 0) ) {
         q1_v = 0;
         q2_v = 0;
+        q1_ref = q1;
+        q2_ref = q2;
+        q1IntError = 0;
+        q2IntError = 0;
+        q1_error_prev = 0;
+        q2_error_prev = 0;
     } else if ( (q2 < (-140*2*3.1415/360)) && (q2_v < 0) ) {        // WAARDES VINDEN -2.4434 (-140 graden) --> werkelijke max -2.672452
         q1_v = 0;
         q2_v = 0;
+        q1_ref = q1;
+        q2_ref = q2;
+        q1IntError = 0;
+        q2IntError = 0;
+        q1_error_prev = 0;
+        q2_error_prev = 0;
     } else if ( (q2 > 0) && (q2_v > 0) ) {
         q1_v = 0;
         q2_v = 0;
+        q1_ref = q1;
+        q2_ref = q2;
+        q1IntError = 0;
+        q2IntError = 0;
+        q1_error_prev = 0;
+        q2_error_prev = 0;
     }
 
-    q1_ref = q1 + q1_v*TS;
-    q2_ref = q2 + q2_v*TS;
+    q1_ref_prev = q1_ref;
+    q2_ref_prev = q2_ref;
+
+    q1_ref = q1_ref_prev + q1_v*TS;
+    q2_ref = q2_ref_prev + q2_v*TS;
 }
 
-void PID(float q1,float q1_ref,float q2,float q2_ref,float TS,float &ctrlOutput_M1, float &ctrlOutput_M2)
+void PID(double q1,double q1_ref,double q2,double q2_ref,double TS,double &ctrlOutput_M1, double &ctrlOutput_M2)
 {
     // linear feedback control
     q1_error = q1_ref - q1; //referencePosition1 - Position1;             // proportional angular error in radians
     q2_error = q2_ref - q2; //referencePosition1 - Position1;             // proportional angular error in radians
-    float Kp = 10;
+    double Kp = 10;
 
     q1IntError = q1IntError + q1_error*TS;             // integrated error in radians
     q2IntError = q2IntError + q2_error*TS;             // integrated error in radians
-    float Ki = 1;
+    double Ki = 1;
 
     q1DerivativeError = (q1_error - q1_error_prev)/TS;  // derivative of error in radians
     q2DerivativeError = (q2_error - q2_error_prev)/TS;  // derivative of error in radians
-    float Kd = 0;
+    double Kd = 0;
 
-    TotalError1 = (q1_error * Kp) + (q1IntError * Ki) + (q1DerivativeError * Kd);         //total controller output = motor input
-    TotalError2 = (q2_error * Kp) + (q2IntError * Ki) + (q2DerivativeError * Kd);         //total controller output = motor input
+    q1_total_error = (q1_error * Kp) + (q1IntError * Ki) + (q1DerivativeError * Kd);         //total controller output = motor input
+    q2_total_error = (q2_error * Kp) + (q2IntError * Ki) + (q2DerivativeError * Kd);         //total controller output = motor input
 
-    ctrlOutput_M1 = TotalError1/MotorGain_M1;
-    ctrlOutput_M2 = TotalError2/MotorGain_M2;
+    ctrlOutput_M1 = q1_total_error/MotorGain_M1;
+    ctrlOutput_M2 = q2_total_error/MotorGain_M2;
 
     q1_error_prev = q1_error;
     q2_error_prev = q2_error;
@@ -344,7 +460,7 @@
 {
     ledr = 1;
     ledg = 1;
-    ledb = 0;
+    ledb = 1;
     pc.baud(115200);
     wheel_M1.reset();
     wheel_M2.reset();
@@ -357,13 +473,15 @@
     knop_biceps.rise(&biceps);
     knop_triceps.rise(&triceps);
     knop_switch.rise(&switcher);
+    knop_calibrate.rise(&start_calibration);
+
+    // initialize -> beginposities
+    initialize();
 
     // flag functions/tickers
     emgticker.attach(&emgsample, 0.002f); // 500 Hz --> moet kloppen met frequentie gebruikt voor filter coefficienten
     update_encoder_ticker.attach(&flag_update_encoder, TS);
     update_ref_ticker.attach(&flag_update_ref, TS);
-    // initialize -> beginposities
-    initialize();
 
     if (active_PID_ticker == true) {
         PIDcontrol.attach(&flag_controller, TS);