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:
- 12:ff7947a79149
- Parent:
- 11:03d979f1517f
- Child:
- 13:fe2efec19eda
--- a/main.cpp Fri Oct 28 10:36:46 2016 +0000 +++ b/main.cpp Fri Oct 28 14:17:12 2016 +0000 @@ -30,6 +30,8 @@ volatile float q2_v; volatile float q1_ref = 0; volatile float q2_ref = 0; +volatile float q1_ref_prev = 0; +volatile float q2_ref_prev = 0; volatile float q1_error = 0; volatile float q2_error = 0; volatile float q1_error_prev = 0; @@ -38,8 +40,8 @@ volatile float q2DerivativeError = 0; volatile float q1IntError = 0; volatile float q2IntError = 0; -volatile float TotalError1= 0; -volatile float TotalError2= 0; +volatile float q1_total_error= 0; +volatile float q2_total_error= 0; float motorValue1Out = 0.0; float motorValue2Out = 0.0; volatile float ctrlOutput_M1 = 0; @@ -63,9 +65,9 @@ { //q1 = wheel_M1.getPulses()/(1334.355/2); //q2 = wheel_M2.getPulses()/(1334.355/2); - //pc.printf("q1 = %f \tq1_ref = %f \tq2 = %f \tq2_ref = %f \ttotalerr1 = %f \ttotalerr2 = %f\n\r",q1, q1_ref,q2,q2_ref,TotalError1,TotalError2); - pc.printf("vx = %f \tvy = %f \tq1_r = %f \tq2_r = %f \tq1 = %f \tq2 = %f \tPID_M1 = %f \tPID_M2 = %f\n\r",vx,vy,q1_ref,q2_ref,q1,q2,ctrlOutput_M1,ctrlOutput_M2); - //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,TotalError1,TotalError2); + //pc.printf("q1 = %f \tq1_ref = %f \tq2 = %f \tq2_ref = %f \ttotalerr1 = %f \ttotalerr2 = %f\n\r",q1, q1_ref,q2,q2_ref,q1_total_error,q2_total_error); + //pc.printf("vx = %f \tvy = %f \tq1_v = %f \tq2_v = %f \tq1 = %f \tq2 = %f \tPID_M1 = %f \tPID_M2 = %f\n\r",vx,vy,q1_v,q2_v,q1,q2,ctrlOutput_M1,ctrlOutput_M2); + //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; @@ -105,19 +107,20 @@ } pwm_M1 = 0; - while (q2 > -80*2*3.1415/360) { + while (q2 > -90*2*3.1415/360) { q2 = wheel_M2.getPulses()/(1334.355/2); pwm_M2 = 0.05; wait(0.005f); } pwm_M2 = 0; - ledg = !ledg; begin_hoeken(); } void biceps() { + q1_ref_prev = 0; + q2_ref_prev = 0; q1IntError = 0; q2IntError = 0; q1_error_prev = 0; @@ -133,6 +136,8 @@ void triceps() { + q1_ref_prev = 0; + q2_ref_prev = 0; q1IntError = 0; q2IntError = 0; q1_error_prev = 0; @@ -156,10 +161,17 @@ } else { vx = 0; vy = 0; + q1_ref = q1; + q2_ref = q2; + q1_error = 0; + q2_error = 0; q1IntError = 0; q2IntError = 0; q1_error_prev = 0; q2_error_prev = 0; + q1_total_error = 0; + q1_total_error = 0; + pc.printf("\n\rSWITCH!!! \n\n\r"); } if (translatie_richting == 1) { @@ -197,22 +209,51 @@ q1_v = J_1 * vx + J_2 * vy; q2_v = J_3 * vx + J_4 * vy; + //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) q1_v = 0; q2_v = 0; + q1_ref_prev = 0; + q2_ref_prev = 0; + q1IntError = 0; + q2IntError = 0; + q1_error_prev = 0; + q2_error_prev = 0; } else if ( (q1 < -(90*2*3.1415/360)) && (q1_v < 0) ) { q1_v = 0; q2_v = 0; + q1_ref_prev = 0; + q2_ref_prev = 0; + 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 q1_v = 0; q2_v = 0; - } else if ( (q2 > 0) && (q2_v > 0) ) { + q1_ref_prev = 0; + q2_ref_prev = 0; + q1IntError = 0; + q2IntError = 0; + q1_error_prev = 0; + q2_error_prev = 0; + } else if ( (q2 >= 0) && (q2_v > 0) ) { q1_v = 0; q2_v = 0; + q1_ref_prev = 0; + q2_ref_prev = 0; + q1IntError = 0; + q2IntError = 0; + q1_error_prev = 0; + q2_error_prev = 0; } - - q1_ref = q1 + q1_v*TS; - q2_ref = q2 + q2_v*TS; + + q1_ref_prev = q1_ref; + q2_ref_prev = q2_ref; + + q1_ref = q1_ref_prev + q1_v*TS; + q2_ref = q2_ref_prev + q2_v*TS; } void PID(float q1,float q1_ref,float q2,float q2_ref,float TS,float &motorValue1Out, float &motorValue2Out) @@ -230,12 +271,12 @@ q2DerivativeError = (q2_error - q2_error_prev)/TS; // derivative of error in radians float Kd = 0; - TotalError1 = (q1_error * Kp) + (q1IntError * Ki) + (q1DerivativeError * Kd); //total controller output = motor input - TotalError2 = (q2_error * Kp) + (q2IntError * Ki) + (q2DerivativeError * Kd); //total controller output = motor input + q1_total_error = (q1_error * Kp) + (q1IntError * Ki) + (q1DerivativeError * Kd); //total controller output = motor input + q2_total_error = (q2_error * Kp) + (q2IntError * Ki) + (q2DerivativeError * Kd); //total controller output = motor input - motorValue1Out = TotalError1/MotorGain_M1; - motorValue2Out = TotalError2/MotorGain_M2; - + motorValue1Out = q1_total_error/MotorGain_M1; + motorValue2Out = q2_total_error/MotorGain_M2; + q1_error_prev = q1_error; q2_error_prev = q2_error; } @@ -246,6 +287,8 @@ ctrlOutput_M1 = motorValue1Out; ctrlOutput_M2 = motorValue2Out; + pc.printf("q1_err = %0.9f \tq2_err = %0.9f \tq1IntErr = %0.9f \tq2IntErr = %0.9f \tq1_ref = %0.9f \tq2_ref = %0.9f \tq1 = %f \tq2 = %f\n\r",q1_error,q2_error,q1IntError,q2IntError,q1_ref,q2_ref,q1,q2); + if (ctrlOutput_M1 < 0) { dir_M1 = 1; } else { @@ -282,12 +325,13 @@ knop_biceps.rise(&biceps); knop_triceps.rise(&triceps); knop_switch.rise(&switcher); - + + // initialize -> beginposities + initialize(); + // flag functions/tickers update_encoder_ticker.attach(&flag_update_encoder, TS); update_ref_ticker.attach(&flag_update_ref, TS); - // initialize -> beginposities - initialize(); if (active_PID_ticker == true) { PIDcontrol.attach(&flag_controller, TS);