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

Anpassung Zeitmessung + Form Reglerimplementierung

parent b3ae3b5f
......@@ -9,7 +9,6 @@
#ifndef F_CONTROL
#define F_CONTROL 200 //noch festzulegen
#endif
#define C_SCALE 1 //Skaliert regler Konstanten
#define INITIAL_OFFSET 2.8
static float sum_error = 0;
......@@ -43,12 +42,12 @@ float two_point_control(float e)
*/
float pi_control(float e)
{
const float K_p = 25.0f;
const float K_i = 5.0f;
const float K = 20.0f;
const float T_i = 0.18f;
sum_error += e;
sum_error += e/F_CONTROL;
float new_out = K_p*e + K_i*sum_error/F_CONTROL;
float new_out = K*(e + sum_error/T_i);
if(new_out>OUT_MAX){
new_out = OUT_MAX;
......@@ -69,9 +68,9 @@ float pi_control(float e)
*/
float pid_control(float e)
{
const float K_p = 20.0f*C_SCALE;
const float K_i = 110.0f*C_SCALE;
const float K_d = 0.00*C_SCALE;
const float K = 20.0f;
const float T_i = 0.18f;
const float T_d = 0.00;
static float e_old = 0;
......@@ -79,7 +78,7 @@ float pid_control(float e)
sum_error += e/F_CONTROL;
float derivative = (e - e_old)*F_CONTROL;
float new_out = K_p*e + K_i*sum_error + K_d*derivative;
float new_out = K*(e + sum_error/T_i + T_d*derivative);
if(new_out>OUT_MAX){
new_out = OUT_MAX;
......
......@@ -39,11 +39,12 @@
#define F_SAMPLING 200 //acc_rate = 200, gyro = 190
#ifndef F_CONTROL //F_CONTROL auch in control.c
#define F_CONTROL 200 //noch festzulegen
#endif
//#define WCET //aktiviert Messung
#define PID //TWO_POINT, PI, PID
#ifdef TWO_POINT
......@@ -72,11 +73,13 @@ enum{
cyg_uint32 time_sample;
cyg_uint32 time_control;
#ifdef WCET
float ticks_to_ms(cyg_uint32 time){
cyg_resolution_t res = ezs_counter_get_resolution();
float ms = (time*res.dividend/res.divisor)/1000000.0f;
return ms;
}
#endif
/* ISR data */
#define EXTI0_VECT CYGNUM_HAL_INTERRUPT_EXTI0 // See HAL definitions for interrupt vector table
......@@ -110,14 +113,18 @@ static cyg_alarm sampling_alarm;
static cyg_handle_t sampling_alarm_handle;
void sampling_alarm_fn(cyg_handle_t alarmH, cyg_addrword_t data){
cyg_thread_resume(sampling_handle);
//ezs_watch_start(&time_sample);
#ifdef WCET
ezs_watch_start(&time_sample);
#endif
}
static cyg_alarm control_alarm;
static cyg_handle_t control_alarm_handle;
void control_alarm_fn(cyg_handle_t alarmH, cyg_addrword_t data){
cyg_thread_resume(control_handle);
//ezs_watch_start(&time_control);
#ifdef WCET
ezs_watch_start(&time_control);
#endif
}
static cyg_alarm led_alarm;
......@@ -139,8 +146,9 @@ void sampling_thread(cyg_addrword_t arg)
{
float gyro_rates[3] = {0,0,0};
float phi_acc = 0;
//uint32_t max_time = 0;
#ifdef WCET
uint32_t max_time = 0;
#endif
while(1){
//sample gyro
gyro_xyz(gyro_rates);
......@@ -150,10 +158,14 @@ void sampling_thread(cyg_addrword_t arg)
//Filter
phi_cor = complementary_filter(phi_acc, gyro_rates[1],F_SAMPLING);
//ezs_printf("x: %f, y: %f, z: %f, phi_acc: %f\n",gyro_rates[0],gyro_rates[1],gyro_rates[2], phi_acc);
//ezs_printf("%f,%f,%f\n",gyro_rates[1],phi_acc,phi_cor);
//uint32_t current_run = ezs_watch_stop(&time_sample);
//if(current_run > max_time){max_time = current_run;}
#ifdef WCET
uint32_t current_run = ezs_watch_stop(&time_sample);
if(current_run > max_time){
max_time = current_run;
ezs_printf("max time sample: %f, %d \n",ticks_to_ms(max_time), max_time);
}
#endif
cyg_thread_suspend(cyg_thread_self());
}
}
......@@ -163,16 +175,22 @@ void control_thread(cyg_addrword_t arg)
float null_offset_pos = 2.8;
float error = 0;
float motor_signal = 0;
//uint32_t max_time = 0;
#ifdef WCET
uint32_t max_time = 0;
#endif
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);
motor_pwm(motor_signal);
//uint32_t current_run = ezs_watch_stop(&time_sample);
//if(current_run > max_time){max_time = current_run;}
#ifdef WCET
uint32_t current_run = ezs_watch_stop(&time_sample);
if(current_run > max_time){
max_time = current_run;
ezs_printf("max time control: %f, %d \n",ticks_to_ms(max_time), max_time);
}
#endif
cyg_thread_suspend(cyg_thread_self());
}
}
......
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