PID controller voor 1 motor die een hoek van 20 graden draait, niet werkend.
Dependencies: MODSERIAL QEI mbed biquadFilter
Inverse Kinematics + PID Controller
Diff: main.cpp
- Revision:
- 1:e5d2d638eaf4
- Parent:
- 0:26ce65a63616
- Child:
- 2:0a976fb06ff8
--- a/main.cpp Fri Oct 14 09:26:14 2016 +0000 +++ b/main.cpp Fri Oct 14 09:29:28 2016 +0000 @@ -45,7 +45,7 @@ const double M1_Kp = 4.348, M1_Ki = 36.632, M1_Kd = 0.126; double M1_err_int = 0, n1_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 M2_f_v2 = 0, m2_f_v2 = 0; +double M1_f_v1 = 0, M1_f_v2 = 0; //Biquad filter @@ -82,7 +82,7 @@ void M1_controller(){ double reference = pwm_M1; - q1 = wheel_M1.getPulses() / (1334.355/3); + q1 = wheel_M1.getPulses() / (1334.355/3); // 1334.355/3 counts/rad double velocity = (q1 - q1_prev) / 0.002; q1_prev = q1; pwm_M1 = PID(reference - velocity, 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);