Commit a4b7e84b authored by Quirin Apfel's avatar Quirin Apfel
Browse files

d-Anteil eingefuegt

parent b975d8ce
......@@ -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;
......
......@@ -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;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment