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:
- 8:008a7bf80fa0
- Parent:
- 7:1444604f10d4
- Child:
- 9:334b1596637b
diff -r 1444604f10d4 -r 008a7bf80fa0 main.cpp --- a/main.cpp Tue Oct 25 13:03:24 2016 +0000 +++ b/main.cpp Wed Oct 26 07:59:23 2016 +0000 @@ -37,8 +37,8 @@ volatile bool translatie_richting = true; //true is verticaal, false is horizontaal const double TS = 0.02; -const double M1_Kp = 0.5, M1_Ki = 0.00, M1_Kd = 0.1; // Waardes vinden? -const double M2_Kp = 0.5, M2_Ki = 0.00, M2_Kd = 0.1; +const double M1_Kp = 0.5, M1_Ki = 0.00, M1_Kd = 0; // Waardes vinden? +const double M2_Kp = 0.5, M2_Ki = 0.00, M2_Kd = 0; const double N = 0.1; Ticker begin_waarde_q; @@ -148,8 +148,9 @@ q1_v = J_1 * vx + J_2 * vy; q2_v = J_3 * vx + J_4 * vy; - q1_ref = q1_ref + q1_v*TS + q1_begin; - q2_ref = q2_ref + q2_v*TS + q2_begin; + q1_ref = q1 + q1_v*TS; + q2_ref = q2 + q2_v*TS; + if ( (q1 > (90*2*3.1415/360)) && (q1_v > 0 ) ) { // WAARDES VINDEN 0.8726 (50 graden) q1_v = 0; q2_v = 0; @@ -192,6 +193,11 @@ dir_M1 = 0; } pwm_M1 = abs(ctrlOutput_M1); + if (pwm_M1 < 0) { + pwm_M1 = 0; + //} else { + // pwm_M1 = pwm_M1 + 0.1; + } } void M2_controller() @@ -204,6 +210,11 @@ dir_M2 = 0; } pwm_M2 = abs(ctrlOutput_M2); + if (pwm_M2 < 0) { + pwm_M2 = 0; + //} else { + // pwm_M2 = pwm_M2 + 0.1; + } } int main() @@ -224,6 +235,9 @@ PIDcontrol_M2.attach(&flag_M2_controller, TS); begin_waarde_q.attach(&begin_waarde, 3); + pc.printf("pidf_M1 is %s", pidf_M1.stable() ? "stable" : "unstable"); + pc.printf("pidf_M2 is %s", pidf_M2.stable() ? "stable" : "unstable"); + while(1) { // initialize function