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

Blockpraktikum Endstand

parent ab8b3910
......@@ -3,11 +3,7 @@
#define GYRO_OUR_H
#include <stdint.h>
//defines
#define F_GYRO 190 //gesetzt in stm32f411e_discovery_gyroscope.c
#define CALIBRATED 1
#define G_ERROR 0
#define CAL_SAMPLES 1000
/*!
* \defgroup gyro PROJECT GYRO Library
......
......@@ -9,7 +9,7 @@
#ifndef F_CONTROL
#define F_CONTROL 200 //noch festzulegen
#endif
#define INITIAL_OFFSET 2.8
#define INITIAL_OFFSET 3.0
static float sum_error = 0;
static float sum_offset = INITIAL_OFFSET;
......@@ -176,8 +176,8 @@ float offset_control(float motor_signal)
//sum_offset += K*motor_signal/F_CONTROL;
static float sum = 0;
sum += motor_signal;
if(i==10){
sum_offset = K*iir_filter_05Hz_N3(sum/10) + INITIAL_OFFSET;
if(i==15){
sum_offset = K*iir_filter_05Hz_N3(sum/i) + INITIAL_OFFSET;
i=0;
sum = 0;
}
......
......@@ -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 k = 0.993;
float k = 0.995;
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;
......
......@@ -3,6 +3,12 @@
#include "stm32f411e_discovery_gyroscope.h"
#include "ezs_io.h"
//defines
#define F_GYRO 190 //gesetzt in stm32f411e_discovery_gyroscope.c
#define CALIBRATED 1
#define G_ERROR 0
#define CAL_SAMPLES 1000
/*
* Uses BSP_functions to read current angular rate
* and corrects the value by saved offsets.
......
......@@ -172,7 +172,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 = 2.99;
float error = 0;
float motor_signal = 0;
#ifdef WCET
......@@ -181,7 +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);
//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
......
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