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

Entfernen printf's + Anpassung filter-reset

parent 4e06d72e
......@@ -20,7 +20,7 @@
float complementary_filter(float phi_acc, float gyro_rate, uint16_t f_sampling);
/*! \brief Resets filter angle
/*! \brief Resets filter
*/
void reset_filter(void);
#endif
......@@ -17,16 +17,14 @@ float acc_get_phi(void)
float ac_Y = (accel_XYZ[1] >> 4)/1000.0f;
float ac_Z = (accel_XYZ[2] >> 4)/1000.0f;
//Korrektur auf Gyro-Koordinaten
//Transformation to gyro-coordinates
float ac_X_cor = -ac_Y;
float ac_Y_cor = ac_X;
float ac_Z_cor = -ac_Z;
//Berechnung phi
//Calculation of phi
float phi = atan(ac_Z_cor/ac_X_cor);
phi = phi*180/M_PI;
//if(phi<0)phi+=180; //Verschiebung, wegen Sprung
return phi;
}
......@@ -72,35 +72,9 @@ float pi_control(float e)
float pid_control(float e)
{
static float K = 5.0;
const float T_i = 0.1f;//0.18
const float T_i = 0.1f;
const float T_d = 0.025;
/*
if(e < 0.45 && e > -0.45){
K = 0.4f;
}
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;
......@@ -160,11 +134,10 @@ static float iir_filter_1Hz_N2(float offset)
*/
float offset_control(float motor_signal)
{
const float K = -0.01f;//0.001
const float K = -0.01f;
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;
......
......@@ -5,12 +5,13 @@ static float phi_cor = 0;
/* IIR filter with butterworth coefficients */
static float in_1 = 0;
static float in_2 = 0;
static float out_1 = 0;
static float out_2 = 0;
static float iir_filter(float phi_acc)
{
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};
......@@ -40,8 +41,12 @@ float complementary_filter(float phi_acc, float gyro_rate, uint16_t f_sampling)
}
/*
* Resets current filter angle.
* Resets current filter angle and iir_filter.
*/
void reset_filter(void){
phi_cor = 0;
in_1 = 0;
in_2 = 0;
out_1 = 0;
out_2 = 0;
}
......@@ -45,7 +45,6 @@ uint8_t calibrate_gyro(void)
}
double sum[3] = {0,0,0}; //fuer Durchschnittsbildung
float gyro_XYZ[3] = {0,0,0};
//ezs_printf("Starting gyro calibration \n");
uint16_t i;
uint32_t delay_us = 1e6/F_GYRO;
for(i=0;i<CAL_SAMPLES;i++){
......@@ -59,6 +58,5 @@ uint8_t calibrate_gyro(void)
calibration_data.x = sum[0]/CAL_SAMPLES;
calibration_data.y = sum[1]/CAL_SAMPLES;
calibration_data.z = sum[2]/CAL_SAMPLES;
//ezs_printf("Ending gyro calibration \n");
return CALIBRATED;
}
......@@ -94,10 +94,6 @@ static void led_init(void);
/* Thread-Stack */
#define STACKSIZE CYGNUM_HAL_STACK_SIZE_MINIMUM+1024
static cyg_uint8 my_stack[STACKSIZE];
static cyg_handle_t handle;
static cyg_thread threaddata;
static cyg_uint8 sampling_stack[STACKSIZE];
static cyg_handle_t sampling_handle;
static cyg_thread sampling_threaddata;
......@@ -172,7 +168,7 @@ void sampling_thread(cyg_addrword_t arg)
void control_thread(cyg_addrword_t arg)
{
float null_offset_pos = 2.8;
float null_offset_pos = 3.0;
float error = 0;
float motor_signal = 0;
#ifdef WCET
......@@ -181,8 +177,8 @@ 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,%f\n",null_offset_pos,phi_cor,error,motor_signal);
//null_offset_pos = offset_control(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);
......@@ -194,24 +190,6 @@ void control_thread(cyg_addrword_t arg)
cyg_thread_suspend(cyg_thread_self());
}
}
/*
void test_thread(cyg_addrword_t arg)
{
ezs_printf("Hallo \n");
diy_gpio_set_pins(GPIOB,GPIO0);
while(1){
uint16_t i;
uint16_t pwm = 0;
for(i=1;i<=10;i++){
pwm = 1200+(PWM_PERIOD*i)/100;
ezs_printf("pwm: %d \n",pwm);
pwm_config.compare_value = pwm;
diy_tim_channel_init(&pwm_config, TIM2);
ezs_delay_us(2e6);
}
ezs_delay_us(1e6);
}
}*/
void cyg_user_start(void)
{
......@@ -223,9 +201,6 @@ void cyg_user_start(void)
// STM32F411 data sheet
// see: http://www.st.com/content/ccc/resource/technical/document/datasheet/b3/a5/46/3b/b4/e5/4c/85/DM00115249.pdf/files/DM00115249.pdf/jcr:content/translations/en.DM00115249.pdf
/* Threads erzeugen */
//cyg_thread_create(11, &test_thread, 0, "thread", my_stack, STACKSIZE,
// &handle, &threaddata);
cyg_thread_create(11, &sampling_thread, 0, "sampling_thread", sampling_stack, STACKSIZE,
&sampling_handle, &sampling_threaddata);
cyg_thread_create(10, &control_thread, 0, "control_thread", control_stack, STACKSIZE,
......@@ -263,8 +238,6 @@ void cyg_user_start(void)
cyg_alarm_initialize(led_alarm_handle,cyg_current_time()+1,500);
cyg_interrupt_unmask(EXTI0_VECT);
//cyg_thread_resume(handle);
}
/* Declare ISR */
......
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