first publish not working
Dependencies: MODSERIAL mbed ttmath FastPWM Motor_with_encoder biquadFilter
Diff: main.cpp
- Revision:
- 4:5f7d1654108d
- Parent:
- 3:b353ee86230a
- Child:
- 5:a1a5b5bebd5c
diff -r b353ee86230a -r 5f7d1654108d main.cpp --- a/main.cpp Tue Oct 31 21:35:54 2017 +0000 +++ b/main.cpp Tue Oct 31 22:42:21 2017 +0000 @@ -45,7 +45,7 @@ //Start constants PID ------------------------------- const double pi = 3.1415926535897; -const double M1_TS = 0.001f; // (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; @@ -82,9 +82,9 @@ //e_der1 = 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; - + } 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){ @@ -103,6 +103,7 @@ float Get_X_Position(){ double X = potMeter1 * potmultiplier; return X; + } float Get_Y_Position(){ @@ -113,11 +114,14 @@ //-------------Get current Position-------------------START double motor1_Position(){ // has as output Theta - double pos_m1 = gainM1*Encoder1.getPosition(); // current position for theta + double pos_m1 = gainM1*Encoder1.getPosition(); // current position for theta return pos_m1; } double motor2_Position(){ //output R double pos_m2 = gainM2 *Encoder2.getPosition(); // current position for the radius; + pc.baud(115200); + pc.printf("\r x = %f",pos_m2); + return pos_m2; } //-----------------------------------------------------END @@ -136,7 +140,7 @@ float pos_M2 = motor2_Position(); // current position for the radius 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, 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; @@ -144,7 +148,8 @@ pc.baud(115200); - pc.printf("\r DesPosition(X,Y):(%f,%f), pos Error(dTheta, dError):(%f,%f)\n",x,y, delta1 ,delta2); + //pc.printf("\r DesPosition(X,Y):(%f,%f), pos Error(dTheta, dError):(%f,%f)\n",x,y, dTheta ,dRadius); + //pc.printf("\r PID1 result: %f\n", delta1); //motor1PWM = motor1; //motor2PWM = motor2;