算法 - 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());