72 lines
2.7 KiB
C
72 lines
2.7 KiB
C
/******************** (C) COPYRIGHT 2023 GeekRebot *****************************
|
|
* File Name : pid.c
|
|
* Current Version : V1.0 & ST 3.5.0
|
|
* Author : zhanli 719901725@qq.com
|
|
* Date of Issued : 2023.04.06 zhanli: Create
|
|
* Comments : GeekRebot的电机PID控制部分
|
|
********************************************************************************/
|
|
#include "pid.h"
|
|
|
|
/* 增量式pid,定义PID相关宏,这三个参数设定对电机运行影响非常大 */
|
|
float Proportion = 0.6; // 比例常数 Proportional Const
|
|
float Integral = 0.1; // 积分常数 Integral Const
|
|
float Derivative = 0; // 微分常数 Derivative Const
|
|
|
|
//显示pid参数到OLED上
|
|
void showPID(void)
|
|
{
|
|
// float temp1,temp2,temp3;
|
|
// char PID_P[3],PID_I[3],PID_D[3];
|
|
// temp1 = Proportion;
|
|
// sprintf(PID_P,"%1.1f",temp1);
|
|
// OLED_ShowString(16,4, (u8*)PID_P,16);
|
|
|
|
// temp2 = Integral;
|
|
// sprintf(PID_I,"%1.1f",temp2);
|
|
// OLED_ShowString(56,4, (u8*)PID_I,16);
|
|
|
|
// temp3 = Derivative;
|
|
// sprintf(PID_D,"%1.1f",temp3);
|
|
// OLED_ShowString(104,4, (u8*)PID_D,16);
|
|
}
|
|
|
|
/********************增量式PID控制设计************************************/
|
|
//NextPoint当前输出值
|
|
//SetPoint设定值
|
|
|
|
//左轮PID
|
|
int PID_Calc_Left(int NextPoint,int SetPoint)
|
|
{
|
|
static int LastError; // Error[-1]
|
|
static int PrevError; // Error[-2]
|
|
int iError,Outpid; // 当前误差
|
|
|
|
iError=SetPoint-NextPoint; // 增量计算
|
|
Outpid=(Proportion * iError) // E[k]项
|
|
-(Integral * LastError) // E[k-1]项
|
|
+(Derivative * PrevError); // E[k-2]项
|
|
|
|
PrevError=LastError; // 存储误差，用于下次计算
|
|
LastError=iError;
|
|
return(Outpid); // 返回增量值
|
|
}
|
|
|
|
|
|
//右轮PID
|
|
int PID_Calc_Right(int NextPoint,int SetPoint)
|
|
{
|
|
static int LastError; // Error[-1]
|
|
static int PrevError; // Error[-2]
|
|
int iError,Outpid; // 当前误差
|
|
|
|
iError = SetPoint-NextPoint; // 增量计算
|
|
Outpid = (Proportion * iError) // E[k]项
|
|
-(Integral * LastError) // E[k-1]项
|
|
+(Derivative * PrevError); // E[k-2]项
|
|
|
|
PrevError=LastError; // 存储误差，用于下次计算
|
|
LastError=iError;
|
|
return(Outpid); // 返回增量值
|
|
}
|
|
|