Commit 40849a70 authored by Stefan's avatar Stefan
Browse files

Merge branch 'master' of gitlab.cs.fau.de:diygrp2/diy-project

parents 7349c216 229f2517
......@@ -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;
......@@ -55,14 +55,14 @@ float pi_control(float e)
}else if(new_out<OUT_MIN){
new_out = OUT_MIN;
sum_error -= e;
}
}
return new_out;
}
#define K_FEIN 0.4
#define K_GROB 40.0
#define I_MAX 1.2
#define I_MIN -1.2
/*
* Implements PID-controller.
* Feeds back correction for system input depending on system error e,
......@@ -71,19 +71,19 @@ float pi_control(float e)
*/
float pid_control(float e)
{
static float K = 5.0;
const float T_i = 0.1f;
const float T_d = 0.025;
static float K = 4.0;
const float T_i = 0.1f;//0.18
const float T_d = 0.04;
static float e_old = 0;
sum_error += e/F_CONTROL;
float derivative = (e - e_old)*F_CONTROL;
e_old = e;
float new_out = K*(e + sum_error/T_i + T_d*derivative);
float p = e;
float c_i = sum_error/T_i;
float c_d = T_d*derivative;
float new_out = K*(p + c_i + c_d);
if(new_out>OUT_MAX){
new_out = OUT_MAX;
sum_error -= e/F_CONTROL;
......@@ -91,7 +91,12 @@ float pid_control(float e)
new_out = OUT_MIN;
sum_error -= e/F_CONTROL;
}
if(sum_error>I_MAX){
sum_error = I_MAX;
}else if(sum_error<I_MIN){
sum_error = I_MIN;
}
return new_out;
}
......@@ -137,10 +142,14 @@ float offset_control(float motor_signal)
const float K = -0.01f;
float max_offset = 10.0;
float min_offset = -10.0;
static int i = 0;
if(i==10){
sum_offset = K*iir_filter_05Hz_N3(motor_signal) + INITIAL_OFFSET;
static int i = 1;
static float sum = 0;
sum += motor_signal;
if(i==15){
sum_offset = K*iir_filter_05Hz_N3(sum/i) + INITIAL_OFFSET;
i=0;
sum = 0;
}
float new_offset = sum_offset;
......
......@@ -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.
......
......@@ -168,7 +168,7 @@ void sampling_thread(cyg_addrword_t arg)
void control_thread(cyg_addrword_t arg)
{
float null_offset_pos = 3.0;
float null_offset_pos = 2.99;
float error = 0;
float motor_signal = 0;
#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