日志
PID 位置环 速度环 电流环
||
位置环控制器的输出是速度，作为速度环的输入，而速度环控制器的输出是电流，作为电流环的输入,电流环的输出是电机控制占空比
Angle_Delta = Angle_Now - Angle_Pre;
scale_adjust(&Angle_Delta);
location=location-Angle_Delta;
PIDsum1.p = PID1.p * location;
PIDsum1.i += PID1.i * location * dt;
PIDsum1.d = PID1.d * (location - location_pre) / dt;
Target = (int)(PIDsum1.p + PIDsum1.i + PIDsum1.d);
Angle_Error =Target - Angle_Delta;
PIDsum2.p = PID2.p * Angle_Error;
PIDsum2.i += PID2.i * Angle_Error * dt;
PIDsum2.d = PID2.d * (Angle_Error - Angle_Error_Pre) / dt;
I_Value = (int)(PIDsum2.p + PIDsum2.i + PIDsum2.d);
I_Error=I_Value-I_now;
PIDsum3.p = PID3.p * I_Error;
PIDsum3.i += PID3.i * I_Error * dt;
PIDsum3.d = PID3.d * (I_Error - I_Error_Pre) / dt;
ControlLaw = (int)(PIDsum3.p + PIDsum3.i + PIDsum3.d);
Angle_Delta = Angle_Now - Angle_Pre;
scale_adjust(&Angle_Delta);
location=location-Angle_Delta;
PIDsum1.p = PID1.p * location;
PIDsum1.i += PID1.i * location * dt;
PIDsum1.d = PID1.d * (location - location_pre) / dt;
Target = (int)(PIDsum1.p + PIDsum1.i + PIDsum1.d);
Angle_Error =Target - Angle_Delta;
PIDsum2.p = PID2.p * Angle_Error;
PIDsum2.i += PID2.i * Angle_Error * dt;
PIDsum2.d = PID2.d * (Angle_Error - Angle_Error_Pre) / dt;
I_Value = (int)(PIDsum2.p + PIDsum2.i + PIDsum2.d);
I_Error=I_Value-I_now;
PIDsum3.p = PID3.p * I_Error;
PIDsum3.i += PID3.i * I_Error * dt;
PIDsum3.d = PID3.d * (I_Error - I_Error_Pre) / dt;
ControlLaw = (int)(PIDsum3.p + PIDsum3.i + PIDsum3.d);
