算法 - FOC闭环位置控制

目录

一、理论部分

二、伪代码

---

一、理论部分

二、伪代码

float Kp=0.133;//限制
#define _constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))// 归一化角度到 [0,2PI]
float _normalizeAngle(float angle){float a = fmod(angle, 2*PI);   //取余运算可以用于归一化，列出特殊值例子算便知return a >= 0 ? a : (a + 2*PI);  
}//根据输入Uq、Ud、角度，获取三相PWM；
void setPhaseVoltage(float Uq,float Ud, float angle_el) {angle_el = _normalizeAngle(angle_el);// 帕克逆变换Ualpha =  -Uq*sin(angle_el); Ubeta =   Uq*cos(angle_el); // 克拉克逆变换Ua = Ualpha + voltage_power_supply/2;Ub = (sqrt(3)*Ubeta-Ualpha)/2 + voltage_power_supply/2;Uc = (-Ualpha-sqrt(3)*Ubeta)/2 + voltage_power_supply/2;setPwm(Ua,Ub,Uc);
}//电角度计算
//zero_electric_angle为电角度偏差
//PP 为 极对数
float _electricalAngle(){return  _normalizeAngle((float)(DIR *  PP) * getAngle_Without_track()-zero_electric_angle);
}//Uq值=Kp*偏差e*DIR(正负方向)
setPhaseVoltage(_constrain(Kp*(motor_target-DIR*Sensor_Angle)*180/PI,-6,6),0,_electricalAngle());