Speed_Error=Speed_Ref - Speed_Fdb;
Speed_Up=_IQmpy(Speed_Kp,Speed_Error);
Speed_Ui=Speed_Ui + _IQmpy(Speed_Ki,Speed_Up) + _IQmpy(Speed_Ki,Speed_SatError);
Speed_OutPreSat=Speed_Up+Speed_Ui;
if(Speed_OutPreSat>Speed_OutMax)
Speed_Out=Speed_OutMax;
else if(Speed_OutPreSat<Speed_OutMin)
Speed_Out=Speed_OutMin;
else
Speed_Out=Speed_OutPreSat;
Speed_SatError=Speed_Out-Speed_OutPreSat;
IQ_Given=Speed_Out;
Speed_run=1;
}
else
{
SpeedLoopCount++;
}
if(Speed_run==1)
{
ialfa=ia;
ibeta=_IQmpy(ia,_IQ(0.57735026918963))+_IQmpy(ib,_IQ(1.15470053837926));
//0.577=
id = _IQmpy(ialfa,Cosine) +_IQmpy(ibeta,Sine);
iq = _IQmpy(ibeta,Cosine)- _IQmpy(ialfa,Sine) ;
IQ_Error=IQ_Ref-IQ_Fdb;
IQ_Up=_IQmpy(IQ_Kp,IQ_Error);
IQ_Ui=IQ_Ui + _IQmpy(IQ_Ki,IQ_Up) + _IQmpy(IQ_Ki,IQ_SatError);
IQ_OutPreSat=IQ_Up+IQ_Ui;
if(IQ_OutPreSat>IQ_OutMax)
IQ_Out=IQ_OutMax;
else if(IQ_OutPreSat<IQ_OutMin)
IQ_Out=IQ_OutMin;
else
IQ_Out=IQ_OutPreSat;
IQ_SatError=IQ_Out-IQ_OutPreSat;
Uq=IQ_Out;
ID_Error=ID_Ref-ID_Fdb;
ID_Up=_IQmpy(ID_Kp,ID_Error);
ID_Ui=ID_Ui+_IQmpy(ID_Ki,ID_Up)+_IQmpy(ID_Ki,ID_SatError);
ID_OutPreSat=ID_Up+ID_Ui;
if(ID_OutPreSat>ID_OutMax)
ID_Out=ID_OutMax;
else if(ID_OutPreSat<ID_OutMin)
ID_Out=ID_OutMin;
else
ID_Out=ID_OutPreSat;
ID_SatError=ID_Out-ID_OutPreSat;
Ud=ID_Out;
Speed_Error = Speed_Ref - Speed_Fdb; //计算设定速度和反馈速度的差值
Speed_Up = _IQmpy(Speed_Kp, Speed_Error);//IQmpy是IQ lib里的乘法运算,这一步用来计算速度环比例增益和误差的乘积,即PID比例环节的结果
Speed_Ui = Speed_Ui + _IQmpy(Speed_Ki, Speed_Up) + _IQmpy(Speed_Ki, Speed_SatError);//这一步则计算积分环节的结果
Speed_OutPreSat = Speed_Up + Speed_Ui;//将比例环节和积分环节相加,获得PID的输出
if (Speed_OutPreSat > Speed_OutMax)//限幅判断,如果Speed_OutPreSat大于幅值,则Speed_Out=幅值,即Speed_OutMax
Speed_Out = Speed_OutMax;
else if (Speed_OutPreSat < Speed_OutMin)//限幅判断,如果Speed_OutPreSat小于幅值,则Speed_Out=幅值,即Speed_OutMin
Speed_Out = Speed_OutMin;
else//如果没有超过两侧的幅度,则Speed_Out = Speed_OutPreSat
Speed_Out = Speed_OutPreSat;
Speed_SatError = Speed_Out - Speed_OutPreSat;//更新Speed_SatError以用于下一周期的积分计算
IQ_Given = Speed_Out;//将速度环的输出值设定为电机Q轴电流目标值
Speed_run = 1;//Speed_run是一个运行标志,当其非0时进行电流环计算
}
else
{
SpeedLoopCount++;//速度环计数
}
if (Speed_run == 1)//Speed_run是一个运行标志,当其非0时进行电流环计算
{
//Clark变换,把ABC三相电流转换为 α β两相电流
ialfa = ia;
ibeta = _IQmpy(ia, _IQ(0.57735026918963)) + _IQmpy(ib, _IQ(1.15470053837926));
//Park变换,把α β两相电流变为dq轴电流
id = _IQmpy(ialfa, Cosine) + _IQmpy(ibeta, Sine);
iq = _IQmpy(ibeta, Cosine) - _IQmpy(ialfa, Sine);
IQ_Error = IQ_Ref - IQ_Fdb;//计算Q轴电流误差
IQ_Up = _IQmpy(IQ_Kp, IQ_Error);//电流环Q轴PID比例环节
IQ_Ui = IQ_Ui + _IQmpy(IQ_Ki, IQ_Up) + _IQmpy(IQ_Ki, IQ_SatError);//电流环Q轴PID积分环节
IQ_OutPreSat = IQ_Up + IQ_Ui;//电流环Q轴输出
if (IQ_OutPreSat > IQ_OutMax)//电流环Q轴限幅判断,道理同速度环限幅
IQ_Out = IQ_OutMax;
else if (IQ_OutPreSat < IQ_OutMin)
IQ_Out = IQ_OutMin;
else
IQ_Out = IQ_OutPreSat;
IQ_SatError = IQ_Out - IQ_OutPreSat;//更新IQ_SatError以用于下一轮电流环积分环节计算
Uq = IQ_Out;//更新要输出的q轴电压
ID_Error = ID_Ref - ID_Fdb;//计算d轴电流误差
ID_Up = _IQmpy(ID_Kp, ID_Error);//电流环d轴PID比例环节
ID_Ui = ID_Ui + _IQmpy(ID_Ki, ID_Up) + _IQmpy(ID_Ki, ID_SatError);//电流环d轴PID积分环节
ID_OutPreSat = ID_Up + ID_Ui;////电流环d轴PID输出
if (ID_OutPreSat > ID_OutMax)//限幅判断
ID_Out = ID_OutMax;
else if (ID_OutPreSat < ID_OutMin)
ID_Out = ID_OutMin;
else
ID_Out = ID_OutPreSat;
ID_SatError = ID_Out - ID_OutPreSat;
Ud = ID_Out;
//这段代码可能是刚启动时的控制
Speed_Error=Speed_Ref - Speed_Fdb;//计算误差
//PID计算输出占空比
Speed_Up=_IQmpy(Speed_Kp,Speed_Error);//KP计算
Speed_Ui=Speed_Ui + _IQmpy(Speed_Ki,Speed_Up) + _IQmpy(Speed_Ki,Speed_SatError);//抑制,防止超调
Speed_OutPreSat=Speed_Up+Speed_Ui;//应输出占空比
//限幅,防止超出电机占空比
if(Speed_OutPreSat>Speed_OutMax)
Speed_Out=Speed_OutMax;
else if(Speed_OutPreSat<Speed_OutMin)
Speed_Out=Speed_OutMin;
else
Speed_Out=Speed_OutPreSat;
Speed_SatError=Speed_Out-Speed_OutPreSat;//实际输出与应输出误差,用于下一次PID计算
IQ_Given=Speed_Out; //Q电机输出占空比
Speed_run=1; //标志位置1
}
else
{
SpeedLoopCount++;//速度环计数
}
if(Speed_run==1)
{
//这部分你给的代码不全,我看不出来什么作用
ialfa=ia;
ibeta=_IQmpy(ia,_IQ(0.57735026918963))+_IQmpy(ib,_IQ(1.15470053837926));
//0.577=
id = _IQmpy(ialfa,Cosine) +_IQmpy(ibeta,Sine);
iq = _IQmpy(ibeta,Cosine)- _IQmpy(ialfa,Sine) ;
//Q电机PID控制
IQ_Error=IQ_Ref-IQ_Fdb; //Q电机误差计算
//PID计算输出占空比
IQ_Up=_IQmpy(IQ_Kp,IQ_Error);
IQ_Ui=IQ_Ui + _IQmpy(IQ_Ki,IQ_Up) + _IQmpy(IQ_Ki,IQ_SatError);
IQ_OutPreSat=IQ_Up+IQ_Ui;//应输出占空比
if(IQ_OutPreSat>IQ_OutMax)
IQ_Out=IQ_OutMax;
else if(IQ_OutPreSat<IQ_OutMin)
IQ_Out=IQ_OutMin;
else
IQ_Out=IQ_OutPreSat;
IQ_SatError=IQ_Out-IQ_OutPreSat;
Uq=IQ_Out;//更新Q电机占空比
//D电机PID控制
ID_Error=ID_Ref-ID_Fdb;//D电机误差计算
//PID计算输出占空比
ID_Up=_IQmpy(ID_Kp,ID_Error);
ID_Ui=ID_Ui+_IQmpy(ID_Ki,ID_Up)+_IQmpy(ID_Ki,ID_SatError);
ID_OutPreSat=ID_Up+ID_Ui;//应输出占空比
if(ID_OutPreSat>ID_OutMax)
ID_Out=ID_OutMax;
else if(ID_OutPreSat<ID_OutMin)
ID_Out=ID_OutMin;
else
ID_Out=ID_OutPreSat;
ID_SatError=ID_Out-ID_OutPreSat;
Ud=ID_Out;//更新D电机占空比