/* * msp_PID.c * * Created on: 2024年1月18日 * Author: Administrator */ #include "msp_PID.h" #include 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; 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) 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;, 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; } void GF_MSP_Auto_Motion_adj_Com(double Current_Angle, double Horizontal_Compen_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; if (Desire_Angle > 180) { Desire_Angle = Desire_Angle - 360; } } //水平 前进抬车头 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_PID_Now_Der_adj_Com(double Current_Angle, double Desire_Angle_Input, double Auto_Speed, double Auto_Speed_Max, double System_time, double *W_Speed_1) { Desire_Angle = Desire_Angle_Input; //水平 前进抬车头 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; } double Error = 0; double P_Error = 0; double I_Error = 0; double D_Error = 0; double delta = 0; //喷漆机器人PID算法 int32_t PositionalPID(double pidTargetValue, double pidCurrentValue, double Kp, double Kd) { 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); } int Angle_Tune_PID(float CurrentAngle, float TargetAngle, int Position_KP, int Position_KI, int Position_KD, int MaxValue) { static float Bias, delta_Speed, Integral_bias, Last_Bias; 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 (int) delta_Speed; } //int Angle_Tune_PID(float CurrentAngle, float TargetAngle, int Position_KP, // int Position_KI, int Position_KD, int MaxValue) //{ // static float Bias, delta_Speed, Integral_bias, Last_Bias; // 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 (int) delta_Speed; //}