BNO055通过rosserial经A4988驱动Nema17步进电机抖动问题求助

• 电机控制采用阻塞式脉冲输出：现有代码每次输出5个步进脉冲时，会执行累计7.2ms的delayMicroseconds阻塞等待，这段时间无法响应其他逻辑，一旦和IMU读取任务重叠就会打断脉冲时序，导致丢步、抖动
• 单线程任务抢占：IMU的I2C读取、ROS网络发布都是毫秒级阻塞操作，和电机控制在同一线程串行执行，会抢占电机脉冲输出的时间片，破坏脉冲的均匀性
• 精度不匹配：电机步进脉冲需要微秒级的时间精度，现有代码用毫秒级的millis()做电机控制的时间基准，误差过大导致转速波动
• 变量命名冲突：get_imu_data函数内定义的局部变量ey_s和全局变量重名，会导致传感器读数传递异常

利用ESP32的双核硬件特性，通过FreeRTOS任务绑定核心实现两个任务完全隔离：

• 高优先级电机控制任务绑定到核心1（APP_CPU），核心1默认不运行WIFI、蓝牙等系统后台任务，实时性有保障，电机控制逻辑完全采用非阻塞的微秒级计时，没有任何阻塞延时
• IMU读取、ROS通信任务绑定到核心0（PRO_CPU），所有网络、I2C开销都在核心0运行，不会抢占电机控制的时间片
• 传感器读数通过加临界区保护的全局变量传递，避免多核读写冲突
• 去掉电机控制逻辑内的所有串口打印等高开销操作，保证脉冲输出的稳定性

#include <WiFi.h>
#include <ros.h>
#include <Wire.h>
#include <std_msgs/Header.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Quaternion.h>
#include <HardwareSerial.h>
#include <analogWrite.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <math.h>

/************************** 全局配置 **************************/
// BNO055配置
#define I2C_SDA 21
#define I2C_SCL 22
TwoWire I2Cbno = TwoWire(0);
Adafruit_BNO055 bno_master = Adafruit_BNO055(55, 0x29);
Adafruit_BNO055 bno_slave = Adafruit_BNO055(55, 0x28); 
geometry_msgs::Quaternion Quaternion;
std_msgs::String imu_msg; 

// WIFI配置
const char* ssid = "FRITZ!Box 7430 PN";
const char* password = "37851923282869978396";
IPAddress server(192,168,178,112);
IPAddress ip_address;
WiFiClient client;

// 步进电机配置
#define STEP_PIN 4
#define DIR_PIN 2
const long MAX_POSITION = 4100;
const uint16_t STEP_PULSE_WIDTH = 5; // 步进脉冲高电平宽度(us)
const uint32_t STEP_INTERVAL = 2400; // 两个步进脉冲间隔(us)，对应原来1200us高低电平的速度

// 线程安全变量
portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED;
volatile float shared_ey_s = 0; // 核心0写入、核心1读取的IMU Y轴数据

/************************** ROS WIFI适配 **************************/
class WiFiHardware {
public:
  WiFiHardware() {};
  void init() {
    client.connect(server, 11411);
  }
  int read() {
    return client.read();
  }
  void write(uint8_t* data, int length) {
    for(int i=0; i<length; i++)
      client.write(data[i]);
  }
  unsigned long time() {
    return millis();
  }
};

ros::Subscriber<std_msgs::String> sub("message", &chatterCallback);
ros::Publisher pub("imu_data/", &imu_msg);
ros::NodeHandle_<WiFiHardware> nh;

void chatterCallback(const std_msgs::String& msg) {
  // 预留回调逻辑
}

void setupWiFi()
{
  WiFi.begin(ssid, password);
  Serial.print("\nConnecting to "); Serial.println(ssid);
  uint8_t i = 0;
  while (WiFi.status() != WL_CONNECTED && i++ < 20) delay(500);
  if(i == 21){
    Serial.print("Could not connect to"); Serial.println(ssid);
    while(1) delay(500);
  }
  Serial.print("Ready! IP: ");
  Serial.println(WiFi.localIP());
}

/************************** 核心0任务：IMU读取+ROS通信 **************************/
void imu_ros_task(void *pvParameters) {
  while(1) {
    // 读取IMU数据
    imu::Vector<3> Euler_s = bno_slave.getVector(Adafruit_BNO055::VECTOR_EULER);
    float ex_s = Euler_s.x();
    float ey_s = Euler_s.y();
    float ez_s = Euler_s.z();

    // 写入共享变量，加临界区保护
    portENTER_CRITICAL(&mux);
    shared_ey_s = ey_s;
    portEXIT_CRITICAL(&mux);

    // ROS发布
    String data = String(ex_s) + "," + String(ey_s) + "," + String(ez_s) + "!";
    int length_data = data.indexOf("!") + 1;
    char data_final[length_data + 1];
    data.toCharArray(data_final, length_data + 1);
    imu_msg.data = data_final;
    pub.publish(&imu_msg);
    nh.spinOnce();

    Serial.print("IMU Y轴: "); Serial.println(ey_s);
    vTaskDelay(10 / portTICK_PERIOD_MS); // 100Hz读取频率
  }
}

/************************** 核心1任务：步进电机控制 **************************/
void motor_control_task(void *pvParameters) {
  pinMode(STEP_PIN, OUTPUT);
  pinMode(DIR_PIN, OUTPUT);
  digitalWrite(DIR_PIN, HIGH);
  bool dir_state = HIGH;
  long current_steps = 0;
  unsigned long last_step_time = 0;

  while(1) {
    unsigned long now = micros();
    // 非阻塞输出步进脉冲
    if(now - last_step_time >= STEP_INTERVAL) {
      last_step_time = now;
      // 输出脉冲
      digitalWrite(STEP_PIN, HIGH);
      delayMicroseconds(STEP_PULSE_WIDTH);
      digitalWrite(STEP_PIN, LOW);

      // 更新步数
      current_steps++;
      // 到达限位换向
      if(current_steps >= MAX_POSITION) {
        dir_state = !dir_state;
        digitalWrite(DIR_PIN, dir_state);
        current_steps = 0;
      }

      // 后续可在此处读取shared_ey_s做位置闭环控制
      // float target_ey;
      // portENTER_CRITICAL(&mux);
      // target_ey = shared_ey_s;
      // portEXIT_CRITICAL(&mux);
      // 根据target_ey计算目标位置、调整STEP_INTERVAL实现速度控制
    }
  }
}

/************************** 初始化 **************************/
void setup() {
  Serial.begin(57600);
  setupWiFi();

  // 初始化I2C和IMU
  Wire.begin(I2C_SDA, I2C_SCL);
  I2Cbno.begin(I2C_SDA, I2C_SCL, 400000); 
  bno_master.begin();
  bno_slave.begin();
  uint8_t system, gyro, accel, mg = 0;
  bno_master.getCalibration(&system, &gyro, &accel, &mg);
  bno_slave.getCalibration(&system, &gyro, &accel, &mg);
  bno_master.setExtCrystalUse(true);
  bno_slave.setExtCrystalUse(true);

  // 初始化ROS
  nh.initNode();
  nh.advertise(pub);
  nh.subscribe(sub);

  // 绑定任务到对应核心
  xTaskCreatePinnedToCore(
    imu_ros_task,
    "IMU_ROS_Task",
    8192, // 栈大小
    NULL,
    2, // 优先级
    NULL,
    0 // 绑定核心0
  );

  xTaskCreatePinnedToCore(
    motor_control_task,
    "Motor_Task",
    4096,
    NULL,
    24, // 最高优先级
    NULL,
    1 // 绑定核心1
  );
}

void loop() {
  // 主loop留空，所有逻辑都在FreeRTOS任务中执行
  vTaskDelay(1000 / portTICK_PERIOD_MS);
}

• 你可以在电机控制任务的预留位置读取shared_ey_s变量，实现角度到步数的映射逻辑，计算PI控制量后修改STEP_INTERVAL即可调整电机转速
• 若需要更高的IMU读取频率，直接修改imu_ros_task中的vTaskDelay参数即可，不会影响电机运行稳定性

内容的提问来源于stack exchange，提问作者Paulita