repurposed void update_encoder copy
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of EMG_controlled_Inv_Kin_PID_Control by
Diff: main.cpp
- Revision:
- 1:078e96685ed3
- Parent:
- 0:e03285f8a410
- Child:
- 2:6523e21391e5
diff -r e03285f8a410 -r 078e96685ed3 main.cpp --- 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);