forked from logzhan/RobotHardware-UESTC
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<EFBFBD>ĵ<EFBFBD><EFBFBD><EFBFBD>PID<EFBFBD><EFBFBD><EFBFBD>Ʋ<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
********************************************************************************/
|
|||
|
|
#include "pid.h"
|
|||
|
|
|
|||
|
|
/* <20><><EFBFBD><EFBFBD>ʽpid,<2C><><EFBFBD><EFBFBD>PID<49><44><EFBFBD>غ<EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>趨<EFBFBD>Ե<EFBFBD><D4B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӱ<EFBFBD><D3B0><EFBFBD>dz<EFBFBD><C7B3><EFBFBD> */
|
|||
|
|
float Proportion = 0.6; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> Proportional Const
|
|||
|
|
float Integral = 0.1; // <20><><EFBFBD>ֳ<EFBFBD><D6B3><EFBFBD> Integral Const
|
|||
|
|
float Derivative = 0; // <>ֳ<EFBFBD><D6B3><EFBFBD> Derivative Const
|
|||
|
|
|
|||
|
|
//<2F><>ʾpid<69><64><EFBFBD><EFBFBD><EFBFBD><EFBFBD>OLED<45><44>
|
|||
|
|
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);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/********************<2A><><EFBFBD><EFBFBD>ʽPID<49><44><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>************************************/
|
|||
|
|
//NextPoint<6E><74>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>ֵ
|
|||
|
|
//SetPoint<6E>趨ֵ
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>PID
|
|||
|
|
int PID_Calc_Left(int NextPoint,int SetPoint)
|
|||
|
|
{
|
|||
|
|
static int LastError; // Error[-1]
|
|||
|
|
static int PrevError; // Error[-2]
|
|||
|
|
int iError,Outpid; // <20><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>
|
|||
|
|
|
|||
|
|
iError=SetPoint-NextPoint; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
Outpid=(Proportion * iError) // E[k]<5D><>
|
|||
|
|
-(Integral * LastError) // E[k-1]<5D><>
|
|||
|
|
+(Derivative * PrevError); // E[k-2]<5D><>
|
|||
|
|
|
|||
|
|
PrevError=LastError; // <20>洢<EFBFBD><E6B4A2><EFBFBD><EFBFBD><EEA3AC><EFBFBD><EFBFBD><EFBFBD>´μ<C2B4><CEBC><EFBFBD>
|
|||
|
|
LastError=iError;
|
|||
|
|
return(Outpid); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
//<2F><><EFBFBD><EFBFBD>PID
|
|||
|
|
int PID_Calc_Right(int NextPoint,int SetPoint)
|
|||
|
|
{
|
|||
|
|
static int LastError; // Error[-1]
|
|||
|
|
static int PrevError; // Error[-2]
|
|||
|
|
int iError,Outpid; // <20><>ǰ<EFBFBD><C7B0><EFBFBD><EFBFBD>
|
|||
|
|
|
|||
|
|
iError = SetPoint-NextPoint; // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
Outpid = (Proportion * iError) // E[k]<5D><>
|
|||
|
|
-(Integral * LastError) // E[k-1]<5D><>
|
|||
|
|
+(Derivative * PrevError); // E[k-2]<5D><>
|
|||
|
|
|
|||
|
|
PrevError=LastError; // <20>洢<EFBFBD><E6B4A2><EFBFBD><EFBFBD><EEA3AC><EFBFBD><EFBFBD><EFBFBD>´μ<C2B4><CEBC><EFBFBD>
|
|||
|
|
LastError=iError;
|
|||
|
|
return(Outpid); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
|
|||
|
|
}
|
|||
|
|
|