|
|
|
@ -43,8 +43,8 @@ void GF_MSP_PID_Now_Der_adj_Com_Weld(double Current_Angle, double Desire_Angle_I |
|
|
|
/* 将角度归一化到 [-180, 180) */ |
|
|
|
static float normalize_angle(float angle) |
|
|
|
{ |
|
|
|
while (angle >= 180.0f) angle -= 360.0f; |
|
|
|
while (angle < -180.0f) angle += 360.0f; |
|
|
|
if (angle >= 180.0f) angle -= 360.0f; |
|
|
|
else if (angle < -180.0f) angle += 360.0f; |
|
|
|
return angle; |
|
|
|
} |
|
|
|
|