加了个滤波

User/lib/filter.c

@@ -0,0 +1,184 @@
+/*
+  各类滤波器。
+*/
+
+#include "filter.h"
+
+#include "user_math.h"
+
+/**
+ * @brief 初始化滤波器
+ *
+ * @param f 滤波器
+ * @param sample_freq 采样频率
+ * @param cutoff_freq 截止频率
+ */
+void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq,
+                          float cutoff_freq) {
+  if (f == NULL) return;
+
+  f->cutoff_freq = cutoff_freq;
+
+  f->delay_element_1 = 0.0f;
+  f->delay_element_2 = 0.0f;
+
+  if (f->cutoff_freq <= 0.0f) {
+    /* no filtering */
+    f->b0 = 1.0f;
+    f->b1 = 0.0f;
+    f->b2 = 0.0f;
+
+    f->a1 = 0.0f;
+    f->a2 = 0.0f;
+
+    return;
+  }
+  const float fr = sample_freq / f->cutoff_freq;
+  const float ohm = tanf(M_PI / fr);
+  const float c = 1.0f + 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm;
+
+  f->b0 = ohm * ohm / c;
+  f->b1 = 2.0f * f->b0;
+  f->b2 = f->b0;
+
+  f->a1 = 2.0f * (ohm * ohm - 1.0f) / c;
+  f->a2 = (1.0f - 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm) / c;
+}
+
+/**
+ * @brief 施加一次滤波计算
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample) {
+  if (f == NULL) return 0.0f;
+
+  /* do the filtering */
+  float delay_element_0 =
+      sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2;
+
+  if (isinf(delay_element_0)) {
+    /* don't allow bad values to propagate via the filter */
+    delay_element_0 = sample;
+  }
+
+  const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 +
+                       f->delay_element_2 * f->b2;
+
+  f->delay_element_2 = f->delay_element_1;
+  f->delay_element_1 = delay_element_0;
+
+  /* return the value. Should be no need to check limits */
+  return output;
+}
+
+/**
+ * @brief 重置滤波器
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample) {
+  if (f == NULL) return 0.0f;
+
+  const float dval = sample / (f->b0 + f->b1 + f->b2);
+
+  if (isfinite(dval)) {
+    f->delay_element_1 = dval;
+    f->delay_element_2 = dval;
+
+  } else {
+    f->delay_element_1 = sample;
+    f->delay_element_2 = sample;
+  }
+
+  return LowPassFilter2p_Apply(f, sample);
+}
+
+/**
+ * @brief 初始化滤波器
+ *
+ * @param f 滤波器
+ * @param sample_freq 采样频率
+ * @param notch_freq 中心频率
+ * @param bandwidth 带宽
+ */
+void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq,float bandwidth) 
+{
+  if (f == NULL) return;
+
+  f->notch_freq = notch_freq;
+  f->bandwidth = bandwidth;
+
+  f->delay_element_1 = 0.0f;
+  f->delay_element_2 = 0.0f;
+
+  if (notch_freq <= 0.0f) {
+    /* no filtering */
+    f->b0 = 1.0f;
+    f->b1 = 0.0f;
+    f->b2 = 0.0f;
+
+    f->a1 = 0.0f;
+    f->a2 = 0.0f;
+
+    return;
+  }
+
+  const float alpha = tanf(M_PI * bandwidth / sample_freq);
+  const float beta = -cosf(M_2PI * notch_freq / sample_freq);
+  const float a0_inv = 1.0f / (alpha + 1.0f);
+
+  f->b0 = a0_inv;
+  f->b1 = 2.0f * beta * a0_inv;
+  f->b2 = a0_inv;
+
+  f->a1 = f->b1;
+  f->a2 = (1.0f - alpha) * a0_inv;
+}
+
+/**
+ * @brief 施加一次滤波计算
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+inline float NotchFilter_Apply(NotchFilter_t *f, float sample) {
+  if (f == NULL) return 0.0f;
+
+  /* Direct Form II implementation */
+  const float delay_element_0 = sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2;
+  const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 +
+                       f->delay_element_2 * f->b2;
+
+  f->delay_element_2 = f->delay_element_1;
+  f->delay_element_1 = delay_element_0;
+
+  return output;
+}
+
+/**
+ * @brief 重置滤波器
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float NotchFilter_Reset(NotchFilter_t *f, float sample) {
+  if (f == NULL) return 0.0f;
+
+  float dval = sample;
+
+  if (fabsf(f->b0 + f->b1 + f->b2) > FLT_EPSILON) {
+    dval = dval / (f->b0 + f->b1 + f->b2);
+  }
+
+  f->delay_element_1 = dval;
+  f->delay_element_2 = dval;
+
+  return NotchFilter_Apply(f, sample);
+}

User/lib/filter.h

@@ -0,0 +1,104 @@
+/*
+  各类滤波器。
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "user_math.h"
+
+/* 二阶低通滤波器 */
+typedef struct {
+  float cutoff_freq; /* 截止频率 */
+
+  float a1;
+  float a2;
+
+  float b0;
+  float b1;
+  float b2;
+
+  float delay_element_1;
+  float delay_element_2;
+
+} LowPassFilter2p_t;
+
+/* 带阻滤波器 */
+typedef struct {
+  float notch_freq; /* 阻止频率 */
+  float bandwidth;  /* 带宽 */
+
+  float a1;
+  float a2;
+
+  float b0;
+  float b1;
+  float b2;
+  float delay_element_1;
+  float delay_element_2;
+
+} NotchFilter_t;
+
+/**
+ * @brief 初始化滤波器
+ *
+ * @param f 滤波器
+ * @param sample_freq 采样频率
+ * @param cutoff_freq 截止频率
+ */
+void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq,
+                          float cutoff_freq);
+
+/**
+ * @brief 施加一次滤波计算
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample);
+
+/**
+ * @brief 重置滤波器
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample);
+
+/**
+ * @brief 初始化滤波器
+ *
+ * @param f 滤波器
+ * @param sample_freq 采样频率
+ * @param notch_freq 中心频率
+ * @param bandwidth 带宽
+ */
+void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq,
+                      float bandwidth);
+
+/**
+ * @brief 施加一次滤波计算
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float NotchFilter_Apply(NotchFilter_t *f, float sample);
+
+/**
+ * @brief 重置滤波器
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float NotchFilter_Reset(NotchFilter_t *f, float sample);
+
+#ifdef __cplusplus
+}
+#endif

User/lib/kalman.c

@@ -0,0 +1,65 @@
+/**    
+  * @author  Liu heng
+  * 一阶卡尔曼滤波器来自RoboMaster论坛  
+  *   一维卡尔曼滤波器                     
+  *   使用时先定义一个kalman指针，然后调用kalmanCreate()创建一个滤波器 
+  *   每次读取到传感器数据后即可调用KalmanFilter()来对数据进行滤波
+  *          使用示例                                             
+  *          extKalman p;                  //定义一个卡尔曼滤波器结构体                                                 
+  *          float SersorData;             //需要进行滤波的数据                                          
+  *          KalmanCreate(&p,20,200);      //初始化该滤波器的Q=20 R=200参数                                                  
+  *          while(1)                                                                
+  *          {                                                                            
+  *             SersorData = sersor();                     //获取数据                                           
+  *             SersorData = KalmanFilter(&p,SersorData);  //对数据进行滤波                                                                            
+  *          }                                                                            
+  */
+
+#include "kalman.h"
+
+/**
+  * @name   kalmanCreate
+  * @brief  创建一个卡尔曼滤波器
+  * @param  p:  滤波器
+  *         T_Q:系统噪声协方差
+  *         T_R:测量噪声协方差
+  *         
+  * @retval none
+  * @attention R固定，Q越大，代表越信任侧量值，Q无穷代表只用测量值
+  *		       	反之，Q越小代表越信任模型预测值，Q为零则是只用模型预测
+  */
+void KalmanCreate(extKalman_t *p,float T_Q,float T_R)
+{
+    p->X_last = (float)0;
+    p->P_last = 0;
+    p->Q = T_Q;
+    p->R = T_R;
+    p->A = 1;
+	p->B = 0;
+    p->H = 1;
+    p->X_mid = p->X_last;
+}
+
+/**
+  * @name   KalmanFilter
+  * @brief  卡尔曼滤波器
+  * @param  p:  滤波器
+  *         dat:待滤波数据
+  * @retval 滤波后的数据
+  * @attention Z(k)是系统输入,即测量值   X(k|k)是卡尔曼滤波后的值,即最终输出
+  *            A=1 B=0 H=1 I=1  W(K)  V(k)是高斯白噪声,叠加在测量值上了,可以不用管
+  *            以下是卡尔曼的5个核心公式
+  *            一阶H'即为它本身,否则为转置矩阵
+  */
+
+float KalmanFilter(extKalman_t* p,float dat)
+{
+    p->X_mid =p->A*p->X_last;                     //百度对应公式(1)    x(k|k-1) = A*X(k-1|k-1)+B*U(k)+W(K)
+    p->P_mid = p->A*p->P_last+p->Q;               //百度对应公式(2)    p(k|k-1) = A*p(k-1|k-1)*A'+Q
+    p->kg = p->P_mid/(p->P_mid+p->R);             //百度对应公式(4)    kg(k) = p(k|k-1)*H'/(H*p(k|k-1)*H'+R)
+    p->X_now = p->X_mid+p->kg*(dat-p->X_mid);     //百度对应公式(3)    x(k|k) = X(k|k-1)+kg(k)*(Z(k)-H*X(k|k-1))
+    p->P_now = (1-p->kg)*p->P_mid;                //百度对应公式(5)    p(k|k) = (I-kg(k)*H)*P(k|k-1)
+    p->P_last = p->P_now;                         //状态更新
+    p->X_last = p->X_now;
+    return p->X_now;							  //输出预测结果x(k|k)
+}

User/lib/kalman.h

@@ -0,0 +1,28 @@
+/**
+  * @author  Liu heng
+  * 卡尔曼滤波器来自RoboMaster论坛  
+  */
+  
+#ifndef _KALMAN_H
+#define _KALMAN_H
+
+
+typedef struct {
+    float X_last; //上一时刻的最优结果  X(k-|k-1)
+    float X_mid;  //当前时刻的预测结果  X(k|k-1)
+    float X_now;  //当前时刻的最优结果  X(k|k)
+    float P_mid;  //当前时刻预测结果的协方差  P(k|k-1)
+    float P_now;  //当前时刻最优结果的协方差  P(k|k)
+    float P_last; //上一时刻最优结果的协方差  P(k-1|k-1)
+    float kg;     //kalman增益
+    float A;      //系统参数
+	float B;
+    float Q;
+    float R;
+    float H;
+}extKalman_t;
+
+void KalmanCreate(extKalman_t *p,float T_Q,float T_R);
+float KalmanFilter(extKalman_t* p,float dat);
+
+#endif

User/module/ball.cpp

@@ -32,8 +32,8 @@ Ball ::Ball()
    PID_init(&extend_speed_pid,PID_POSITION,Extend_speed_PID,3000, 200);
    PID_init(&extend_angle_pid,PID_POSITION,Extend_angle_PID,3000, 200);
 
-//    e_result = 0;
+   e_result = 0;
-//    angleSet = 0;
+   angleSet = 0;
  //三摩擦轮
    for(int i = 0;i < MOTOR_NUM;i ++)
  {
@@ -47,11 +47,24 @@ Ball ::Ball()
     speedSet[i] = 0;    
  }
 
+
   //状态机状态初始化
   currentState1= BALL_IDLE;
 
 }
 
+void  Ball :: Filter_init(float target_freq)
+{
+    for(int i = 0;i < MOTOR_NUM;i ++)
+    {
+     //摩擦轮滤波器初始化
+     LowPassFilter2p_Init(filter.in + i , target_freq,
+        -1.0f);
+     LowPassFilter2p_Init(filter.out + i, target_freq,
+        -1.0f);   
+    }
+}
+
 void Ball ::Extend_control(int angle)
 {
     int16_t delta;
@@ -72,6 +85,11 @@ void Ball ::Extend_control(int angle)
 void Ball ::Spin(float speed,float speed2)
 {
 
+    for(int i = 0;i < MOTOR_NUM;i ++)
+    {
+        hand_Motor[i]->speed_rpm=LowPassFilter2p_Apply(filter.in + i, hand_Motor[i]->speed_rpm);
+    }
+
     speedSet[MOTOR_1] = -speed;
     result[MOTOR_1] = PID_calc(&speed_pid[MOTOR_1],hand_Motor[MOTOR_1]->speed_rpm,speedSet[MOTOR_1]);
 
@@ -81,6 +99,11 @@ void Ball ::Spin(float speed,float speed2)
     speedSet[MOTOR_3] = speed2;
     result[MOTOR_3] = PID_calc(&speed_pid[MOTOR_3],hand_Motor[MOTOR_3]->speed_rpm,speedSet[MOTOR_3]);
 
+    for(int i = 0;i < MOTOR_NUM;i ++)
+    {
+        result[i]=LowPassFilter2p_Apply(filter.out + i, result[i]);
+    }
+
 }
 
 	int flag =0;

User/module/ball.hpp

@@ -8,6 +8,7 @@
 #include "bsp_delay.h"
 #include "djiMotor.h"
 #include "pid.h"
+#include "filter.h"
 
 // 定义状态枚举
 typedef enum {
@@ -33,6 +34,12 @@ typedef enum
     MOTOR_NUM
 }motorhangd_e;
 
+  /* 滤波器 */
+  typedef struct {
+    LowPassFilter2p_t *in;  /* 反馈值滤波器 */
+    LowPassFilter2p_t *out; /* 输出值滤波器 */
+  } Filter;
+
 // 定义光电传感器检测宏
 #define IS_PHOTOELECTRIC_BALL() (HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin) == GPIO_PIN_RESET)
 
@@ -40,6 +47,7 @@ class Ball
 {
 public:
     Ball();
+    void Filter_init(float target_freq);
     void Spin(float speed,float speed2);
     void ballHadling(void);
     void Send_control(void);
@@ -59,6 +67,9 @@ public:
     volatile uint32_t flag_thread;//接收传回的线程通知
 
 private:  
+    //滤波一下
+    Filter filter;
+
     //三个摩擦轮
     static const float M3508_speed_PID[3];
     static const float M3508_angle_PID[3];