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

HalloRobot/libProject/include/gyro_our.h

@@ -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

HalloRobot/libProject/src/control.c

@@ -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;

HalloRobot/libProject/src/gyro_our.c

@@ -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.

HalloRobot/main.c

@@ -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