From a4b7e84b0dd1049bc5bb138cb6191dbec7850a04 Mon Sep 17 00:00:00 2001 From: Quirin Apfel <quirin.apfel@fau.de> Date: Fri, 3 Aug 2018 11:09:58 +0200 Subject: [PATCH] d-Anteil eingefuegt --- HalloRobot/libProject/src/control.c | 36 +++++++++++++++++++++++------ HalloRobot/libProject/src/filter.c | 2 +- 2 files changed, 30 insertions(+), 8 deletions(-) diff --git a/HalloRobot/libProject/src/control.c b/HalloRobot/libProject/src/control.c index 931e731..e37e29c 100644 --- a/HalloRobot/libProject/src/control.c +++ b/HalloRobot/libProject/src/control.c @@ -60,6 +60,9 @@ float pi_control(float e) return new_out; } + +#define K_FEIN 0.4 +#define K_GROB 40.0 /* * Implements PID-controller. * Feeds back correction for system input depending on system error e, @@ -68,32 +71,51 @@ float pi_control(float e) */ float pid_control(float e) { - static float K = 0.4f; + static float K = 5.0; const float T_i = 0.1f;//0.18 const float T_d = 0.025; - - if(e < 0.5 && e > -0.5){ + /* + if(e < 0.45 && e > -0.45){ K = 0.4f; } - if(e > 2.0 || e < -2.0){ + if(e > 4.0 || e < -4.0){ K = 100.0; } + */ + /* + static float mode = 0; //0=fein, 1= eskalation mit richtung + if(e > 2.5){ + K = K_GROB; + mode = 1; + }else if(e < - 2.5){ + K = K_GROB; + mode = -1; + } + + if(mode == 1 && e < 0.3){ + mode = 0; + K = K_FEIN; + }else if(mode == -1 && e > -0.3){ + K = K_FEIN; + mode = 0; + } + */ static float e_old = 0; sum_error += e/F_CONTROL; float derivative = (e - e_old)*F_CONTROL; - + e_old = e; float new_out = K*(e + sum_error/T_i + T_d*derivative); if(new_out>OUT_MAX){ new_out = OUT_MAX; - sum_error = OUT_MAX/300.0; + sum_error -= e/F_CONTROL; }else if(new_out<OUT_MIN){ new_out = OUT_MIN; - sum_error = OUT_MIN/300.0; + sum_error -= e/F_CONTROL; } return new_out; diff --git a/HalloRobot/libProject/src/filter.c b/HalloRobot/libProject/src/filter.c index bd51720..52d6128 100644 --- a/HalloRobot/libProject/src/filter.c +++ b/HalloRobot/libProject/src/filter.c @@ -33,7 +33,7 @@ static float iir_filter(float phi_acc) */ float complementary_filter(float phi_acc, float gyro_rate, uint16_t f_sampling) { - float k = 0.999; + float k = 0.995; float phi_acc_fil = iir_filter(phi_acc); phi_cor = k*(phi_cor+gyro_rate/f_sampling)+(1.0-k)*phi_acc_fil; return phi_cor; -- GitLab