SimpleFOC之ESP32（六）—— 双电机协同与通信实战-编程阁

1. 硬件配置与方案选择

在开始双电机控制之前，我们需要先了解ESP32的硬件特性和常见的驱动方案。ESP32作为一款功能强大的微控制器，内置双核处理器、丰富的外设接口和灵活的PWM输出，非常适合用于电机控制场景。我实际测试过多种方案，发现硬件配置的选择会直接影响后续的编程复杂度和控制效果。

目前主流的方案有两种：ESP32drive-D和SimpleFOCShield。ESP32drive-D采用了非对称电路设计，M1接口使用三个独立的使能引脚，这与PowerShield V0.2大功率驱动板的控制方式一致；而M2接口的使能引脚则是并联的，类似于SimpleFOCShield V2.0.4的设计。这种设计虽然灵活，但在调试时需要特别注意引脚配置的差异。我在第一次使用时就在这里栽过跟头，电机死活不动，后来才发现是使能引脚配置错了。

SimpleFOCShield的方案相对简单，只需要一个使能引脚（通常定义为IO32）。但要注意的是，SPI和I2C2的引脚有复用，不能同时使用。在实际连接时，两块SimpleFOCShield可以堆叠使用，具体引脚对应关系如下：

ESP32-DevKitC SimpleFOCShield 3 5 2 6 9 14 6 32 8 35 10 34 5 21 16 33 17 10 4 7 1 9 3V3 VCC GND GND

对于编码器的连接，AS5600和AS5047P的接线方式有所不同。AS5600使用I2C接口，接线相对简单；而AS5047P需要SPI接口，接线时要特别注意MOSI、MISO、SCLK和SS的对应关系。我曾经因为接反了MOSI和MISO导致编码器读数异常，调试了半天才发现问题所在。

2. 双I2C接口配置实战

双电机控制的一个关键难点在于如何正确配置和使用ESP32的双I2C接口。ESP32有两个I2C接口（Wire和Wire1），而且这两个接口的引脚可以灵活配置到任意GPIO上。这个特性非常有用，但在实际使用中也容易出现问题。

首先需要明确的是，I2C接口的配置不能使用标准的begin()函数，而应该使用setPins()和setClock()函数。这是因为在双电机控制场景下，我们需要精确控制每个I2C接口的引脚分配和时钟频率。我在项目中就遇到过因为时钟频率设置不当导致的通信不稳定问题，后来通过调整setClock()参数解决了。

具体配置步骤如下：

包含必要的头文件：

#include <Wire.h> #include <SimpleFOC.h>

初始化第一个I2C接口（Wire）：

Wire.setPins(SDA1_PIN, SCL1_PIN); Wire.setClock(400000); // 400kHz Wire.begin();

初始化第二个I2C接口（Wire1）：

Wire1.setPins(SDA2_PIN, SCL2_PIN); Wire1.setClock(400000); Wire1.begin();

在实际调试时，建议先用两个编码器测试I2C接口是否正常工作。可以通过读取编码器的角度值来验证通信是否稳定。我通常会编写一个简单的测试程序，连续读取两个编码器的值并打印到串口，观察数据是否连续、稳定。

3. 双电机驱动实现

3.1 速度模式控制

速度模式是最常用的控制方式，特别适合需要精确转速控制的场景，比如机械臂关节或移动平台的差速转向。在SimpleFOC中实现双电机速度控制需要以下几个步骤：

首先，需要为每个电机创建独立的FOC对象：

BLDCMotor motor1 = BLDCMotor(7); BLDCMotor motor2 = BLDCMotor(7);

然后分别配置两个电机的参数，包括极对数、编码器、PWM设置等。这里有个小技巧：可以先配置好一个电机，然后复制配置到第二个电机，再修改差异部分。这样可以避免重复劳动，我在实际项目中都是这么做的。

速度控制的核心是PID参数的调节。SimpleFOC提供了VelocityPID结构体来配置速度环参数：

motor1.PID_velocity.P = 0.2; motor1.PID_velocity.I = 20; motor1.PID_velocity.D = 0; motor1.LPF_velocity.Tf = 0.01; motor2.PID_velocity.P = 0.2; motor2.PID_velocity.I = 20; motor2.PID_velocity.D = 0; motor2.LPF_velocity.Tf = 0.01;

上传程序后，可以通过串口发送指令控制电机。例如：

控制A电机："A6.28"表示6.28rad/s（约1圈/秒）
控制B电机："B20"表示20rad/s

在实际调试时，建议先用较低的转速测试，观察电机运行是否平稳，再逐步提高转速。我遇到过因为PID参数不当导致的电机震动问题，通过调整I参数解决了。

3.2 其他控制模式

除了速度模式，SimpleFOC还支持力矩模式和角度模式，以及它们的混合模式。这些模式在双电机协同控制中也非常有用。

力矩模式适合需要精确控制输出力矩的场景，比如机械臂抓取物体时：

motor1.torque_controller = TorqueControlType::voltage; motor2.torque_controller = TorqueControlType::voltage;

角度模式则适合需要精确定位的应用：

motor1.controller = MotionControlType::angle; motor2.controller = MotionControlType::angle;

混合模式可以结合速度和位置控制，实现更复杂的运动轨迹。我曾经在一个项目中需要电机先快速到达某个位置，然后保持精确的速度，就是通过混合模式实现的。

切换控制模式时，记得要同时调整对应的PID参数。不同模式对PID参数的要求差异很大，需要反复调试才能达到最佳效果。我的经验是先从官方示例的参数开始，然后根据实际表现逐步调整。

4. 双电机协同算法

4.1 速度同步控制

在机器人或精密仪器中，经常需要两个电机保持同步运行。比如移动平台的差速转向，或者机械臂两个关节的协调运动。实现这种同步控制需要考虑以下几个方面：

首先，需要建立一个主从关系。通常选择一个电机作为主电机，另一个作为从电机。主电机按照设定的速度运行，从电机则跟随主电机的速度。代码实现上可以这样：

float master_speed = target_velocity; float slave_speed = master_speed * ratio; // ratio是主从速度比 motor1.move(master_speed); motor2.move(slave_speed);

其次，要考虑两个电机之间的耦合效应。在实际系统中，两个电机可能存在机械耦合，一个电机的运动会影响另一个。我曾经遇到过一个电机加速时，另一个电机速度波动的问题。解决方法是在控制算法中加入交叉补偿项。

4.2 位置跟随控制

位置跟随是另一种常见的协同模式，比如让一个电机的位置跟随另一个电机的位置变化。这在仿生机器人中特别有用。

实现位置跟随的关键是建立位置映射关系。可以通过编码器获取两个电机的位置，然后计算跟随误差：

float master_position = motor1.shaft_angle; float desired_slave_position = master_position * gear_ratio + offset; float position_error = desired_slave_position - motor2.shaft_angle; // 使用PID控制器计算修正速度 float correction_speed = position_pid(position_error); motor2.move(correction_speed);

在实际应用中，还需要考虑机械结构的限制，比如关节角度范围、最大速度等。我曾经因为没加限制，导致电机试图超出机械限位，产生了很大的冲击力。后来在代码中加入软限位保护解决了这个问题。