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