Skip to content

CDFOC 代码说明 下篇 之 app

Jump to bottom
dukelec edited this page Aug 1, 2024 · 14 revisions

代码基础框架和通讯框架和 bootloader 一样。这篇重点聚焦电机控制。

驱动芯片使用的是 drv8323rs，它通过 spi 做一些配置和状态查询，您完全可以改用 drv8323rh 或其它驱动芯片，就不需要占用一路 spi 接口。

drv8323 我们使用的是 3x pwm 模式，只需要 3 根 pwm 线，节省掉的另 3 根互补的 pwm 信号以及死区保护由 drv8323 内部自动生成。
您也可以通过 stm32cubemx 修改 mcu 配置，使其输出 6 路 pwm 线，驱动一些不支持 3x pwm 模式的驱动芯片。

硬件重点

mcu timer 只使用了 tim1 做 pwm 输出，总共配置了 5 路 pwm，中间对齐模式，其中只有低 4 路真正输出到管脚，但只有低 3 路是用于 mos 控制，另一路管脚是方便抓波形调试（也可以配置为不输出）。

电流是低端采样，在 pwm 输出低电平的中心附近采样，编码器也是在同一时间读取，具体配置和实现：

pwm ch5 和 ch4 分别控制 adc 采样触发和编码器 spi 读取的触发，都是在 pwm 波形的下降沿触发，可以通过调整这两路 pwm 的 duty 值来调整触发的时间点。

pwm ch5 下降沿的时候，会输出 OC5REF 信号，这个信号在 tim1 的设置里面连接到 TRGO2 输出。
而 adc1 和 adc2 设置为 联合采样，其中 injected 采样的触发条件设置为 Timer 1 Trigger Out event2, 也就是刚提到的 TRGO2 信号。
这样以来，每当 ch5 下降沿的时候，adc1 和 adc2 就会按照各自 injected 模式配置好的通道和参数进行采样，采集到的即为电机的两路电流信号，另一路由采集到的两路计算得来。
adc injected 采样完成会产生中断，中断服务函数是 app_motor.c 中的 HAL_ADCEx_InjectedConvCpltCallback，也是我们 foc 控制最核心的部分。

pwm ch4 我们在 app_main 初始化函数里面同时开启了 ch4 的 中断和 dma，ch4 下降沿的时候，自动通过 dma 下拉编码器 spi 的 cs 脚，让编码器的数据锁存不变化。
然后在同时到来的 ch4 溢出中断（非 dma 中断）里面使能编码器的 spi dma，此 dma 预先通过 encoder_isr_prepare 函数准备就续，dma 结束没有开中断。
（tle5012b 这样的 3 线 spi，我们主机把它当作 4 线 spi 来通讯，中途不用切换 io 方向，省事很多。）

HAL_ADCEx_InjectedConvCpltCallback 函数开头，会软件拉高 spi 的 cs 片选，然后读取 spi 读到的编码器数据，函数结尾的时候再读一次，如果两次不一致，表示首次读编码器的时候，spi dma 还没结束，数据不是最新的，需要调整 ch4 和 ch5 的 duty 设置，确保首次读到的编码器数据是最新的。此函数结尾，会调用 encoder_isr_prepare 为下一次读编码器做好准备。

如果当前为电机使能状态，HAL_ADCEx_InjectedConvCpltCallback 会通过 HAL_ADCEx_InjectedGetValue 读取采集到的 adc1 和 adc2 的数据。
然后通过当前 adc_sel 设置，分配读到的 adc 值到相应的 ia ib ic 中的两者，3 者相加为 0 可以推算剩下一路的值。

既然单次只需要采集 2 路电流，另一路电流可以算出来，为何还要搞 3 路电流采样？
因为当某一路 pwm 输出接近甚至达到 100% 的时候，采集该路电流会有很大的干扰（因为 pwm 刚切换，隔的时间太短，电流还没稳定下来，pwm 切换的噪音也会干扰到 adc 采样）。
所以我们后续会判断，当前采样的两路 pwm 中如果有一路输出太大，就会切换到 pwm 输出较小的两路来采样电流。
如果不需要 pwm 输出开到很大，譬如最大只开到 90%，那么可以改为只使用 2 路采样。

为了不干扰 Injected 采样时机，Regular 的 adc 采样也在 HAL_ADCEx_InjectedConvCpltCallback 中启动，免得 Regular 采样进行的时候被 Injected 打断（虽然硬件支持打断，但还是担心有不良影响）。

软件重点

HAL_ADCEx_InjectedConvCpltCallback 开头先读了编码器的值，然后做了一下过滤，为了防止 spi 受到干扰，读到一个错的很远的值，导致控制产生很大的抖动。
编码器的数据我们缩放到 0~0xffff 这个 16 位的范围，编码器的数据不能简单相加再除计算平均值（譬如 0x0010 和 0xfff2 这两个值的平均值 0x8001 错很远），所以我们做了一些处理。

处理的方法是，把 0~0xffff 分成 3 等份，判断历史数据是否在中间份，只要有一个数据在中间份，那新的数据一定不会超越上下边界（下界为 0，上界为 0xffff），此时简单相加再除就可以了。否则，我们会选择性给数据加上 0x10000 偏移再来计算平均值，算的结果如果超过 0xffff 再减去 0x10000 即可。（譬如 0x0010 和 0xfff2 这两个值的平均值，先把 0x0010 偏移到 0x10010，再和 0xfff2 相加后除 2 为 0x10001，最后再降低偏移得到 0x0001.）

如果检测到错的编码器数据，则通过历史数据推算新的数据。

FOC 核心算法如下，先把 ia ib 转换成 i_alpha 和 i_beta:

        float i_alpha = ia;
        float i_beta = (ia + ib * 2) / 1.732050808f; // √3

再继续转换成 i_sq 和 i_sd:

        csa.sen_i_sq = -i_alpha * sin_sen_angle_elec + i_beta * cos_sen_angle_elec;
        csa.sen_i_sd = i_alpha * cos_sen_angle_elec + i_beta * sin_sen_angle_elec;

这里的 sin_sen_angle_elec 和 cos_sen_angle_elec 是提前计算出数据，避免重复计算，减轻 cpu 计算压力。
如果需要进一步优化，此处可以换成 sin 和 cos 查表，甚至是换成不使用 float 的整数计算。
这里 angle_elec 是电角度的意思，电角度是编码器机械角度减去校准的偏移后，除以电机 poles 数得到。

电机 poles 数的判断方法：电机和驱动板断开，使用数字电源，恒流给电机线圈任意两根线通电，电流适当，用手转动电机，看电机跳几次回到原处。跳几次即为几 poles.
不断开驱动板，为驱动板设置合适的 cali_current 电流值，再设置 state 为 1，也可以进行相同的测试。

sen_i_sq sen_i_sd 前面的 sen_ 是传感器的缩写，表示数据是传感器检测到的实际值。
后续通过 2 个电流 pid 让实际值追随目标值，其中，正常运转时的 i_sd 目标值为 0，i_sq 的目标值为 csa.cal_current 设定的值。

开环拖拽模式下，通常反过来，i_sq 目标设置为 0，i_sd 目标为非 0 值。不过我们依然是让 i_sd 为 0，i_sq 为非 0.
两种做法会影响相位差，校准模式下，加减这个相位角度差就行了。

sen_i_sq sen_i_sd 通过 pid（正常运转模式）追随后，得到 cal_i_sq cal_i_sd，这里的 cal_ 是计算的意思，表示这个数据是计算得来的。

继续再转换成 i_alpha 和 i_beta，和之前的不同，这次是输出，因为是临时变量，所以就没有在前面增加 cal_ 等名称。

最后生成 pwm 输出值：

        out_pwm_u = lroundf(i_alpha);
        out_pwm_v = lroundf(-i_alpha / 2 + i_beta * 0.866025404f); // (√3÷2)
        out_pwm_w = -out_pwm_u - out_pwm_v;

在转换成 pwm 值之前，我们做了一次限幅，计算 i_alpha 和 i_beta 平方和再开根号，再等比例缩放 i_alpha 和 i_beta，避免 pwm 输出超限。
（这里算法有问题，正常超限的话，应该降低 cal_i_sq，不能降低 cal_i_sd. 有待优化。）

转换成 pwm 值后，我们计算了一下 out_mid，并让 pwm 值减去这个偏移。目的是让输出的三相电压幅值最大化。生成 svpwm 波形。

加减速代码说明参见：https://github.com/dukelec/cdstep/wiki/CDSTEP-代码说明

svpwm 说明参见：https://blog.d-l.io/svpwm-cn

FOC 感悟

有刷电机、无刷六步换相、无刷 FOC 控制的区别：

  • 有刷和无刷六步换相：这两者区别不大，电机每圈的换相次数是固定死的，一个是机械换相一个是 mos 换相。
  • 无刷 FOC 控制：和前两者区别也不大，只是每圈的换相次数增加了很多。

以上都属于永磁同步电机，这种电机的特点是，永磁和电磁铁的磁性相位相差 正负 90 度的时候，转动力最大。 （0 度和 180 度的话要不是相互吸引，要不是相互排斥，没有转动方向的力。）
有刷和无刷六步换相由于每圈换相次数有限，只能尽量接近 正负 90 度，角度偏差大了就开始换相了，而 无刷 FOC 控制可以始终保持在 正负 90 度。

FOC 控制就是要合成一个电磁铁，这个电磁铁始终在永磁铁的正负 90 度磁性的位置，通过电流大小控制电磁铁的磁性强度，从而控制力矩，进而控制转速、位置等。

电机 kv 值：
电机转动会产生反向电动势，转速越快其值越高。实际供给电机的有效电压是 输入电压 - 反电动势 后剩下的电压，随着转速增加，剩下的有效电压降低到一定程度，电机的转速就无法继续提升，从而达到一个平衡。此时，输入电压和转速可以得到电机 kv 值。

id iq:
因为电机转动产生了反向电动势，我们如果想要让电磁铁和永磁铁保持正负 90 度，直接按照当前编码器位置计算输出的三相电压的正弦波相位是不行的，因为真正有效的电压是减去反向电动势之后的有效电压，反向电动势的相位和我们输出的相位不同，减去后得到的有效电压的相位会有偏移。更准确来说，电机控制最终看的是线圈电流，而不是电压。我们用 pid 使检测到的 id 保持为 0，本质上是补偿反电动势和线圈电感导致的相位偏移，目的是使电磁铁和永磁铁更准的保持在正负 90 度。

反向发电：
当电机减速的时候，可能会抬升母线电压，细节分析见：https://blog.d-l.io/motor-regeneration-detail-cn

步进电机：
步进电机和上面提到的 3 种电机区别很大，它是类似让永磁和电磁铁的磁性相位相差 180 度（N 和 S 对齐），N 和 S 对齐的时候，转动方向的力很小，很软，当负载让转子转动一点点角度后，力才变大。这个一点点角度会导致误差，为了降低这个误差，步进电机每圈的 poles 数通常很大，这样相同电角度对应的机械角度自然就小了。
FOC 驱动器的 state 为 1 的校准模式，和步进电机是类似的，所以校准的时候，不要连接负载，电机本身也建议水平摆放，降低转子重心不平衡等效出来的负载。

干扰处理：
降低 mos gate 脚电流，可降低输出电压变化陡率（越陡高频成份越多），可以从源头上大大降低干扰，比各种手段去治理已经产生的大干扰要高效很多。
mos 输出接电机，首先有导线（有天线效应），然后电机线圈关电的瞬间，mos 死区的时间，线圈电压会突变很大（mos 等效开关瞬间断闸，要是没有 mos 寄生二极管钳位，理论上都能打火花的那种）。
而降低 mos gate 电流，mos 开关速度会变慢（mos 等效电阻缓慢增大到无穷，平稳过度），那么线圈电压突变就会小很多，产生的干扰就会小很多，代价是 mos 效率低一些、死区时间也要加长一些。
有打火花（或者有倾向）的带线圈的电路，才是最猛的干扰源，譬如有刷电机也算一个，碳刷容易产生火花。又譬如拿线圈在电池上摩擦会产生火花。

通讯技巧

总线多轴控制有两种方式：

  1. 伺服电机不开加减速，接收到的位置直接喂给位置环 PID，用 PID 让当前位置去追目标位置，目标位置周期性由主机下发更新，更新频率要比较高才好

  2. 伺服电机启用位置加减速规划，好处是主机同步周期可以慢一些，对主机实时性要求也没那么高。核心的一个技巧是：譬如理论上 99 ms 后开始减速，那么故意过 100 ms 再同步下一个目标位置。这样可以预防累积误差，这 1ms 用来消除各节点的不同步：时钟快的电机提前一点减速，时钟慢的电机晚一点开始减速。刚减速一点点，由于收到下一个目标，又重新把速度升回去。

Clone this wiki locally