first publish not working
Dependencies: MODSERIAL mbed ttmath FastPWM Motor_with_encoder biquadFilter
Diff: main.cpp
- Revision:
- 8:b932f8b71d3a
- Parent:
- 7:88d1ccba9200
- Child:
- 9:edf01d06935e
diff -r 88d1ccba9200 -r b932f8b71d3a main.cpp --- a/main.cpp Wed Nov 01 00:01:58 2017 +0000 +++ b/main.cpp Wed Nov 01 00:26:33 2017 +0000 @@ -13,8 +13,8 @@ DigitalOut ledg(LED_GREEN); DigitalOut motor1DC(D7); DigitalOut motor2DC(D4); -PwmOut motor1PWM(D6); -PwmOut motor2PWM(D5); +FastPWM motor1PWM(D6); +FastPWM motor2PWM(D5); AnalogIn potMeter1(A0); AnalogIn potMeter2(A1); @@ -38,28 +38,28 @@ //Start constants PID ------------------------------- const double pi = 3.1415926535897; -const double M1_TS = 0.001; // (was 0.0001) 0.001 and 0.01 work without biquad filter. // // Sample time (motor - timestep) +const double M1_TS = 0.01; // (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.001; //TIME INTERVAL/ hZ -const float M1_KP = 10; +const float CONTROLLER_TS = 0.01; //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 +const float M1_KD = 0.5; //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_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--------------------------- +// Constants Biquad 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_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; @@ -69,22 +69,24 @@ //-----------------Start PID part----------------------------START 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){ - double e_der1 = (e1 - e_prev1)/Ts; // Ts = motor1-timestep // Derivative + 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 + 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 + 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 + e_int2 += Ts*e2; // Integral + return Kp2*e2 + Ki2*e_int2 + Kd2 * e_der2; //PID } //------------Get reference position-----------------START @@ -135,14 +137,14 @@ //motor1PWM = motor1; //motor2PWM = motor2; - if(delta1 > 5.0){ + if(delta1 > 10.0){ motor1DC = 0; ledr = 1; ledg = 1; //Blau ledb = 0; } - else if (delta1< -5.0) { + else if (delta1< -10.0) { motor1DC = 1; ledb = 1; @@ -166,35 +168,35 @@ 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; - if(motor1 >= 0.50f) { - motor1 = 0.50f; + if(motor2 >= 0.50f) { + motor2 = 0.50f; } motor1PWM = motor1 + 0.50f; - motor2PWM = motor1 + 0.50f; + motor2PWM = motor2 + 0.50f; - //pc.printf("\r motorvalues (M1,M2):(%f,%f) \n", motor1PWM, motor2PWM); + //pc.printf("\r motorvalues (M1,M2):(%f,%f), error:( \n", , motor1PWM, motor2PWM); //pc.printf("\r }