first publish not working
Dependencies: MODSERIAL mbed ttmath FastPWM Motor_with_encoder biquadFilter
Diff: main.cpp
- Revision:
- 7:88d1ccba9200
- Parent:
- 6:d4f6d9400f53
- Child:
- 8:b932f8b71d3a
--- a/main.cpp Tue Oct 31 23:10:30 2017 +0000 +++ b/main.cpp Wed Nov 01 00:01:58 2017 +0000 @@ -13,8 +13,8 @@ DigitalOut ledg(LED_GREEN); DigitalOut motor1DC(D7); DigitalOut motor2DC(D4); -FastPWM motor1PWM(D6); -FastPWM motor2PWM(D5); +PwmOut motor1PWM(D6); +PwmOut motor2PWM(D5); AnalogIn potMeter1(A0); AnalogIn potMeter2(A1); @@ -38,35 +38,53 @@ //Start constants PID ------------------------------- const double pi = 3.1415926535897; -const double M1_TS = 0.01; // (was 0.0001) 0.001 and 0.01 work without biquad filter. // // Sample time (motor - timestep) +const double M1_TS = 0.001; // (was 0.0001) 0.001 and 0.01 work without biquad filter. // // Sample time (motor - timestep) //verplaatst const float RAD_PER_PULSE = (2*pi)/4200; -const float CONTROLLER_TS = 0.01; //TIME INTERVAL/ hZ -const float M1_KP = 10, M1_KI = 0.5, M1_KD = 0.5; //was KP=10 KI=0.5 KD=0.5 -double m1_err_int = 0, m1_prev_err = 0 ; -const double M1_F_A1 = 1.0 , M1_F_A2 = 2.0 , M1_F_B0 = 1.0 , M1_F_B1 = 3.0 , M1_F_B2 = 4.0 ; -double m1_f_v1 = 0 , m1_f_v2 = 0 ; +const float CONTROLLER_TS = 0.001; //TIME INTERVAL/ hZ +const float M1_KP = 10; +const float M1_KI = 0.5; +const float M1_KD = 0.2; //was KP=10 KI=0.5 KD=0.5 +double m1_err_int = 0; +double m1_prev_err = 0; + +const float M2_KP = 10; +const float M2_KI = 0.5; +const float M2_KD = 0.5; //was KP=10 KI=0.5 KD=0.5 +double m2_err_int = 0; +double m2_prev_err = 0; + + +// --- const biquad filter--------------------------- +const double M1_F_A1 = 1.0; +const double M1_F_A2 = 2.0; +const double M1_F_B0 = 1.0; +const double M1_F_B1 = 3.0; +const double M1_F_B2 = 4.0; +double m1_f_v1 = 0; +double m1_f_v2 = 0; //---------------------------------End of constants PID //-----------------Start PID part----------------------------START -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){ - -// Derivative -double e_der = (e - e_prev)/Ts; // Ts = motor1-timestep - -// biquad part, see slide -//e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2); +double PID1(double e1, const double Kp1, const double Ki1, const double Kd1, double Ts, double &e_int1, double &e_prev1, 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){ -e_prev = e; - -// Integral -e_int += Ts*e; - - -//PID -return Kp*e + Ki*e_int + Kd * e_der; + double e_der1 = (e1 - e_prev1)/Ts; // Ts = motor1-timestep // Derivative + // biquad part, see slide + //e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2); + e_prev1 = e1; + e_int1 += Ts*e1; // Integral + return Kp1*e1 + Ki1*e_int1 + Kd1 * e_der1; //PID +} +double PID2(double e2, const double Kp2, const double Ki2, const double Kd2, double Ts, double &e_int2, double &e_prev2, 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 e_der2 = (e2 - e_prev2)/Ts; // Ts = motor1-timestep // Derivative + // biquad part, see slide + //e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2); + e_prev2 = e2; + e_int2 += Ts*e2; // Integral + return Kp2*e2 + Ki2*e_int2 + Kd2 * e_der2; //PID } //------------Get reference position-----------------START @@ -105,8 +123,8 @@ float pos_M1 = motor1_Position(); // current position for theta float pos_M2 = motor2_Position(); // current position for the radius - double delta1 = PID(reference_motor1 - pos_M1, 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 delta2 = PID(reference_motor2 - pos_M2, 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 delta1 = PID1(reference_motor1 - pos_M1, 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 delta2 = PID2(reference_motor2 - pos_M2, M2_KP, M2_KI, M2_KD, M1_TS, m2_err_int, m2_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2); double dTheta = reference_motor1 - pos_M1; double dRadius = reference_motor2 - pos_M2; @@ -117,14 +135,14 @@ //motor1PWM = motor1; //motor2PWM = motor2; - if(delta1 > 10.0){ + if(delta1 > 5.0){ motor1DC = 0; ledr = 1; ledg = 1; //Blau ledb = 0; } - else if (delta1< -10.0) { + else if (delta1< -5.0) { motor1DC = 1; ledb = 1; @@ -148,24 +166,24 @@ if(delta2 > 10.0){ motor2DC = 0; - ledr = 1; - ledg = 1; //Blau - ledb = 0; + //ledr = 1; + //ledg = 1; //Blau + //ledb = 0; } else if (delta2<-10.0) { motor2DC = 1; - ledb = 1; - ledr = 1; - ledg = 0; //Groen + //ledb = 1; + //ledr = 1; + //ledg = 0; //Groen } else{ motor2PWM = 0; - ledb = 1; //Rood - ledr = 0; - ledg = 1; + //ledb = 1; //Rood + //ledr = 0; + //ledg = 1; } motor2 = abs(delta2)/1000.0f; @@ -176,7 +194,7 @@ motor1PWM = motor1 + 0.50f; motor2PWM = motor1 + 0.50f; - //pc.printf("\r motorvalues (M1,M2):(%f,%f), error:( \n", , motor1PWM, motor2PWM); + //pc.printf("\r motorvalues (M1,M2):(%f,%f) \n", motor1PWM, motor2PWM); //pc.printf("\r }