Commit 9cee23c9 authored by Quirin Apfel's avatar Quirin Apfel
Browse files

Regler Tests

parent 8cf51ba3
......@@ -68,17 +68,23 @@ float pi_control(float e)
*/
float pid_control(float e)
{
const float K = 20.0f;
const float T_i = 0.18f;
const float T_d = 0.00;
const float K = 0.8f;
const float T_i = 0.5f;//0.18
const float T_d = 0.05;
static float e_old = 0;
sum_error += e/F_CONTROL;
float ee = e*e;
if(e<0){
ee = -ee;
}
sum_error += ee/F_CONTROL;
float derivative = (e - e_old)*F_CONTROL;
float new_out = K*(e + sum_error/T_i + T_d*derivative);
float new_out = K*(ee + sum_error/T_i + T_d*derivative);
if(new_out>OUT_MAX){
new_out = OUT_MAX;
......@@ -91,21 +97,36 @@ float pid_control(float e)
return new_out;
}
static float iir_filter(float phi_acc)
static float in[3] = {0};
static float out[3] = {0};
static float iir_filter_05Hz_N3(float offset)
{
const float a[3] = {-2.9686,2.9377,-0.9691};
const float b[4] = {4.7695e-7,1.4309e-6,1.4309e-6,4.7695e-7};
float new_out = b[0]*offset + b[1]*in[0] + b[2]*in[1] + b[3]*in[2] - a[0]*out[0] - a[1]*out[1] - a[2]*out[2];
out[2] = out[1];
out[1] = out[0];
out[0] = new_out;
in[2] = in[1];
in[1] = in[0];
in[0] = offset;
return new_out;
}
static float iir_filter_1Hz_N2(float offset)
{
static float in_1 = 0;
static float in_2 = 0;
static float out_1 = 0;
static float out_2 = 0;
const float a[2] = {-1.9112,0.915};
const float b[3] = {9.4469e-4,0.0019,9.4469e-4};
float new_out = b[0]*phi_acc + b[1]*in_1 + b[2]*in_2- a[0]*out_1 - a[1]*out_2;
float new_out = b[0]*offset + b[1]*in[0] + b[2]*in[1] - a[0]*out[0] - a[1]*out[1];
out_2 = out_1;
out_1 = new_out;
in_2 = in_1;
in_1 = phi_acc;
out[1] = out[0];
out[0] = new_out;
in[1] = in[0];
in[0] = offset;
return new_out;
}
......@@ -115,15 +136,17 @@ static float iir_filter(float phi_acc)
*/
float offset_control(float motor_signal)
{
const float K = 0.001f;//0.001
float max_offset = 6.0;
float min_offset = 0.0;
// sum_offset += K*motor_signal/F_CONTROL;
const float K = -0.01f;//0.001
float max_offset = 10.0;
float min_offset = -10.0;
static int i = 0;
//sum_offset += K*motor_signal/F_CONTROL;
if(i==10){
sum_offset = K*iir_filter_05Hz_N3(motor_signal) + INITIAL_OFFSET;
i=0;
}
float new_offset = iir_filter(K*motor_signal);
// sum_offset = sum_offset + iir_filter(K*motor_signal);
// float new_offset = sum_offset
float new_offset = sum_offset;
if(new_offset > max_offset){
new_offset = max_offset;
sum_offset -= motor_signal/F_CONTROL;
......@@ -131,7 +154,7 @@ float offset_control(float motor_signal)
new_offset = min_offset;
sum_offset -= motor_signal/F_CONTROL;
}
i++;
return new_offset;
}
......@@ -140,5 +163,11 @@ float offset_control(float motor_signal)
*/
void reset_control(void){
sum_error = 0;
sum_offset = INITIAL_OFFSET;
sum_offset = INITIAL_OFFSET;
out[2] = 0;
out[1] = 0;
out[0] = 0;
in[2] = 0;
in[1] = 0;
in[0] = 0;
}
......@@ -13,7 +13,7 @@
#include "diy_int.h"
#define PWM_PERIOD 2048 //20 kHz
#define PWM_MIN 66 //percent of PWM_PERIOD
#define PWM_MIN 65 //percent of PWM_PERIOD
static uint16_t usable_pwm_range = 0;
static uint16_t min_pwm_value = 0;
......
......@@ -181,8 +181,7 @@ void control_thread(cyg_addrword_t arg)
while(1){
error = null_offset_pos - phi_cor;
motor_signal = control_func_ptr(error);
null_offset_pos = offset_control(motor_signal);
ezs_printf("%f,%f,%f\n",null_offset_pos,phi_cor,motor_signal);
null_of/\\\////zs_printf("%f,%f,%f\n",null_offset_pos,phi_cor,motor_signal);
motor_pwm(motor_signal);
#ifdef WCET
uint32_t current_run = ezs_watch_stop(&time_sample);
......@@ -295,6 +294,8 @@ static void button_dsr(cyg_vector_t vector, cyg_ucount32 count, cyg_addrword_t d
}else if(status == ACTIVE){
cyg_alarm_disable(sampling_alarm_handle);
cyg_alarm_disable(control_alarm_handle);
cyg_thread_suspend(control_handle);
cyg_thread_suspend(sampling_handle);
diy_gpio_clear_pins(GPIOC, GPIO2 | GPIO3); //motoren aus
reset_filter();
reset_control();
......
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