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:
- 13:fe2efec19eda
- Parent:
- 12:ff7947a79149
--- a/main.cpp Fri Oct 28 14:17:12 2016 +0000 +++ b/main.cpp Tue Nov 01 09:18:58 2016 +0000 @@ -24,8 +24,9 @@ volatile float q2 = 0; volatile float q1_begin; volatile float q2_begin; +const float pi = 3.14159265359; const float l1 = 0.3626; -const float l2 = 0.420; +const float l2 = (0.420-0.065); // middelpunt swiffer volatile float q1_v; volatile float q2_v; volatile float q1_ref = 0; @@ -70,13 +71,6 @@ //pc.printf("q1_err = %0.9f \tq2_err = %0.9f \tq1IntErr = %0.9f \tq2IntErr = %0.9f \tTotErr1 = %0.9f \tTotErr2 = %0.9f\n\r",q1_error,q2_error,q1IntError,q2IntError,q1_total_error,q2_total_error); } -volatile bool go_flag_initialize = false; - -void flag_initialize() -{ - go_flag_initialize = true; -} - Ticker PIDcontrol; volatile bool go_flag_controller = false; @@ -89,34 +83,36 @@ void begin_hoeken() { - wait(3); + wait(1.5); q1_ref = wheel_M1.getPulses()/(1334.355/2); q2_ref = wheel_M2.getPulses()/(1334.355/2); active_PID_ticker = true; } - void initialize() { dir_M1 = 0; //ccw dir_M2 = 1; //cw - while (q1 < 40*2*3.1415/360) { + while ( (q1 < 20*2*pi/360) || (q2 > -45*2*pi/360) ) { q1 = wheel_M1.getPulses()/(1334.355/2); - pwm_M1 = 0.05; + q2 = wheel_M2.getPulses()/(1334.355/2); + if (q1 < 20*2*pi/360) { + pwm_M1 = 0.05; + } else { + pwm_M1 = 0; + } + if (q2 > -45*2*pi/360) { + pwm_M2 = 0.06; + } else { + pwm_M2 = 0; + } wait(0.005f); } pwm_M1 = 0; - - while (q2 > -90*2*3.1415/360) { - q2 = wheel_M2.getPulses()/(1334.355/2); - pwm_M2 = 0.05; - wait(0.005f); - } pwm_M2 = 0; begin_hoeken(); } - void biceps() { q1_ref_prev = 0; @@ -170,7 +166,7 @@ q1_error_prev = 0; q2_error_prev = 0; q1_total_error = 0; - q1_total_error = 0; + q2_total_error = 0; pc.printf("\n\rSWITCH!!! \n\n\r"); } @@ -211,29 +207,29 @@ //pc.printf("q1 = %f \tq2 = %f \tq1_v = %f \tq2_v = %f\n\r", q1,q2,q1_v,q2_v); - if ( (q1 > (90*2*3.1415/360)) && (q1_v > 0 ) ) { // WAARDES VINDEN 0.8726 (50 graden) + if ( (q1 > (135*2*pi/360)) && (q1_v > 0 ) ) { // WAARDES VINDEN 0.8726 (50 graden) q1_v = 0; q2_v = 0; - q1_ref_prev = 0; - q2_ref_prev = 0; + q1_ref = q1; + q2_ref = q2; q1IntError = 0; q2IntError = 0; q1_error_prev = 0; q2_error_prev = 0; - } else if ( (q1 < -(90*2*3.1415/360)) && (q1_v < 0) ) { + } else if ( (q1 < -(135*2*pi/360)) && (q1_v < 0) ) { q1_v = 0; q2_v = 0; - q1_ref_prev = 0; - q2_ref_prev = 0; + q1_ref = q1; + q2_ref = q2; q1IntError = 0; q2IntError = 0; q1_error_prev = 0; q2_error_prev = 0; - } else if ( (q2 < (-140*2*3.1415/360)) && (q2_v < 0) ) { // WAARDES VINDEN -2.4434 (-140 graden) --> werkelijke max -2.672452 + } else if ( (q2 < (-2.6)) && (q2_v < 0) ) { // WAARDES VINDEN -2.4434 (-140 graden) --> werkelijke max -2.672452 q1_v = 0; q2_v = 0; - q1_ref_prev = 0; - q2_ref_prev = 0; + q1_ref = q1; + q2_ref = q2; q1IntError = 0; q2IntError = 0; q1_error_prev = 0; @@ -241,8 +237,8 @@ } else if ( (q2 >= 0) && (q2_v > 0) ) { q1_v = 0; q2_v = 0; - q1_ref_prev = 0; - q2_ref_prev = 0; + q1_ref = q1; + q2_ref = q2; q1IntError = 0; q2IntError = 0; q1_error_prev = 0;