Werkend aansturingsscript voor 2 motoren, incl werkende program switch. Motoren oscilleren nog iets. Vuur mechanisme ontbreekt nog.
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of frdm_Motor_V2_3 by
Diff: main.cpp
- Revision:
- 28:2eb768b9cb3b
- Parent:
- 26:58c3d24b29d2
- Child:
- 29:0f1c25496bdc
--- a/main.cpp Wed Oct 14 10:27:54 2015 +0000 +++ b/main.cpp Mon Oct 19 09:22:05 2015 +0000 @@ -12,10 +12,13 @@ //--------------------------------------------------------------------------------------------------------------------------// // Constanten/Inputs/Outputs //--------------------------------------------------------------------------------------------------------------------------// - MODSERIAL pc(USBTX, USBRX); // To/From PC - QEI Encoder2(D3, D2, NC, 32); // Encoder Motor 2 - QEI Encoder1(D13,D12,NC, 32); // Encoder Motor 1 - HIDScope scope(4); // Scope, 4 channels + MODSERIAL pc(USBTX, USBRX); // To/From PC + QEI Encoder2(D3, D2, NC, 32); // Encoder Motor 2 + QEI Encoder1(D13,D12,NC, 32); // Encoder Motor 1 + HIDScope scope(4); // Scope, 4 channels + AnalogIn EMG_left(A0); // EMG Analog input LEFT + AnalogIn EMG_right(A1); // EMG Analog input RIGHT + // LEDs DigitalOut LedR(LED_RED); @@ -23,15 +26,22 @@ DigitalOut LedB(LED_BLUE); // Motor - DigitalOut motor1direction(D7); // Motor 1, Direction & Speed - PwmOut motor1speed(D6); - DigitalOut motor2direction(D4); // Motor 2, Direction & Speed - PwmOut motor2speed(D5); + DigitalOut motor1direction(D7); // Motor 1, Direction & Speed + PwmOut motor1speed(D6); + DigitalOut motor2direction(D4); // Motor 2, Direction & Speed + PwmOut motor2speed(D5); // Tickers Ticker ScopeTime; Ticker myControllerTicker2; Ticker myControllerTicker1; + // EMG Tickers + Ticker SampleEMGLeft; + Ticker SampleEMGRight; + Ticker ScopeTimer; + Ticker serial; + Ticker MovingAverageLeft; + Ticker MovingAverageRight; // Constants double reference2, reference1; @@ -45,19 +55,45 @@ const double m2_Ts = 0.01, m1_Ts = 0.01; //Controller gain Motor 2 & 1 - const double m2_Kp = 5,m2_Ki = 0.05, m2_Kd = 5; - const double m1_Kp = 5,m1_Ki = 0, m1_Kd = 1; + const double m2_Kp = 0.5, m2_Ki = 0.005, m2_Kd = 0.5; + const double m1_Kp = 0.5,m1_Ki = 0.005, m1_Kd = 0.5; double m2_err_int = 0, m2_prev_err = 0; double m1_err_int = 0, m1_prev_err = 0; -//Derivative filter coeffs Motor 2 & 1 - const double BiGain2 = 0.010681; - const double m2_f_a1 = -1.78114551442*BiGain2, m2_f_a2 = 0.79497571585*BiGain2, m2_f_b0 = 1.0*BiGain2, m2_f_b1 = -0.64542853817*BiGain2, m2_f_b2 = 1.0*BiGain2; - const double m1_f_a1 = m2_f_a1, m1_f_a2 = m2_f_a2, m1_f_b0 = m2_f_b0, m1_f_b1 = m2_f_b1, m1_f_b2 = m2_f_b2; +// Declaring variables EMG + double EMG_L_f_v1 = 0, EMG_L_f_v2 = 0; + double EMG_L_fh=0; + double EMG_left_value; + double EMG_left_f1; + double EMG_left_f2; + double EMG_left_f3; + double EMG_left_abs; -// Filter variables - double m2_f_v1 = 0, m2_f_v2 = 0; - double m1_f_v1 = 0, m1_f_v2 = 0; + double EMG_right_value; + double EMG_right_f1; + double EMG_right_f2; + double EMG_right_f3; + double EMG_right_abs; + double Threshold1; + double Threshold2; + double Threshold3; + double Threshold4; + + int N = 50; + double MAF_left[50]; + double EMG_left_MAF; + double MAF_right[50]; + double EMG_right_MAF; + +// Filtercoëfficiënten EMG + const double BiGainEMG_H1 = 0.796821; + const double EMGH1_a1 = -1.47500228332, EMGH1_a2 = 0.55273994299, EMGH1_b0 = 1.0*BiGainEMG_H1, EMGH1_b1 = -1.99922446977*BiGainEMG_H1, EMGH1_b2 = 1.0*BiGainEMG_H1; //coefficients for high-pass filter + + const double BiGainEMG_L1= 0.001041; + const double EMGL1_a1 = -1.87506717001, EMGL1_a2 = 0.87923101463, EMGL1_b0 = 1.0*BiGainEMG_L1, EMGL1_b1 = 2.00000000000*BiGainEMG_L1, EMGL1_b2 = 1.0*BiGainEMG_L1; // coefficients for low-pass filter + + const double BiGainEMG_N1 = 1.0; + const double EMGN1_a1 = -1.58174308681, EMGN1_a2 = 0.96540248979, EMGN1_b0 = 1.0*BiGainEMG_N1, EMGN1_b1 = -1.61816176147*BiGainEMG_N1, EMGN1_b2 = 1.0*BiGainEMG_N1; //coefficients for notch filter //--------------------------------------------------------------------------------------------------------------------------// // General Functions @@ -68,29 +104,17 @@ { scope.set(0, reference2 - position2); scope.set(1, position2); - scope.set(2, reference1 - position1); - scope.set(3, position1); + scope.set(2, EMG_left_MAF); + scope.set(3, EMG_left); scope.send(); } -// Biquad filter - double biquad( double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2 ) - { - double v = u - a1*v1 - a2*v2; - double y = b0*v + b1*v1 + b2*v2; - v2 = v1; v1 = v; - return y; - } - - // Reusable PID controller - double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1, double &f_v2, - const double f_a1,const double f_a2, const double f_b0, const double f_b1, const double f_b2) + double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev) { // Derivative double e_der = (e-e_prev)/Ts; - e_der = biquad(e_der,f_v1,f_v2,f_a1,f_a2,f_b0,f_b1,f_b2); e_prev = e; // Integral e_int = e_int + Ts*e; @@ -98,6 +122,55 @@ return Kp * e + Ki*e_int + Kd*e_der; } //--------------------------------------------------------------------------------------------------------------------------// +// EMG filtering function +//--------------------------------------------------------------------------------------------------------------------------// + +// Filter creation + biquadFilter EMG_highpass1 (EMGH1_a1, EMGH1_a2, EMGH1_b0, EMGH1_b1, EMGH1_b2); // creates the high pass filter + biquadFilter EMG_lowpass1 (EMGL1_a1, EMGL1_a2, EMGL1_b0, EMGL1_b1, EMGL1_b2); // creates the low pass filter + biquadFilter EMG_notch1 (EMGN1_a1, EMGN1_a2, EMGN1_b0, EMGN1_b1, EMGN1_b2); // creates the notch filter + + biquadFilter EMG_highpass1R (EMGH1_a1, EMGH1_a2, EMGH1_b0, EMGH1_b1, EMGH1_b2); // creates the high pass filter + biquadFilter EMG_lowpass1R (EMGL1_a1, EMGL1_a2, EMGL1_b0, EMGL1_b1, EMGL1_b2); // creates the low pass filter + biquadFilter EMG_notch1R (EMGN1_a1, EMGN1_a2, EMGN1_b0, EMGN1_b1, EMGN1_b2); // creates the notch filter + +void EMGfilterLeft() +{ + EMG_left_value = EMG_left.read(); + EMG_left_f1 = EMG_highpass1.step(EMG_left_value); + EMG_left_abs = fabs(EMG_left_f1); + EMG_left_f2 = EMG_lowpass1.step(EMG_left_abs); + EMG_left_f3 = EMG_notch1.step(EMG_left_f2); + +} + +void EMGfilterRight() +{ + EMG_right_value = EMG_right.read(); + EMG_right_f1 = EMG_highpass1R.step(EMG_right_value); + EMG_right_abs = fabs(EMG_right_f1); + EMG_right_f2 = EMG_lowpass1R.step(EMG_right_abs); + EMG_right_f3 = EMG_notch1R.step(EMG_right_f2); +} + +// Movingaverage Filter + void MovingAverageFilterLeft() + { + EMG_left_MAF = (MAF_left[0]+MAF_left[1]+MAF_left[2]+MAF_left[3]+MAF_left[4]+MAF_left[5]+MAF_left[6]+MAF_left[7]+MAF_left[8]+MAF_left[9]+MAF_left[10]+MAF_left[11]+MAF_left[12]+MAF_left[13]+MAF_left[14]+MAF_left[15]+MAF_left[16]+MAF_left[17]+MAF_left[18]+MAF_left[19]+MAF_left[20]+MAF_left[21]+MAF_left[22]+MAF_left[23]+MAF_left[24]+MAF_left[25]+MAF_left[26]+MAF_left[27]+MAF_left[28]+MAF_left[29]+MAF_left[30]+MAF_left[31]+MAF_left[32]+MAF_left[33]+MAF_left[34]+MAF_left[35]+MAF_left[36]+MAF_left[37]+MAF_left[38]+MAF_left[39]+MAF_left[40]+MAF_left[41]+MAF_left[42]+MAF_left[43]+MAF_left[44]+MAF_left[45]+MAF_left[46]+MAF_left[47]+MAF_left[48]+MAF_left[49])/N; + MAF_left[49] = MAF_left[48], MAF_left[48] = MAF_left[47], MAF_left[47] = MAF_left[46], MAF_left[46] = MAF_left[45], MAF_left[45] = MAF_left[44], MAF_left[44] = MAF_left[43], MAF_left[43] = MAF_left[42], MAF_left[42] = MAF_left[41], MAF_left[41] = MAF_left[40], MAF_left[40] = MAF_left[39], MAF_left[39] = MAF_left[38], MAF_left[38] = MAF_left[37], MAF_left[37] = MAF_left[36], MAF_left[36] = MAF_left[35], MAF_left[35] = MAF_left[34], MAF_left[34] = MAF_left[33], MAF_left[33] = MAF_left[32], MAF_left[32] = MAF_left[31], MAF_left[31] = MAF_left[30], MAF_left[30] = MAF_left[29], MAF_left[29] = MAF_left[28], MAF_left[28] = MAF_left[27], MAF_left[27] = MAF_left[26], MAF_left[26] = MAF_left[25]; + MAF_left[25] = MAF_left[24], MAF_left[24] = MAF_left[23], MAF_left[23] = MAF_left[22], MAF_left[22] = MAF_left[21], MAF_left[21] = MAF_left[20], MAF_left[20] = MAF_left[19], MAF_left[19] = MAF_left[18], MAF_left[18] = MAF_left[17], MAF_left[17] = MAF_left[16], MAF_left[16] = MAF_left[15], MAF_left[15] = MAF_left[14], MAF_left[14] = MAF_left[13], MAF_left[13] = MAF_left[12], MAF_left[12] = MAF_left[11], MAF_left[11] = MAF_left[10], MAF_left[10] = MAF_left[9], MAF_left[9] = MAF_left[8], MAF_left[8] = MAF_left[7], MAF_left[7] = MAF_left[6], MAF_left[6] = MAF_left[5], MAF_left[5] = MAF_left[4], MAF_left[4] = MAF_left[3], MAF_left[3] = MAF_left[2], MAF_left[2] = MAF_left[1], MAF_left[1] = MAF_left[0]; + MAF_left[0] = EMG_left_f3; + } + + void MovingAverageFilterRight() + { + EMG_right_MAF = (MAF_right[0]+MAF_right[1]+MAF_right[2]+MAF_right[3]+MAF_right[4]+MAF_right[5]+MAF_right[6]+MAF_right[7]+MAF_right[8]+MAF_right[9]+MAF_right[10]+MAF_right[11]+MAF_right[12]+MAF_right[13]+MAF_right[14]+MAF_right[15]+MAF_right[16]+MAF_right[17]+MAF_right[18]+MAF_right[19]+MAF_right[20]+MAF_right[21]+MAF_right[22]+MAF_right[23]+MAF_right[24]+MAF_right[25]+MAF_right[26]+MAF_right[27]+MAF_right[28]+MAF_right[29]+MAF_right[30]+MAF_right[31]+MAF_right[32]+MAF_right[33]+MAF_right[34]+MAF_right[35]+MAF_right[36]+MAF_right[37]+MAF_right[38]+MAF_right[39]+MAF_right[40]+MAF_right[41]+MAF_right[42]+MAF_right[43]+MAF_right[44]+MAF_right[45]+MAF_right[46]+MAF_right[47]+MAF_right[48]+MAF_right[49])/N; + MAF_right[49] = MAF_right[48], MAF_right[48] = MAF_right[47], MAF_right[47] = MAF_right[46], MAF_right[46] = MAF_right[45], MAF_right[45] = MAF_right[44], MAF_right[44] = MAF_right[43], MAF_right[43] = MAF_right[42], MAF_right[42] = MAF_right[41], MAF_right[41] = MAF_right[40], MAF_right[40] = MAF_right[39], MAF_right[39] = MAF_right[38], MAF_right[38] = MAF_right[37], MAF_right[37] = MAF_right[36], MAF_right[36] = MAF_right[35], MAF_right[35] = MAF_right[34], MAF_right[34] = MAF_right[33], MAF_right[33] = MAF_right[32], MAF_right[32] = MAF_right[31], MAF_right[31] = MAF_right[30], MAF_right[30] = MAF_right[29], MAF_right[29] = MAF_right[28], MAF_right[28] = MAF_right[27], MAF_right[27] = MAF_right[26], MAF_right[26] = MAF_right[25]; + MAF_right[25] = MAF_right[24], MAF_right[24] = MAF_right[23], MAF_right[23] = MAF_right[22], MAF_right[22] = MAF_right[21], MAF_right[21] = MAF_right[20], MAF_right[20] = MAF_right[19], MAF_right[19] = MAF_right[18], MAF_right[18] = MAF_right[17], MAF_right[17] = MAF_right[16], MAF_right[16] = MAF_right[15], MAF_right[15] = MAF_right[14], MAF_right[14] = MAF_right[13], MAF_right[13] = MAF_right[12], MAF_right[12] = MAF_right[11], MAF_right[11] = MAF_right[10], MAF_right[10] = MAF_right[9], MAF_right[9] = MAF_right[8], MAF_right[8] = MAF_right[7], MAF_right[7] = MAF_right[6], MAF_right[6] = MAF_right[5], MAF_right[5] = MAF_right[4], MAF_right[4] = MAF_right[3], MAF_right[3] = MAF_right[2], MAF_right[2] = MAF_right[1], MAF_right[1] = MAF_right[0]; + MAF_right[0] = EMG_right_f3; + } + +//--------------------------------------------------------------------------------------------------------------------------// // Motor control functions //--------------------------------------------------------------------------------------------------------------------------// @@ -108,9 +181,8 @@ reference2 = m2_ref; // Reference in degrees position2 = Encoder2.getPulses()*360/(32*131); // Position in degrees // Speed control - double m2_P1 = PID( reference2 - position2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2, - m2_f_b0, m2_f_b1, m2_f_b2); - double m2_P2 = biquad(m2_P1, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2,m2_f_b0, m2_f_b1, m2_f_b2); // Filter of motorspeed input + double m2_P1 = PID( reference2 - position2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err); + double m2_P2 = m2_P1; motor2speed = abs(m2_P2); // Direction control if(m2_P2 > 0) @@ -130,9 +202,8 @@ reference1 = m1_ref; // Reference in degrees position1 = Encoder1.getPulses()*360/(32*131); // Position in degrees // Speed control - double m1_P1 = PID( reference1 - position1, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, m1_f_a1, m1_f_a2, - m1_f_b0, m1_f_b1, m1_f_b2); - double m1_P2 = biquad(m1_P1, m1_f_v1, m1_f_v2, m1_f_a1, m1_f_a2, m1_f_b0, m1_f_b1, m1_f_b2); + double m1_P1 = PID( reference1 - position1, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int, m1_prev_err); + double m1_P2 = m1_P1; motor1speed = abs(m1_P2); // Direction control if(m1_P2 > 0) @@ -161,9 +232,37 @@ pc.printf("Tot aan loop werkt\n"); // Tickers - ScopeTime.attach(&ScopeSend, 0.01f); // 100 Hz, Scope - myControllerTicker2.attach(&motor2_Controller, 0.01f ); // 100 Hz, Motor 2 - myControllerTicker1.attach(&motor1_Controller, 0.01f ); // 100 Hz, Motor 1 + ScopeTime.attach(&ScopeSend, 0.01f); // 100 Hz, Scope + myControllerTicker2.attach(&motor2_Controller, 0.01f ); // 100 Hz, Motor 2 + myControllerTicker1.attach(&motor1_Controller, 0.01f ); // 100 Hz, Motor 1 + SampleEMGLeft.attach(&EMGfilterLeft, 0.002); // 500 Hz, EMG LEFT + SampleEMGRight.attach(&EMGfilterRight, 0.002); // 500 Hz, EMG RIGHT + MovingAverageLeft.attach(&MovingAverageFilterLeft, 0.005); // 200 Hz, MAF LEFT + MovingAverageRight.attach(&MovingAverageFilterRight, 0.005);// 200 Hz, MAF RIGHT + +//--------------------------------------------------------------------------------------------------------------------------// +// Determing Threshold +//--------------------------------------------------------------------------------------------------------------------------// + wait(20); + + LedG.write(1); + wait(0.2); + LedG.write(0); + wait(0.2); + LedG.write(1); + wait(0.2); + LedG.write(0); + wait(0.2); + LedG.write(1); + wait(0.2); + LedG.write(0); + wait(2); + Threshold1 = 0.5*EMG_left_MAF; + Threshold2 = 0.2*EMG_left_MAF; + LedG.write(1); + + pc.printf("T1 = %f, T2 = %f\n", Threshold1, Threshold2); + LedG.write(1); //--------------------------------------------------------------------------------------------------------------------------// // Control Program @@ -196,7 +295,7 @@ LedR = LedB = 1; LedG = 0; - if(c == 'r') + if(EMG_left_MAF > Threshold1) { m2_ref = m2_ref + Stapgrootte; m1_ref = m1_ref - Stapgrootte;