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:
- 6:4d254faf2428
- Parent:
- 5:0251fde34cdc
- Child:
- 7:1444604f10d4
diff -r 0251fde34cdc -r 4d254faf2428 main.cpp --- a/main.cpp Mon Oct 24 10:45:05 2016 +0000 +++ b/main.cpp Mon Oct 24 10:50:17 2016 +0000 @@ -16,10 +16,6 @@ volatile double q1 = 0; volatile double q2 = 0; -volatile double q1_prev = 0; -volatile double q2_prev = 0; -volatile double q1_v; -volatile double q2_v; volatile bool go_flag_initialize = false; @@ -28,16 +24,16 @@ go_flag_initialize = true; } -volatile double q1_v_ref; -volatile double q2_v_ref; +volatile double q1_ref; +volatile double q2_ref; void initialize() { - q1_v_ref = 0.4; //2*3.1415 /(2*3.1415); - q2_v_ref = 0*3.1415 /(2*3.1415); + q1_ref = 0.125; //2*3.1415 /(2*3.1415); + q2_ref = 0*3.1415 /(2*3.1415); } const double TS = 0.02; -const double M1_Kp = 0.05, M1_Ki = 0.00, M1_Kd = 0; +const double M1_Kp = 1, M1_Ki = 0.00, M1_Kd = 0; const double M2_Kp = 0.3, M2_Ki = 0.00, M2_Kd = 0; const double N = 0; @@ -55,11 +51,7 @@ { q1 = wheel_M1.getPulses()/(1334.355/2); q2 = wheel_M2.getPulses()/(1334.355/2); - q1_v = (q1-q1_prev)/0.02/0.3; - q2_v = (q2-q2_prev)/0.02/0.3; - q1_prev = q1; - q2_prev = q2; - pc.printf("q1_v = %f \tq1_v_ref = %f \tPID1 = %f \tq2_v = %f \tq2_v_ref = %f \tPID2 = %f\n\r",q1_v, q1_v_ref, ctrlOutput_M1,q2_v,q2_v_ref,ctrlOutput_M2); + pc.printf("q1 = %f \tq1_ref = %f \tPID1 = %f \tq2 = %f \tq2_ref = %f \tPID2 = %f\n\r",q1, q1_ref, ctrlOutput_M1,q2,q2_ref,ctrlOutput_M2); } BiQuad pidf_M1; @@ -81,32 +73,26 @@ void M1_controller() { - ctrlOutput_M1 = pidf_M1.step(q1_v_ref-q1_v); + ctrlOutput_M1 = pidf_M1.step(q1_ref-q1); if (ctrlOutput_M1 < 0) { dir_M1 = 1; } else { dir_M1 = 0; } - if (q1>0.5*3.1415) { - pwm_M1 = 0; - } else if (q1<-0.5*3.1415) { - pwm_M1 = 0; - } else { - pwm_M1 = abs(ctrlOutput_M1) + q1_v; - } + pwm_M1 = abs(ctrlOutput_M1); } void M2_controller() { - ctrlOutput_M2 = pidf_M2.step(q2_v_ref-q2_v); + ctrlOutput_M2 = pidf_M2.step(q2_ref-q2); if (ctrlOutput_M2 < 0) { dir_M2 = 1; } else { dir_M2 = 0; } - pwm_M2 = abs(ctrlOutput_M2) + q2_v; + pwm_M2 = abs(ctrlOutput_M2); } int main() @@ -122,7 +108,7 @@ PIDcontrol_M2.attach(&flag_M2_controller, TS); // initialize function - knop.fall(&initialize); + knop.fall(&flag_initialize); if (go_flag_initialize == true) { go_flag_initialize = false; initialize();