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

Stand Freitag 16:00

parent a4b7e84b
......@@ -55,14 +55,16 @@ float pi_control(float e)
}else if(new_out<OUT_MIN){
new_out = OUT_MIN;
sum_error -= e;
}
}
return new_out;
}
#define K_FEIN 0.4
#define K_GROB 40.0
#define I_MAX 1.2
#define I_MIN -1.2
/*
* Implements PID-controller.
* Feeds back correction for system input depending on system error e,
......@@ -71,15 +73,15 @@ float pi_control(float e)
*/
float pid_control(float e)
{
static float K = 5.0;
static float K = 4.0;
const float T_i = 0.1f;//0.18
const float T_d = 0.025;
const float T_d = 0.04;
/*
if(e < 0.45 && e > -0.45){
K = 0.4f;
if(e < 1 && e > -1){
K = 5.0;
}
if(e > 4.0 || e < -4.0){
if(e > 3.0 || e < -3.0){
K = 100.0;
}
*/
......@@ -106,10 +108,12 @@ float pid_control(float e)
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);
float p = e;
float c_i = sum_error/T_i;
float c_d = T_d*derivative;
float new_out = K*(p + c_i + c_d);
//ezs_printf("%f,%f,%f,%f\n",p,c_i,c_d,new_out);
if(new_out>OUT_MAX){
new_out = OUT_MAX;
sum_error -= e/F_CONTROL;
......@@ -117,7 +121,12 @@ float pid_control(float e)
new_out = OUT_MIN;
sum_error -= e/F_CONTROL;
}
if(sum_error>I_MAX){
sum_error = I_MAX;
}else if(sum_error<I_MIN){
sum_error = I_MIN;
}
return new_out;
}
......@@ -163,11 +172,14 @@ float offset_control(float motor_signal)
const float K = -0.01f;//0.001
float max_offset = 10.0;
float min_offset = -10.0;
static int i = 0;
static int i = 1;
//sum_offset += K*motor_signal/F_CONTROL;
static float sum = 0;
sum += motor_signal;
if(i==10){
sum_offset = K*iir_filter_05Hz_N3(motor_signal) + INITIAL_OFFSET;
sum_offset = K*iir_filter_05Hz_N3(sum/10) + INITIAL_OFFSET;
i=0;
sum = 0;
}
float new_offset = sum_offset;
......
......@@ -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.995;
float k = 0.993;
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;
......
......@@ -182,7 +182,7 @@ void control_thread(cyg_addrword_t arg)
error = null_offset_pos - phi_cor;
motor_signal = control_func_ptr(error);
null_offset_pos = offset_control(motor_signal);
ezs_printf("%f,%f,%f,%f\n",null_offset_pos,phi_cor,error,motor_signal);
//ezs_printf("%f,%f,%f,%f\n",null_offset_pos,phi_cor,error,motor_signal);
motor_pwm(motor_signal);
#ifdef WCET
uint32_t current_run = ezs_watch_stop(&time_sample);
......
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