一、原理说明
! |  l7 z$ _4 Y% W1.1、电流采样
1 V8 d, k8 N- y: ~! A1.1.1、为什么要采样电流
  FOC（Field-Oriented Control），即磁场定向控制，磁场大小与绕组中的电流成正比，所以对磁场的控制在程序上就是对电流的控制。前几节的程序并没有使用电流环，而是直接使用电压值，确实可以达到还行的效果。但是这样做有两个问题：

& C' v/ m6 U/ i& Z* _: Z/ p" h) Z1、不能保证i_d为零，i_q等于目标值，因此电机并不能工作在效率最高的状态，力矩控制也是不准确的。
0 {7 o, b$ _) S8 R3 R$ v7 R
2、相电压施加在电感上产生相电流，电压和电流有相位差且并不恒定，低速运行时相位差对电机的影响不明显，但是当速度快了或者速度变化率高了以后，相位差的影响就会非常明显。
9 Y2 |" D2 X  j( A: F6 a
6 c6 w% J" K. R: {1.1.2、电流采样方式
电流采样主要有三种方式：
低侧电流采样
高侧电流采样
内置电流采样
8 W8 d* D- U. L# d& o
; h' F5 u  P- Y% A" t, \* Q1.1.2.1、低侧电流采样
, [  o! i: ~' L4 n: w) W4 m& Y
% {# I5 A* k' B5 o& h) {
低侧电流检测可能是最常见的电流检测技术，主要是因为它既不需要高性能的PWM抑制运放（如内置），也不需要支持高压的运放（如高侧），采样电阻在低侧MOS和GND之间，确保了运放输入端的电压非常低。这种方法的缺点是，必须在下桥臂MOS打开时检测电流，PWM频率通常为20k～50khz，这意味着低侧MOS的开关频率为每秒20k～50k次，因此PWM设置与ADC采集之间的同步非常重要。

1.1.2.2、高侧电流采样

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

3 Y) Y6 _. H$ b6 o8 u- T
4 n+ {% l. t6 n+ r/ V' o
9 E: P- ~$ w" T  Y7 x# f1.1.2.3、内置电流采样
( H5 J: o& _6 c$ o


  内置电流检测（InlineCurrentSense）是使用起来最简单但是最精准的技术。 采样电阻串联在电机相线上，检测的电流始终都是电机相电流，因为电感中的电流不会突变，所以无论PWM占空比的状态如何，采样到的电流都是连续稳定的。
9 j/ ?! j' }9 w) U
  这种方法非常适合Arduino，采样程序变得简单了，这应该是考虑到了MEGA328P微弱的性能以及跨平台时程序的适配。内置电流检测的缺点主要在于芯片，需要比常规放大器更好的PWM抑制功能的高精度双向运放，简单的说就是硬件成本高。
& |0 N) d& n* O
1.2、电流变换
" J, C& P: {1 Z, D本节增加电流环，主要增加了以下功能，
1、AD转换获取电流值Ia和Ib，
2、通过Clark变换得到Iα和Iβ ，
9 x/ z! |' e3 A" |. u3、获取电机角度，通过Park变换得到Id和Iq，
' l+ B  N; i: W, `7 }# T" n: [4、Id、Iq不能突变，同时为减少干扰，做平滑滤波，
/ m9 R5 l0 E+ L/ d) h1 J9 |' e/ _5、Id、Iq与设定值比较，通过PID运算得到Vd 和Vq,
, W8 _( v( G. D$ V1 l$ J$ C1 M* l
5 d6 p- C! `# u! S# E" M0 C1.2.1、AD转换
  为了与官方代码保持一致，AD转换采用简单的单通道转换模式，主程序循环一次获取一次A/B相的电流。

! b+ f; [: U5 V3 y' g% |

$ F$ w2 M( \& M4 Z* y1.2.2、clark变换
! b+ e* F5 F7 X
+ s/ r7 \  ^8 Y% l6 q+ J/ l' H
& j( @" X! ^" `/ @- s" i6 B
# c/ r% i" g3 d9 i6 Z' T% h& e5 ^. `1.2.3、Park变换
; o0 G4 M& Q! h3 y/ E
; O  R& r& l" ]8 D3 g6 Q6 R: o8 k
3 Z, T! p  |8 r/ L6 p& N: L
+ H3 u: R. ^5 i' S$ T1 R( I  ~  Park变换中的“θ”是电角度，由读出的编码器角度转变而来
1 ~' M% `" ~+ l" l/ C- p


1.2.4、LPF运算
dc_current模式只有 Iq 的LPF运算
. L; Q. P' Z( S4 zfoc_current模式有 Iq 和 Id 的LPF运算
2 i+ v4 e$ b' ~


1.2.5、PID运算
; U2 }0 y% i7 ~dc_current模式只有q轴电流的PID运算
$ U( N* H! b$ f. Bfoc_current模式有q轴电流和d轴电流两个变量的PID运算
4 Z1 U. e8 |+ ]5 D电流环使用 P、I 两个参数。
8 M/ W+ T! j& r/ N- D5 S
$ L6 F4 _6 ?5 Y  O
6 ]5 Z/ @4 B7 Y5 Z
6 h1 L; r4 Z6 d二、电流采样参数设置
2.1、INA240介绍
4 A8 a2 O& _1 lINA240有四个型号，INA240A1是20倍放大倍数，INA240A2是50倍放大倍数。



电机的电流是正弦波，对于采样的差分信号来说，负半周期为负值，为方便处理，需要叠加上偏置电压以保证全周期的输出为正值。
4 x' A! c3 q* c

1 y  E1 B* G' R" {  A
4 u0 M* k, ]2 v2.2、电流实际波形
下图为电机静止时A/B相运放输出端波形，VCC=3.3V，所以此时电压=1.65V，
+ Q1 H/ }) H" j+ {
- Q8 h, Z4 |, g; f
  E/ Y; U+ o5 A5 q
下图为电机转动时运放输出端波形

- h4 w+ d% k  I8 y. m! d
/ h% w! l% s& B
2.3、SimpleFOC V2.0.3 的参数

( c8 H3 w: Z! {0 ]! B) d

( G8 ~% x# @( i' P: e采样电阻0.01 Ω
运放为INA240A2，放大倍数50
背面需分别短接A0/A2至输出
C1_OUT=INA_VCC/2 + 0.01 * I * 50
* o1 O3 d7 W0 \' J如果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）
/ b, y! H! o6 A2 e  _B相采样输入在PCB上为走线方便做了反向输入，所以放大倍数需乘以 -1（移植后的代码已经做了处理）。

0 v5 r3 N2 H3 g1 ]7 X! [2.4、PowerShield V0.2 的参数



采样电阻0.001 Ω
- V' G0 {1 Q9 r- B4 T* c9 x运放为INA240A2，放大倍数50
6 L( m  y( n  i9 H1 q5 r2 @; DC1_OUT=1.65 + 0.001 * I * 50，电流范围（-33A,33A）
$ \4 t: E4 ^# x' e8 E$ s* V
2.5、SimpleMotor 的参数


7 Z- B& {+ P" ^+ e. i# n
8 k- H" r. J3 O+ D5 s8 m" z采样电阻0.001 Ω
运放为INA240A1，放大倍数20
2 d" W5 V% r: [& l; nC1_OUT=1.65 + 0.001 * I * 20，电流范围（-82.5A,82.5A）

" C+ m9 f8 M' M/ H2 m三、扭矩模式
/ r$ E3 |; b; ^/ N3 K) }8 c  SimpleFOC中有三种扭矩模式：
. k4 }3 N, M) _( m. K5 {Voltage mode - the simplest one
4 w: |' l# J7 B' F4 y7 uDC current mode - 1xPID controller + 1xLPF
  O8 z2 m1 N5 I1 I' uFOC current mode - 2xPID controller + 2xLPF filters
& I5 S7 b! ?7 J& f# }
3.2.1、电压模式(Voltage mode)
电压模式是之前示例中一直使用的模式，
因为不用采样电流，所以硬件简洁，程序简单，执行速度快，
# i' Q4 `1 C7 Z适合低速电机控制
, K! r( c" O* ~) B- t% u4 @6 \$ B, M
" K5 u& b" q" d$ b

8 T2 i' _4 ?# B* B; {' u3.2.2、直流模式(DC current mode)
. a  V  l) y# h# b) R; m检测电流大小，实现电流环控制
# s8 W* J( O0 ]% T, ~只控制Iq，设置Vd=0，
像控制直流电机一样控制无刷直流电机的电流


+ l3 X$ j) n, s9 v( L& u
3.2.3、FOC模式(FOC current mode)
4 y6 h; h) S4 ]5 z6 b控制Iq和Id，使转子中的磁力与永磁场精确偏移90度，从而确保最大转矩，
唯一真正的转矩控制方法，
保证电机始终工作在高效状态
& U/ E, O( Y# _& V: i' ^6 A) b

; c4 n$ ~! K4 Z1 B$ Q" A
9 s0 F0 i/ `- v. p/ I8 K3.2.4、三种模式对比
8 m9 h; u  j5 X3 h  x5 |! T3 u

( ~$ o, S7 i  y
四、硬件介绍
# G- z) @9 ?2 Y4 {4.1、原理图


9 k2 s# @0 m8 i+ |; ~1 o2 c7 i
7 Y5 h0 n+ Q1 j) k4.2、SimpleMotor方案
0 Y0 }* w- o3 h, [. _2 ~# ~4.2.1、准备清单

: e. \' e+ m9 I- V: T. F1 C4 c; K( X



带编码器云台电机可以是AS5600，也可以是TLE5012B。

8 |2 h7 E0 ^2 X7 M: a3 R4.2.2、接线



只展示M1的接线，M2的接线根据原理图自行连接。

4.3、STM32方案
4 y; t6 A# _; {8 K( X) j( ~4.3.1、准备清单

) @! G& D5 C: I8 q' G) h
2 t5 Z, y  Y/ {- z# M  m


6 B  f4 A9 c& Z! v
8 u0 |, m, F( O1 T0 ~: P. b7 _带编码器云台电机可以是AS5600，也可以是TLE5012B。

4.3.2、接线

9 S2 r5 ]4 T! ~( d5 f" F" g
- K2 L7 w+ |5 H4 Z2 O+ F; u
Shield V2.0.3的背面按照上图短接
+ n* ?2 o& M" F' Q# t/ ^3 x; M
. R2 e& c% h; q, _9 r
& v) d2 {. F7 j9 G; P
对照 Shield V2.0.3 的原理图：

1 w8 _$ s( X2 u0 _+ e% S8 {) w# V

4 Q0 n) W% C& }9 z; x/ E7 }' i. I
: h8 q0 {. A* v6 A4 F. d; {如果是AS5600编码器，如下


& V  y3 m4 b6 \7 L
如果是TLE5012B编码器，如下
3 D6 {9 F. k/ C1 ^; P& d
1 [$ x! j1 ?8 ~4 @$ }
* s& G3 A* a% y, R' I" E  J# q8 r  k5 C
+ R6 M6 E! U2 T  i& d
/ R; f* t6 y. P. M4 o3 a9 N' j: e
1 h1 p0 L7 j! F只展示M1的接线，M2的接线根据原理图自行连接。

* m6 E$ }0 k( D' _+ Q% r* ~* w- I( n
五、程序演示





  T/ x: x% A% K/ x; l注意：
1、voltage_limit最大只能设置为供电电压的 1/√3（12V限制为6.9，24V限制为13.8），
) }' X8 v" J2 W2、如果检测到的极对数与实际不符，停止继续操作，否则电机会堵转。


六、代码升级说明
. l+ [4 }" w5 v: x* O; D
$ @3 d9 U! V) I' T' t5 s. O1、优化PID子程序。之前代码把位置PID、速度PID和电流PID_q_d 各自做为独立的函数调用，本次升级只写一个函数，把PID相关参数定义为结构体，与官方代码保持一致。
# I" |6 g# o, A2 {
2、优化LPF子程序。与PID的做法相同，只写一个函数，把相关参数定义为结构体，与官方代码保持一致。

% X9 n: k6 K3 j' ?; G" ]3、增加了对AS5047P、MA730编码器的支持（目前共支持AS5600、TLE5012B、AS5047P、MA730四种磁编），使用代码时只需在 MyProject.h 中把使用的型号设置为1即可。

- m$ c. _# z! L3 K! j/ C4、官方最新的SimpleFOC库V2.2.0（2021年12月14日）版本新增加了sensor->update()函数，并因此对角度处理做了大改。
之前调试代码的时候，就发现官方代码中角度读取处理的不太好，特别是速度模式上电的时候电机会突然转一下然后停止，当时按照我的理解改进了这部分代码，最新版本中官方修补了这个问题，我本来是打算把这个部分升级到官方一致，但是修改了一天后发现官方的代码仍然很混乱（相信这部分代码官方以后还会再升级），所以此次放弃了这部分的升级，仍保持之前的设计思路，只做简单优化。
. E( X. {( ]5 w0 E
5、升级零点检测，在已知电机参数的情况下，通过设置 零点偏移角度和方向，可以跳过检测。与官方代码保持一致。

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

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

$ D0 _0 W5 u; D0 M2 Y1 f9 n) L8、升级后的代码可以很好的实现电流闭环，解决了之前发现的所有问题，所有功能都经过了本人的实际测试，当前代码在本人看来已无懈可击，虽为开源但也要认真负责。

! I: n) k' M% j* S————————————————
; }) Q; r) h. B) {, i% T版权声明：loop222