Arnoud Meutstege
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
diff -r d4f6d9400f53 -r 88d1ccba9200 main.cpp
--- 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
}