Willem Hoitzing
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);