ADC显示消抖优化算法 && PID算法示例

// ADC 消抖显示优化算法

/*  优化的消抖滤波法
1、方法：
    设置一个滤波计数器
    将每次采样值与当前有效值比较：
    如果采样值＝当前有效值，则计数器复位
	方法1：
    如果采样值>或<当前有效值，则计数器+1，并判断计数器是否>=上限N(溢出)
    如果计数器溢出，则将本次值替换当前有效值，并复位数器
	方法2：（本算法）
	如果采样值>当前有效值，则计数器+1，如果采样值<当前有效值，则计数器-1，并判断计数器是否>=上限N(溢出)或<=下限N(下溢出)
	如果计数器溢出，则计算溢出的N点（分上溢N点和下溢N点）均值作为当前有效值，并复位数器。
2、优点：
    对于变化缓慢的被测参数有较好的滤波效果，
    可避免在临界值附近控制器的反复开/关跳动或显示器上数值抖动
3、缺点：
    对于快速变化的参数不宜
    如果在计数器溢出的那一次采样到的值恰好是干扰值，则会将干扰值当作有效值导入交易系统
*/

#define  TARGET_N_I  (5u)
#define  TARGET_N_V  (5u)
typedef struct ShakeFreeFilterType {
  uint16_t count;
  uint16_t target_n;
  uint32_t value;
  uint32_t new_value;
  uint8_t first_flag;
  uint32_t *temp_value;
} SFF_T;

static uint32_t sff_i_buf[2*TARGET_N_I] = {0};
static SFF_T sff_i = {
  .count = TARGET_N_I,
  .value = 0,
  .target_n = TARGET_N_I,
  .new_value = 0,
  .first_flag = 0,
  .temp_value = &sff_i_buf[0],
};
static uint32_t sff_v_buf[2*TARGET_N_V] = {0};
static SFF_T sff_v = {
  .count = TARGET_N_V,
  .value = 0,
  .target_n = TARGET_N_V,
  .new_value = 0,
  .first_flag = 0,
  .temp_value = &sff_v_buf[0],
};
//
static uint32_t ShakeFreeFilter(SFF_T *sff_t)
{
  uint64_t temp = 0;
  if (!sff_t->first_flag)
  {
    sff_t->first_flag = 1;
    sff_t->new_value = sff_t->value;
  }

  if (sff_t->value != sff_t->new_value)
  {
    sff_t->temp_value[sff_t->count] = sff_t->value; // 取趋势
    if (sff_t->value > sff_t->new_value)
    {
      sff_t->count++;
    }
    else
    {
      sff_t->count--;
    }
    // 更新显示
    if (sff_t->count >= 2*sff_t->target_n)
    {	
      sff_t->count = sff_t->target_n;
      for (int i = sff_t->target_n; i < 2*sff_t->target_n; i++)
      {
        temp += sff_t->temp_value[i];
      }
      sff_t->new_value = temp / sff_t->target_n;
    }
    else if (sff_t->count <= 0)
    {
      sff_t->count = sff_t->target_n;
      for (int i = sff_t->target_n; i > 0; i--)
      {
        temp += sff_t->temp_value[i];
      }
      sff_t->new_value = temp / sff_t->target_n;
    }
  }
  else
  {
    sff_t->count = sff_t->target_n;
  }
  return sff_t->new_value;
}

uint32_t ShakeFreeFilter_i(uint32_t value)
{
  sff_i.value = value;
  return ShakeFreeFilter(&sff_i);
}
//
uint32_t ShakeFreeFilter_v(uint32_t value)
{
  sff_v.value = value;
  return ShakeFreeFilter(&sff_v);
}

// 下面PID示例

pid.h

#ifndef __PID_H__
#define __PID_H__

#ifdef __cplusplus
extern "C" {
#endif

#include <stdio.h>

// PID算法结构体
typedef struct {
    float SetSpeed;     // 设定速度
    float ActualSpeed;  // 当前速度
    float Kp;           // 比例增益
    float Ki;           // 积分增益
    float Kd;           // 微分增益
    float Integral;     // 积分项
    float LastError;    // 上一个误差
} PID;

// PID算法初始化
void PID_init(PID *pid, float setSpeed, float kp, float ki, float kd);
// PID计算
float PID_calculate(PID *pid, float actualSpeed);

#ifdef __cplusplus
}
#endif

#endif 

pid.c

/*  pid算法C

PID算法是一种常用于控制的算法，它包含三个要素：比例（Proportional）、积分（Integral）和微分（Derivative）。
PID算法的基本公式如下：
输出(torque或力) = Kp * (设定点 - 当前点) + Ki * 积分项 + Kd * 微分项
其中，Kp为比例增益，Ki为积分增益，Kd为微分增益。
下面是一个简单的C语言实现PID算法的例子：
*/

#include "pid.h"

// PID算法初始化
void PID_init(PID *pid, float setSpeed, float kp, float ki, float kd)
{
    pid->SetSpeed = setSpeed;
    pid->Kp = kp;
    pid->Ki = ki;
    pid->Kd = kd;
    pid->Integral = 0;
    pid->LastError = 0;
}
 
// PID计算
float PID_calculate(PID *pid, float actualSpeed)
{
    float error;        // 误差
	float derivative	// 微分
    pid->ActualSpeed = actualSpeed;
    error = pid->SetSpeed - pid->ActualSpeed;  // 误差计算
    pid->Integral += error;  // 积分项计算
    derivative = pid->Kd * (pid->LastError - error);  // 微分项计算
    pid->LastError = error;
    return pid->Kp * error + pid->Ki * pid->Integral + derivative;  // PID输出
}

//int main(void)
int pid_test(void)
{
    PID pid;
    PID_init(&pid, 100, 1, 0.001, 0.0001);  // 初始化PID参数
 
    float actualSpeed = 0;  // 当前速度
    float output;
 
    for (int i = 0; i < 100; i++)
	{
        output = PID_calculate(&pid, actualSpeed);  // 计算PID输出
        actualSpeed += output;  // 当前速度增加PID输出
        printf("PID Output: %f, Actual Speed: %f\n", output, actualSpeed);
    }

    return 0;
}