/******************** (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);                                           // 返回增量值
}