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:
- 27:4d7ca91e2e64
- Parent:
- 25:ae908de29943
diff -r ae908de29943 -r 4d7ca91e2e64 main.cpp --- a/main.cpp Sat Oct 03 18:04:16 2015 +0000 +++ b/main.cpp Thu Oct 15 17:36:25 2015 +0000 @@ -45,19 +45,23 @@ 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 = 2; + const double m2_Kp = 0.1,m2_Ki = 0.001, m2_Kd = 1; const double m1_Kp = 5,m1_Ki = 0.05, m1_Kd = 2; 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.016955, BiGain1 = 0.016955; - const double m2_f_a1 = -0.96608908283*BiGain2, m2_f_a2 = 0.0*BiGain2, m2_f_b0 = 1.0*BiGain2, m2_f_b1 = 1.0*BiGain2, m2_f_b2 = 0.0*BiGain2; - const double m1_f_a1 = -0.96608908283*BiGain1, m1_f_a2 = 0.0*BiGain1, m1_f_b0 = 1.0*BiGain1, m1_f_b1 = 1.0*BiGain1, m1_f_b2 = 0.0*BiGain1; + const double BiGainm2f1 = 0.959332; + const double m2_f1_a1 = -1.55576653052, m2_f1_a2 = 0.61374320375, m2_f1_b0 = 1.0*BiGainm2f1, m2_f1_b1 = -0.90928276835*BiGainm2f1, m2_f1_b2 = 1.0*BiGainm2f1; + + +//Biquads + biquadFilter f_Motor2 (m2_f1_a1, m2_f1_a2, m2_f1_b0, m2_f1_b1, m2_f1_b2); // creates the low pass filter // Filter variables double m2_f_v1 = 0, m2_f_v2 = 0; double m1_f_v1 = 0, m1_f_v2 = 0; + double m2_f2_v1 = 0, m2_f2_v2 = 0; //--------------------------------------------------------------------------------------------------------------------------// // General Functions @@ -68,29 +72,18 @@ { scope.set(0, reference2 - position2); scope.set(1, position2); - scope.set(2, reference1 - position1); - scope.set(3, position1); + scope.set(2, reference2); + scope.set(3, position2); 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_der = f_motor2(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; @@ -108,9 +101,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 = f_Motor2.step(m2_P1);//(m2_P1, m2_f2_v1, m2_f2_v2, m2_f2_a1, m2_f2_a2,m2_f2_b0, m2_f2_b1, m2_f2_b2); // Filter of motorspeed input motor2speed = abs(m2_P2); // Direction control if(m2_P2 > 0) @@ -126,23 +118,7 @@ // Motor1 control void motor1_Controller() { - // Setpoint Motor 1 - 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); - motor1speed = abs(m1_P2); - // Direction control - if(m1_P2 > 0) - { - motor1direction = 1; - } - else - { - motor1direction = 0; - } + } //--------------------------------------------------------------------------------------------------------------------------//