- /**
- ******************************************************************************
- * @file PID_Control.h
- * @author willieon
- * @version V0.1
- * @date January-2015
- * @brief PID控制算法头文件
- * 定义结构体类型以及声明函数
- * #define IF_THE_INTEGRAL_SEPARATION 0/1 为积分分离标志
- ******************************************************************************
- **/
-
- #ifndef __PID_CONTROL_H__
- #define __PID_CONTROL_H__
-
- #define IF_THE_INTEGRAL_SEPARATION 0
- //#define IF_THE_INTEGRAL_SEPARATION 1 //是否积分分离 0-不分离,1 -分离
-
- typedef struct
- {
- double SetPoint; // 设定目标 Desired Value
- double Proportion; // 比例常数 Proportional Const
- double Integral; // 积分常数 Integral Const
- double Derivative; // 微分常数 Derivative Const
- double LastError; // Error[-1]
- double PrevError; // Error[-2]
- double SumError; // Sums of Errors
- }PID;
-
- #if IF_THE_INTEGRAL_SEPARATION //是否积分分离预编译开始
-
- double PIDCalc(double NextPoint ,double SepLimit, PID *pp); //带积分分离的PID运算
-
- #else
-
- double PIDCalc( double NextPoint, PID *pp); //不带积分分离的PID运算
-
- #endif //是否积分分离预编译结束
-
- void PIDInit (double SetPoint, double Proportion, double Integral, double Derivative, PID *pp);
-
- #endif
复制代码
- /**
- ******************************************************************************
- * @file PID_Control.c
- * @author willieon
- * @version V0.1
- * @date January-2015
- * @brief PID控制算法函数代码
- *
- *
- ******************************************************************************
- **/
-
- #include "PID_Control.h"
- #include "math.h"
-
- /*************************************************************************************
- * 名 称: double PIDCalc( PID *pp, double NextPoint ,double SepLimit)
- * 功 能: PID控制运算
- * 入口参数: PID *pp - 定义的运算所需变量的结构体
- * NextPoint - 负反馈输入值
- * SepLimit - 积分分离上限
- * 出口参数: 返回PID控制量
- * 说 明: 默认不进行积分分离,如果用户需要使用积分分离,需在PID_Control.h中
- * 将 #define IF_THE_INTEGRAL_SEPARATION 0 改为
- * #define IF_THE_INTEGRAL_SEPARATION 1
- * 调用方法: 进行积分分离时入口参数为3个,具体方法如下:
- * PID PIDControlStruct ; //定义PID运算结构体
- * PIDInit(50, 0.24, 0.04, 0.2, &PIDControlStruct);//结构体初始化,注意&符号不能省
- * ControlData = PIDCalc(ReadData, 200, &PIDControlStruct); //控制量 = PIDCalc(反馈值,积分分离上限,PID运算结构体)
- *
- ***************************************************************************************
- */
-
- #if IF_THE_INTEGRAL_SEPARATION
-
- double PIDCalc(double NextPoint ,double SepLimit, PID *pp)
- {
- double dError, Error,Flag;
- Error = pp->SetPoint - NextPoint; // 偏差
- if(abs(Error) > SepLimit) //当偏差大于分离上限积分分离
- {
- Flag = 0;
- }
- else //当偏差小于分离上限,积分项不分离
- {
- Flag = 1;
- pp->SumError += Error; // 积分
- }
- dError = pp->LastError - pp->PrevError; // 当前微分
- pp->PrevError = pp->LastError;
- pp->LastError = Error;
- return (
- pp->Proportion * Error // 比例项
- + Flag * pp->Integral * pp->SumError // 积分项
- + pp->Derivative * dError // 微分项
- );
- }
-
- #else
-
- double PIDCalc( double NextPoint, PID *pp)
- {
- double dError, Error;
- Error = pp->SetPoint - NextPoint; // 偏差
- pp->SumError += Error; // 积分
- dError = pp->LastError - pp->PrevError; // 当前微分
- pp->PrevError = pp->LastError;
- pp->LastError = Error;
- return (pp->Proportion * Error // 比例项
- + pp->Integral * pp->SumError // 积分项
- + pp->Derivative * dError // 微分项
- );
- }
-
- #endif
-
-
- /*************************************************************************************
- * 名 称: double PIDCalc( PID *pp, double NextPoint ,double SepLimit)
- * 功 能: PID初始化设定
- * 入口参数: PID *pp - 定义的运算所需变量的结构体
- * SetPoint - 设定的目标值
- * Proportion,Integral ,Derivative - P,I,D系数
- * 出口参数: 无
- * 说 明:
- * 调用方法: PID PIDControlStruct ; //定义PID运算结构体
- * PIDInit(50, 0.24, 0.04, 0.2, &PIDControlStruct);//结构体初始化,注意&符号不能省
- * 因为函数需要传入一个指针,需要对结构体取首地址传给指针
- *
- ***************************************************************************************
- */
-
-
- void PIDInit (double SetPoint, double Proportion, double Integral, double Derivative, PID *pp)
- {
- pp -> SetPoint = SetPoint; // 设定目标 Desired Value
- pp -> Proportion = Proportion; // 比例常数 Proportional Const
- pp -> Integral = Integral; // 积分常数 Integral Const
- pp -> Derivative = Derivative; // 微分常数 Derivative Const
- pp -> LastError = 0; // Error[-1]
- pp -> PrevError = 0; // Error[-2]
- pp -> SumError = 0; // Sums of Errors
-
- //memset ( pp,0,sizeof(struct PID)); //need include "string.h"
- }
复制代码
好了,现在把卡尔曼滤波和PID算法联合起来,在VS平台中运行实验
- #include "kalman.h"
-
- #include "stdio.h"
- #include "stdlib.h"
- #include "PID_Control.h"
-
- void main(void)
-
- {
- KalmanCountData k;
- PID PIDControlStruct;
- Kalman_Filter_Init(&k);
- PIDInit(50, 1, 0.04, 0.2, &PIDControlStruct);
- int m,n;
-
- double out;
-
- for(int a = 0;a<80;a++)
- {
- m = 1+ rand() %100;
- n = 1+ rand() %100;
- Kalman_Filter((float)m,(float)n,&k);
- out = PIDCalc(k.Angle_Final, &PIDControlStruct);
- printf("%3d and %3d is %6f -pid- %6f\r\n",m,n,k.Angle_Final,out);
-
- }
- }
复制代码
写到此处,我觉得,自平衡小车的两座大山应该对读者来说不是问题了,只要会调用函数,会做参数整定,就完全可以解决数据采集和数据融合以及控制了。
好吧!我承认我的代码是仿照 STM32的库的风格写的,真心觉得这种代码风格很NB。学习ing!
代码本人测试过,如果有不足之处望不吝赐教!请直接跟帖讨论
我最近在做的平衡车,硬件清单:
香蕉电机+轮子 6.4元*2
STM32F103C8Tb 最小系统板 31.5元*1
newwayL298N 光耦隔离电机驱动板 30元 *1
MPU6050姿态传感器 9.3元*1
LM2596S DC-DC 降压电源模块 3.2元*1
蓝牙4.0 BLE从模块串口通信+直驱模式 CC2540 CC2541 RF-BM-S02 23元*1
光电测速传感器模块 4.8元*2
小车底盘用自己的小雕刻机雕的亚克力板,感觉可以用薄的铝板做中间层,这样底层放光电测速和姿态传感器+薄铝底板+电动机驱动板+铝底板+STM32主控,可以屏蔽一部分干扰,在inventor里面建模画好图!
之前没有用带光耦隔离的驱动,速度一快STM32就死机,后来换光耦隔离才解决!
|