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.
95 lines
1.9 KiB
95 lines
1.9 KiB
/*
|
|
* 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);
|
|
}
|
|
|
|
|
|
|
|
|