算法 - FOC闭环位置控制

目录

一、理论部分

二、伪代码


一、理论部分

二、伪代码

cpp 复制代码
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());
相关推荐
大数据张老师8 分钟前
数据结构——平衡二叉树
数据结构·算法·查找
py有趣11 分钟前
LeetCode算法学习之合并区间
学习·算法·leetcode
m0_7482336420 分钟前
单调栈详解【C/C++】
c语言·c++·算法·1024程序员节
郝学胜-神的一滴36 分钟前
Linux中的`fork`函数详解:深入解析
linux·服务器·开发语言·c++·算法
大数据张老师1 小时前
数据结构——BF算法
数据结构·算法·1024程序员节
让我们一起加油好吗2 小时前
【数论】欧拉定理 && 扩展欧拉定理
c++·算法·数论·1024程序员节·欧拉定理·欧拉降幂·扩展欧拉定理
一匹电信狗2 小时前
【LeetCode_876_2.02】快慢指针在链表中的简单应用
c语言·数据结构·c++·算法·leetcode·链表·stl
胖咕噜的稞达鸭2 小时前
算法入门---专题二:滑动窗口2(最大连续1的个数,无重复字符的最长子串 )
c语言·数据结构·c++·算法·推荐算法·1024程序员节
兮山与2 小时前
算法18.0
算法
码农多耕地呗2 小时前
力扣543.二叉树的直径(java)(迭代法 and 左右根后序遍历迭代法)
算法·leetcode·职场和发展