You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
511 lines
14 KiB
511 lines
14 KiB
/*
|
|
* msp_PID.c
|
|
*
|
|
* Created on: 2024年1月18日
|
|
* Author: Administrator
|
|
*/
|
|
|
|
#include "msp_PID.h"
|
|
#include <math.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;
|
|
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;
|
|
//}
|
|
|
|
|