Blockpraktikum Endstand

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

HalloRobot/libProject/src/filter.c

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

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

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