Commit 5c69c4e3 authored by Quirin Apfel's avatar Quirin Apfel
Browse files

motor fix

parent 99641003
......@@ -31,7 +31,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.999;
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;
......
......@@ -92,7 +92,7 @@ void motor_init(void){
*/
void motor_pwm(float speed){
static uint8_t old_direction = 0; //remember to only set pins if necessary
static uint8_t old_direction = 2; //remember to only set pins if necessary
//get direction
uint8_t direction = 0; //0 positive, 1 negative
......
......@@ -158,7 +158,7 @@ void sampling_thread(cyg_addrword_t arg)
//Filter
phi_cor = complementary_filter(phi_acc, gyro_rates[1],F_SAMPLING);
ezs_printf("%f,%f,%f\n",gyro_rates[1],phi_acc,phi_cor);
//ezs_printf("%f,%f,%f\n",gyro_rates[1],phi_acc,phi_cor);
#ifdef WCET
uint32_t current_run = ezs_watch_stop(&time_sample);
if(current_run > max_time){
......@@ -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\n",null_offset_pos,phi_cor,motor_signal);
ezs_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);
......
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