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

#include "MSP/msp_PID.h"



void  Speedl_PID(double CurrentValue , double TargetValue, double Sys_T, double *Gain_speed);


double Desire_Angle=0;
double PID_KP=100/2;
double PID_KD=25/2;
//double Sys_T=0.01;//System time
double k_1_error=0;

/**喷漆PID参数**/
double Kp = 40;
double Ki = 0;
double Kd  = 20;
double k_error_1 = 0;
double k_error_2 = 0;
double all_error = 0;

void  Speedl_PID(double CurrentValue , double TargetValue, double Sys_T, double *Gain_speed)
{
	 double Error;
	 double P_Error;
	 double D_Error;
	 Error = TargetValue -CurrentValue;

	 if (Error >= 180)
	 {
		 Error = Error - 360;
	 }
	 else if (Error <= -180)
	 {
		 Error = Error + 360;
	 }

	 P_Error = Error;
	 D_Error = Error - k_1_error;
	 k_1_error=Error;
	 Gain_speed[0] = PID_KP * P_Error  + PID_KD * D_Error/Sys_T;
}

double Kp1=1;
double Kp2=0.950;
double Kp3=0.80;
double Kp4=0.50;
/* Input:
*/
void GF_MSP_Auto_Motion_adj(double Current_Angle, double Auto_Speed, double Auto_Speed_Max, double System_time ,double *W_Speed_1)
{
		 // double Desire_Angle=0;
	double Horizontal_Compen_Angle=0;
	 if((Current_Angle>=45)&(Current_Angle<135))
	 {
		 Desire_Angle = 90 + Horizontal_Compen_Angle;
	 }
	 else if ((Current_Angle<=-45)&(Current_Angle>=-135))
	 {
		 Desire_Angle = -90 + Horizontal_Compen_Angle;
	 }
	 else if ((Current_Angle>-45)&(Current_Angle<45))
	 {
		 Desire_Angle = 0 + Horizontal_Compen_Angle;
	 }
	 else
	 {
		 Desire_Angle = 180 + Horizontal_Compen_Angle;
	 }

			//水平 前进抬车头
   double Detal_Angle=fabs(Current_Angle-Desire_Angle);
   double Incre_Speed[1];
   Speedl_PID(Current_Angle,Desire_Angle, System_time, Incre_Speed);
   Incre_Speed[0]=1*Incre_Speed[0];
   double deltaAngle1=2;
   double deltaAngle2=5;
   double deltaAngle3=10;
 //  double deltaAngle4=25;

   double LeftSpeed_Con_1; double RightSpeed_Con_1;double LeftSpeed_Con_2; double RightSpeed_Con_2;
    if ( Detal_Angle<= deltaAngle1)
    {
	LeftSpeed_Con_1 = (Auto_Speed + Kp1 * Incre_Speed[0]);
	RightSpeed_Con_1 = (Auto_Speed  - Kp1 * Incre_Speed[0]);
	LeftSpeed_Con_2 =Auto_Speed + Kp1 * Incre_Speed[0];
	RightSpeed_Con_2=Auto_Speed - Kp1 * Incre_Speed[0];
//小角度时，两前轮调整
    }
    else if (Detal_Angle > deltaAngle1 && Detal_Angle <= deltaAngle2)
    {
	LeftSpeed_Con_1 = (Auto_Speed  + Kp2 * Incre_Speed[0]);
	RightSpeed_Con_1 = (Auto_Speed  - Kp2 * Incre_Speed[0]);
	LeftSpeed_Con_2 =Auto_Speed + Kp2 * Incre_Speed[0];
	RightSpeed_Con_2=Auto_Speed - Kp2 * Incre_Speed[0];
//中小角度时，两前轮差速，两后轮跟踪调整
    }
    else if (Detal_Angle > deltaAngle2 && Detal_Angle <= deltaAngle3)
    {
	LeftSpeed_Con_1 = (Auto_Speed  + Kp3 * Incre_Speed[0]);
	RightSpeed_Con_1 = (Auto_Speed - Kp3 * Incre_Speed[0]);
	LeftSpeed_Con_2 =Auto_Speed + Kp3 * Incre_Speed[0];
	RightSpeed_Con_2=Auto_Speed - Kp3 * Incre_Speed[0];
    }
    else
    {
	LeftSpeed_Con_1 = (Auto_Speed  + Kp4 * Incre_Speed[0]);
	RightSpeed_Con_1 = (Auto_Speed  - Kp4 * Incre_Speed[0]);
	LeftSpeed_Con_2 =Auto_Speed + Kp4 * Incre_Speed[0];
	RightSpeed_Con_2=Auto_Speed - Kp4 * Incre_Speed[0];
    }

  double Jud_value=fabs (LeftSpeed_Con_1) -fabs(RightSpeed_Con_1);
  double factor=0;
  if(Jud_value>=0)
  {
	  if (LeftSpeed_Con_1>Auto_Speed_Max )
	  {
		 factor=Auto_Speed_Max/LeftSpeed_Con_1;
		 LeftSpeed_Con_1=Auto_Speed_Max;
		 LeftSpeed_Con_2=LeftSpeed_Con_2*factor;
		 RightSpeed_Con_1=RightSpeed_Con_1*factor;
		 RightSpeed_Con_2=RightSpeed_Con_2*factor;
	  }
	  else if (LeftSpeed_Con_1<-Auto_Speed_Max)
	 {
		 factor=-Auto_Speed_Max/LeftSpeed_Con_1;
		 LeftSpeed_Con_1=-Auto_Speed_Max;
		 LeftSpeed_Con_2=LeftSpeed_Con_2*factor;
		 RightSpeed_Con_1=RightSpeed_Con_1*factor;
		 RightSpeed_Con_2=RightSpeed_Con_2*factor;
	 }
  }
  else
  {
	  if (RightSpeed_Con_1>Auto_Speed_Max)
	  {
		 factor=Auto_Speed_Max/RightSpeed_Con_1;
		 RightSpeed_Con_1=Auto_Speed_Max;
		 RightSpeed_Con_2=RightSpeed_Con_2*factor;

		 LeftSpeed_Con_1=LeftSpeed_Con_1*factor;
		 LeftSpeed_Con_2=LeftSpeed_Con_2*factor;
	  }
	  else if (LeftSpeed_Con_2<-Auto_Speed_Max)
	 {
		factor=-Auto_Speed_Max/RightSpeed_Con_1;
		 RightSpeed_Con_1=-Auto_Speed_Max;
		 RightSpeed_Con_2=RightSpeed_Con_2*factor;

		 LeftSpeed_Con_1=LeftSpeed_Con_1*factor;
		 LeftSpeed_Con_2=LeftSpeed_Con_2*factor;
	 }
  }

  W_Speed_1[0]=LeftSpeed_Con_1;
  W_Speed_1[1]=RightSpeed_Con_1;
  W_Speed_1[2]=LeftSpeed_Con_2;
  W_Speed_1[3]=RightSpeed_Con_2;

}


//喷漆机器人PID算法
double PositionalPID(double pidTargetValue, double pidCurrentValue)
{
     double Error = 0;
     double P_Error = 0;
     double I_Error = 0;
     double D_Error = 0;
     double deltaSpeed = 0;

     Error = pidTargetValue - pidCurrentValue;

     //计算误差
     P_Error = Error;                                            //比例环节
     I_Error = Error + all_error;           //积分环节
     D_Error = Error - k_error_1;          //微分环节

     deltaSpeed = Kp * P_Error + Ki * I_Error + Kd * D_Error;      //计算Speed输出值
    // deltaSpeed = deltaSpeed*500/3;
     k_error_2 = k_error_1;
     k_error_1 = Error;
     all_error = I_Error;

     return deltaSpeed;
}



