STM32上跑SimpleFOC，电流环、速度环、位置环、棘轮软硬件全开源
引入
我之前写过不少SVPWM、FOC的介绍文章，比如：
SVPWM算法原理及详解
从电机本质到park变换再到SVPWM，SVPWM代码实现
电机FOC算法的解释
FOC和SVPWM的C语言代码实现
simple foc可以看成是他们的简化版本。本来simple foc是跑在arduino上的，为了方便网上有人把它移植到了STM32上，我这里也只是拿过来介绍并讲解下，方便大家使用。
simple foc学习可以去看灯哥的视频，一行一行代码教你写出来。
灯哥手把手教你写FOC算法 系列课程
硬件、软件例程下载地址：
链接: https://pan.baidu.com/s/1OGqoYWYTxoRtALSN4BOnpA 提取码: 24kv
包含4个例程和硬件板子：
硬件
硬件使用立创EDA：
电路很简单，主控STM32F103C8T6，电机驱动DRV8313PWPR（最大电压60V，最大电流2.5A），
电流采集芯片INA199A1，12位磁编AS5600。
把板子做好后，需要如下配件来组装：
1个3205b无刷电机 带径向磁环（最好买去除限位的，这样可以一直转，不然只能转1圈。电机要带径向磁环配合磁编，不然就需要自己贴磁钢。参考购买链接）
3根8mm长M2尼龙铜柱
3个M2尼龙螺丝
除了以上 3205B，还建议选用以下较为常见的云台电机：
电流环
框图如下，通过电流传感器读取电流后做clarke、park变换，然后做PID
框图如下，通过电流传感器读取电流后做clarke、park变换，然后做PID，最后输出给SVPWM。
//电流闭环
shaft_velocity = shaftVelocity(); //电机转速
shaft_angle = shaftAngle(); //转子电角度
electrical_angle = electricalAngle(); //转子电角度减去零位
current_ADC.a=-((float)Get_Adc_Average(ADC_Channel_3,1)*(3.3/4096)-1.65)*0.4; //电机相电流 1÷0.05÷50=0.4
current_ADC.b=-((float)Get_Adc_Average(ADC_Channel_4,1)*(3.3/4096)-1.65)*0.4;
current_cal = getFOCCurrents(_normalizeAngle(electrical_angle)); //相电流做clarke、park变换
// filter values
// current.q = LPFoperator(&LPF_current_q,current.q); //低通滤波
// current.d = LPFoperator(&LPF_current_d,current.d);
// calculate the phase voltages
voltage.d = PID_current_my_d( current_tar.d-current_cal.d); //当前电流和目前电流做PID计算
voltage.q = PID_current_my(current_tar.q- current_cal.q);
setPhaseVoltage(voltage.q, voltage.d , electrical_angle); //SVPWM
//电流闭环
程序下载后，电机就会转动。
速度环
目标速度和当前速度做PID，然后输出给SVPWM。
//速度环********************************
shaft_velocity = shaftVelocity();
// calculate the torque command
current_sp = PID_velocity(shaft_velocity_sp - shaft_velocity); // if current/foc_current torque control
// if torque controlled through voltage control
voltage.q = current_sp; // use voltage if phase-resistance not provided
voltage.d = 0;
shaft_angle = shaftAngle();// shaft angle
electrical_angle = electricalAngle();// electrical angle - need shaftAngle to be called first
setPhaseVoltage(voltage.q, voltage.d, electrical_angle);
//速度环
程序下载后，电机就会以shaft_velocity_sp 速度转动。
位置环
目标位置和当前位置做PID，然后输出给SVPWM。
//位置环********************************
//shaft_angle_sp=2;//在这里设置
shaft_angle = shaftAngle();
// calculate the torque command
current_sp = PID_angle(shaft_angle_sp - shaft_angle); // if current/foc_current torque control
// if torque controlled through voltage control
voltage.q = current_sp; // use voltage if phase-resistance not provided
voltage.d = 0;
electrical_angle = electricalAngle();// electrical angle - need shaftAngle to be called first
setPhaseVoltage(voltage.q, voltage.d, electrical_angle);
//位置环
程序下载后，电机会锁定在shaft_angle_sp 位置，shaft_angle_sp会隔1s自增。
棘轮旋钮
实现棘轮旋钮的效果
shaft_angle = shaftAngle();// shaft angle
electrical_angle = electricalAngle();// electrical angle - need shaftAngle to be called first
//棘轮旋钮：给不同位置施加不同的电流（转矩）
//棘轮模式，中点无输出0，边界给力矩1,2，超过了阈值切换中点3,4
if (shaft_angle >= ratchet_center_angle - ratchet_center_dead_angle &&
shaft_angle <= ratchet_center_angle + ratchet_center_dead_angle)
{
setPhaseVoltage(0,0, electrical_angle);
flag=0;
}
else if (shaft_angle <= ratchet_center_angle-ratchet_boundary_angle)
{
ratchet_center_angle=shaft_angle-ratchet_step_angle;
}
else if (shaft_angle >= ratchet_center_angle+ratchet_boundary_angle)
{
ratchet_center_angle=shaft_angle+ratchet_step_angle;
}
else
{
if (shaft_angle <= ratchet_center_angle)
{
force=(shaft_angle-ratchet_center_angle+ratchet_center_dead_angle)*force_MAX/ratchet_boundary_angle*4;
force=-force;
if(force>force_MAX)force=force_MAX;
setPhaseVoltage(force,0, electrical_angle);//为正的力
flag=1;
}
if (shaft_angle >= ratchet_center_angle)
{
force=(shaft_angle-ratchet_center_angle-ratchet_center_dead_angle)*force_MAX/ratchet_boundary_angle*4;
force=-force;
if(force<-force_MAX)force=-force_MAX;
setPhaseVoltage(force,0, electrical_angle);//为负的力
flag=2;
}
}
程序下载后，扭动电机会有棘轮的手感。