/*
 * 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=50/2;
double PID_KD=12/2;
//double Sys_T=0.01;//System time

/**喷漆PID参数**/
double Kp = 80;
double Ki = 0;
double Kd  = 5;
double k_1_error = 0;
double k_2_error = 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;

}


void GF_MSP_Auto_adj_Unicycle(double Current_Angle_11,double Desire_Angle_11, double Auto_Speed, double Auto_Speed_Max, double System_time ,double *W_Speed_1)
{
	//水平 前进抬车头
   double Detal_Angle=fabs(Current_Angle_11-Desire_Angle_11);
   double Incre_Speed[1];

   Speedl_PID(Current_Angle_11,Desire_Angle_11, System_time, Incre_Speed);
   Incre_Speed[0]=1*Incre_Speed[0];

   double deltaAngle0=1;
   double deltaAngle1=2;
   double deltaAngle2=5;
   double deltaAngle3=10;
   //double deltaAngle4=25;
   double LeftSpeed_Con_1;

    if(Detal_Angle<= deltaAngle0)
    {
    	LeftSpeed_Con_1 =0;
    }
    else if(Detal_Angle<= deltaAngle1)
    {
    	LeftSpeed_Con_1 = (Auto_Speed + Kp1 * Incre_Speed[0]/2);
    }
    else if(Detal_Angle > deltaAngle1 && Detal_Angle <= deltaAngle2)
    {
    	LeftSpeed_Con_1 = (Auto_Speed  + Kp2 * Incre_Speed[0]/2);
    }
    else if(Detal_Angle > deltaAngle2 && Detal_Angle <= deltaAngle3)
    {
    	LeftSpeed_Con_1 = (Auto_Speed  + Kp3 * Incre_Speed[0]/2);
    }
    else
    {
    	LeftSpeed_Con_1 = (Auto_Speed  + Kp4 * Incre_Speed[0]/2);
    }

    if(LeftSpeed_Con_1>Auto_Speed_Max )
    {
    	LeftSpeed_Con_1=Auto_Speed_Max;
	}
	else if(LeftSpeed_Con_1<-Auto_Speed_Max)
	{
		LeftSpeed_Con_1=-Auto_Speed_Max;
	}
	else
	{
		LeftSpeed_Con_1=LeftSpeed_Con_1;
	}
    W_Speed_1[0]=LeftSpeed_Con_1;
}




//喷漆机器人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;
     //
     if (pidTargetValue - pidCurrentValue >= 180)
     {
         Error = (pidTargetValue - pidCurrentValue) - 360;
     }
     if (pidTargetValue - pidCurrentValue <= -180)
     {
         Error = pidTargetValue - pidCurrentValue + 360;
     }
     //计算误差
     P_Error = Error;                                            //比例环节
     I_Error = Error + all_error;           //积分环节
     D_Error = Error - k_1_error;          //微分环节

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



     return deltaSpeed;
}













