一、原理说明
# G- G. ?; M* _7 b% M4 e1.1、电流采样
1.1.1、为什么要采样电流
  FOC（Field-Oriented Control），即磁场定向控制，磁场大小与绕组中的电流成正比，所以对磁场的控制在程序上就是对电流的控制。前几节的程序并没有使用电流环，而是直接使用电压值，确实可以达到还行的效果。但是这样做有两个问题：

1、不能保证i_d为零，i_q等于目标值，因此电机并不能工作在效率最高的状态，力矩控制也是不准确的。
2 {5 z9 X9 z5 z. Q. E4 v+ u
2、相电压施加在电感上产生相电流，电压和电流有相位差且并不恒定，低速运行时相位差对电机的影响不明显，但是当速度快了或者速度变化率高了以后，相位差的影响就会非常明显。

* @* [) M6 k# W* \2 ]1.1.2、电流采样方式
电流采样主要有三种方式：
' S* O& ?( a  x/ k  B+ s低侧电流采样
高侧电流采样
7 u! l6 T1 ?- R内置电流采样

1.1.2.1、低侧电流采样
6 i3 y9 \- n* I1 x% G4 u( x

低侧电流检测可能是最常见的电流检测技术，主要是因为它既不需要高性能的PWM抑制运放（如内置），也不需要支持高压的运放（如高侧），采样电阻在低侧MOS和GND之间，确保了运放输入端的电压非常低。这种方法的缺点是，必须在下桥臂MOS打开时检测电流，PWM频率通常为20k～50khz，这意味着低侧MOS的开关频率为每秒20k～50k次，因此PWM设置与ADC采集之间的同步非常重要。
# Y; S5 k7 u" n$ x
9 e8 q- a4 ~$ @/ N1.1.2.2、高侧电流采样

1 S( }: z1 r  u( ?, v/ m  高侧电流检测可能是最不常见的电流检测技术，因为它需要支持高压的运放，采样电阻在高侧MOS和直流电源电压之间，使放大器的输入端始终有高电压。 这种方法的另一个缺点和低侧电流采样一样，需要同步PWM和ADC。

) Y: H3 p& c  b- }% ^6 U! R

  i+ [1 w. g2 W1.1.2.3、内置电流采样

. p7 w- b3 N; m4 b9 \

! G) N' x+ ^2 M. J/ A0 W  内置电流检测（InlineCurrentSense）是使用起来最简单但是最精准的技术。 采样电阻串联在电机相线上，检测的电流始终都是电机相电流，因为电感中的电流不会突变，所以无论PWM占空比的状态如何，采样到的电流都是连续稳定的。
: Z$ Z( n( o( a3 O: Y0 b% S
  这种方法非常适合Arduino，采样程序变得简单了，这应该是考虑到了MEGA328P微弱的性能以及跨平台时程序的适配。内置电流检测的缺点主要在于芯片，需要比常规放大器更好的PWM抑制功能的高精度双向运放，简单的说就是硬件成本高。
5 T4 U; t1 j+ t) u9 D9 [( t
/ S& H8 z9 @0 g1.2、电流变换
& q$ G  m$ A& Y- y本节增加电流环，主要增加了以下功能，
: M* @, R' q4 H! X1、AD转换获取电流值Ia和Ib，
2、通过Clark变换得到Iα和Iβ ，
1 j8 D4 t. u: j: ~2 i  j3、获取电机角度，通过Park变换得到Id和Iq，
4、Id、Iq不能突变，同时为减少干扰，做平滑滤波，
* D" u+ V7 O: O2 J* R$ W  m5、Id、Iq与设定值比较，通过PID运算得到Vd 和Vq,

1.2.1、AD转换
( z# _  f+ V7 S8 G0 Q  为了与官方代码保持一致，AD转换采用简单的单通道转换模式，主程序循环一次获取一次A/B相的电流。
2 y' s/ D4 `8 K2 H6 }6 ^' C
3 T8 ]( M& O, U* N0 J- O* D& V# g

1.2.2、clark变换



; t) h- k7 J8 N1.2.3、Park变换

& }; T2 V  b+ R
. t9 c6 ^+ B, M3 a1 A" j
# o, Q# e- u! ]: I  Park变换中的“θ”是电角度，由读出的编码器角度转变而来
3 T5 L4 s& B- e) c  s
3 R4 V& h; M: Z8 ?, @" C( |

1.2.4、LPF运算
dc_current模式只有 Iq 的LPF运算
# R" {- @8 e4 Kfoc_current模式有 Iq 和 Id 的LPF运算


; N' X9 u; t+ X; ~5 R- u) V
1.2.5、PID运算
dc_current模式只有q轴电流的PID运算
foc_current模式有q轴电流和d轴电流两个变量的PID运算
) `7 }, F$ K1 a电流环使用 P、I 两个参数。
0 E! `( E. M6 u& ]% P# r
. E' D( ]# T3 i$ z7 B7 ^
9 |" U6 w1 E& j# ?6 s
二、电流采样参数设置
2.1、INA240介绍
INA240有四个型号，INA240A1是20倍放大倍数，INA240A2是50倍放大倍数。
$ b; P6 b( j3 {5 x6 N


: P9 X) |4 c4 b, V$ h电机的电流是正弦波，对于采样的差分信号来说，负半周期为负值，为方便处理，需要叠加上偏置电压以保证全周期的输出为正值。
3 e9 h) u. d2 D' v


2.2、电流实际波形
下图为电机静止时A/B相运放输出端波形，VCC=3.3V，所以此时电压=1.65V，

* x) z5 h7 l. u0 J

1 r7 ^% C9 U$ _: P( B# P下图为电机转动时运放输出端波形
9 S8 r9 _& z; {


2.3、SimpleFOC V2.0.3 的参数



采样电阻0.01 Ω
; s+ ^- C7 [" w9 y  {# D$ j运放为INA240A2，放大倍数50
背面需分别短接A0/A2至输出
C1_OUT=INA_VCC/2 + 0.01 * I * 50
如果INA_VCC=3.3V，C1_OUT=1.65 + 0.01 * I * 50，电流范围（-3.3A,3.3A）
如果INA_VCC=5.0V，C1_OUT=2.50 + 0.01 * I * 50，电流范围（-5A,5A）
# x' L- o5 i4 q9 `6 L% C; }4 [: sB相采样输入在PCB上为走线方便做了反向输入，所以放大倍数需乘以 -1（移植后的代码已经做了处理）。

2.4、PowerShield V0.2 的参数

+ O( o) o( o- |/ T* y& D
' u( b7 {# e$ ]" R2 R4 P) w; e
采样电阻0.001 Ω
运放为INA240A2，放大倍数50
+ a- D5 S* `& w+ ]C1_OUT=1.65 + 0.001 * I * 50，电流范围（-33A,33A）

2.5、SimpleMotor 的参数



3 O) ]7 u( o% W4 l8 z采样电阻0.001 Ω
运放为INA240A1，放大倍数20
C1_OUT=1.65 + 0.001 * I * 20，电流范围（-82.5A,82.5A）

$ x3 p5 q; P: L$ ]三、扭矩模式
  SimpleFOC中有三种扭矩模式：
* c2 G3 O$ e$ m# G7 Z, l6 sVoltage mode - the simplest one
; m( k4 c) a6 H: I9 EDC current mode - 1xPID controller + 1xLPF
FOC current mode - 2xPID controller + 2xLPF filters
' N4 z% @" p5 q, [6 u2 b
3.2.1、电压模式(Voltage mode)
电压模式是之前示例中一直使用的模式，
0 c4 l/ I, L( B* N  X0 Y8 r2 Y因为不用采样电流，所以硬件简洁，程序简单，执行速度快，
适合低速电机控制



% y8 m) U) i+ u$ T( S  g  d3 Y1 M3.2.2、直流模式(DC current mode)
检测电流大小，实现电流环控制
只控制Iq，设置Vd=0，
像控制直流电机一样控制无刷直流电机的电流



3.2.3、FOC模式(FOC current mode)
控制Iq和Id，使转子中的磁力与永磁场精确偏移90度，从而确保最大转矩，
唯一真正的转矩控制方法，
保证电机始终工作在高效状态

. I* K  o8 F4 P# s! ]( Y
$ ^8 W* [* i, k7 M- A4 {
3.2.4、三种模式对比

% X5 l) l! A2 i! y) Q

: K' }' b6 W; l- l4 a/ I四、硬件介绍
4 }8 z: j9 H& ~4.1、原理图
4 s; J' r7 b! [  q6 m% h; Q: K* m7 n
, l( N+ @9 W) I. L4 W) a6 o9 a

6 ]0 T4 M3 ?7 K! u4.2、SimpleMotor方案
4.2.1、准备清单



9 M5 W3 Z: Z+ Y& `2 b9 n
4 C+ F  Y& P5 `
2 T  G6 ?5 ?! {4 D# {" h0 \带编码器云台电机可以是AS5600，也可以是TLE5012B。

4.2.2、接线
# v4 F3 I% z6 h8 o
: E" t# E+ y& a; r. ^

& S1 C+ Q8 k/ d4 B, b3 w6 d只展示M1的接线，M2的接线根据原理图自行连接。
- l) I& f* Q7 f$ `. o1 b
4.3、STM32方案
4.3.1、准备清单
3 c- R6 S! S& R3 F
) L. m6 C5 v0 q% s- h/ W( i
  n& v9 w0 D+ ~% a  W1 U" ~7 x
9 @- o; l) a7 Z, s% p3 w

" Z* s  V) b* c: e- Q! L7 @1 i
3 h' ^$ c; x4 p( M0 y" P+ |带编码器云台电机可以是AS5600，也可以是TLE5012B。
& D& P- t0 Y  M2 y
4.3.2、接线
/ F7 t% {3 \" S# g/ _4 N# C


Shield V2.0.3的背面按照上图短接
8 F3 x( f# _& u6 i


, l- z, `% D7 Y1 ]对照 Shield V2.0.3 的原理图：



2 q; p/ i: ]5 O; u& d8 z
, A5 g5 d8 W- T/ |& b如果是AS5600编码器，如下

0 l/ g1 E" _: U1 S0 ]3 n
1 Z7 @0 ]0 ^* ~: h, }' ~7 ~9 p
" C3 u( ]/ h) V# R; U. V如果是TLE5012B编码器，如下
, ^) E% i, X5 a




! ~' O5 {5 \0 B; q只展示M1的接线，M2的接线根据原理图自行连接。

  J% @) l, a" v7 ?9 K, a5 o
五、程序演示




3 T0 v- i: w% _" D' w+ w- H
5 j( h# X: m+ `* |; u. `- G" ~注意：
- x& F/ p% U! H* Q1、voltage_limit最大只能设置为供电电压的 1/√3（12V限制为6.9，24V限制为13.8），
2、如果检测到的极对数与实际不符，停止继续操作，否则电机会堵转。
# A% c8 O* w  E1 s" h$ E/ I; }
& r2 H- D) N$ G* L- p3 o% G& g
六、代码升级说明
8 e9 B3 \; K) P& L+ R8 n. _. c
1、优化PID子程序。之前代码把位置PID、速度PID和电流PID_q_d 各自做为独立的函数调用，本次升级只写一个函数，把PID相关参数定义为结构体，与官方代码保持一致。
' v4 N- E2 ~. B- \# _+ O9 k& E& y
2、优化LPF子程序。与PID的做法相同，只写一个函数，把相关参数定义为结构体，与官方代码保持一致。
) |* {2 k- i" Y* Q1 I0 O$ r$ c/ }* r
0 O$ E; r3 z  Z0 `/ Y8 q" L" S6 ~; S3、增加了对AS5047P、MA730编码器的支持（目前共支持AS5600、TLE5012B、AS5047P、MA730四种磁编），使用代码时只需在 MyProject.h 中把使用的型号设置为1即可。
; {( D. C( ?% B! U
4、官方最新的SimpleFOC库V2.2.0（2021年12月14日）版本新增加了sensor->update()函数，并因此对角度处理做了大改。
之前调试代码的时候，就发现官方代码中角度读取处理的不太好，特别是速度模式上电的时候电机会突然转一下然后停止，当时按照我的理解改进了这部分代码，最新版本中官方修补了这个问题，我本来是打算把这个部分升级到官方一致，但是修改了一天后发现官方的代码仍然很混乱（相信这部分代码官方以后还会再升级），所以此次放弃了这部分的升级，仍保持之前的设计思路，只做简单优化。

# |. B& ~# I# U8 ^5、升级零点检测，在已知电机参数的情况下，通过设置 零点偏移角度和方向，可以跳过检测。与官方代码保持一致。

6、优化了while主循环中的计时方式，由原来的timer4中断累加修改为systick查询。当前代码中只有一个串口接收中断。与官方代码保持一致。

7、电流采样修改为三相采样。之前只写了两相采样，觉得两相就可以三相似乎浪费，调试ESP32drive的时候仔细看了下代码，发现三相多了一个平均处理，应该会比两相更可靠一点点，所以这次升级为第三相可选择，与官方代码保持一致。

8、升级后的代码可以很好的实现电流闭环，解决了之前发现的所有问题，所有功能都经过了本人的实际测试，当前代码在本人看来已无懈可击，虽为开源但也要认真负责。
1 c1 k3 [7 m" K7 |3 ^4 B1 H: _
————————————————
版权声明：loop222
4 H; p7 r; w+ G; m
, J9 J# a" Y0 B8 v) z8 A2 p