/*
 * msp_PID.c
 *
 *  Created on: 2024年1月18日
 *      Author: Administrator
 */

#include <math.h>
#include <stdint.h>

void Speedl_PID(double CurrentValue, double TargetValue, double Sys_T,
        double *Gain_speed);
double Desire_Angle = 0;
double PID_KP = 50;
double PID_KD = 12;
//double Sys_T=0.01;//System time
double k_1_error = 0;


/* PID
 * CurrentAngle :0.01度
 * TargetAngle	:0.01度
 * MaxValue	:0.1rpm
 * */
double Angle_Tune_PID(double CurrentAngle, double TargetAngle, double Position_KP,
		double Position_KI, double Position_KD, double MaxValue)
{
	static double Bias, delta_Speed, Integral_bias, Last_Bias;
	/*
	 if (TargetValue - CurrentValue >= 180)
	 {
		 Bias = (TargetValue - CurrentValue) - 360;
	 }
	 if (pidTargetValue - pidCurrentValue <= -180)
	 {
		 Bias = TargetValue - CurrentValue + 360;
	 }
	*/

	Bias = CurrentAngle - TargetAngle;
	Integral_bias += Bias;


	delta_Speed = Position_KP * Bias + Position_KI * Integral_bias
	        + Position_KD * (Bias - Last_Bias);
	Last_Bias = Bias;
	if (delta_Speed >= MaxValue)
	{
		delta_Speed = MaxValue;
	}
	if (delta_Speed <= -MaxValue)
	{
		delta_Speed = -MaxValue;
	}

	return  delta_Speed;
}



//喷漆机器人PID算法
int32_t PositionalPID(double pidTargetValue, double pidCurrentValue, double Kp,
        double Kd)
{
	double Error = 0;
	double P_Error = 0;
	double D_Error = 0;
	double delta = 0;

	Error = pidTargetValue - pidCurrentValue;
	//
	if (pidTargetValue - pidCurrentValue >= 180)
	{
		Error = (pidTargetValue - pidCurrentValue) - 360;
	}
	if (pidTargetValue - pidCurrentValue <= -180)
	{
		Error = pidTargetValue - pidCurrentValue + 360;
	}
	//计算误差
	P_Error = Error;                                            //比例环节

	D_Error = Error - k_1_error;          //微分环节

	//deltaSpeed = Kp * P_Error + Ki * I_Error + Kd * D_Error;      //计算Speed输出值

	delta = Kp * P_Error + Kd * D_Error;

	k_1_error = Error;

	return (int32_t) (delta);
}