笔者在学期初的时候，跟着赵虚左老师的ROS课程手搓了一个基于Arduino的移动机器人，当时完成机器人基于键盘节点的移动控制初步测试后，未继续开发建图及导航功能。

先说所用到的硬件，该移动机器人的开发板型号是Arduino mega 2560。

电机驱动用的是TB6612电机驱动板。

上位机是树莓派4B

关于直流减速电机，电机编码器和雷达这里就不一一列举了。这里雷达笔者使用的是乐动LD14P，买完雷达之后可以问客服要相关代码，你只需要跟着教程修改一下串口信息，然后将具体launch文件集成到机器人使能的总launch文件里即可。

先说硬件的具体连接吧，mega2560和TB6612的具体接线如下图所示。

这里TB6612需要两个串口来控制一个电机的方向，具体控制逻辑如下图。

笔者的接线也作为记录和参考，放在下面。

———————————————————————————————————————————

然后具体的软件方面的实现跟着赵老师的课程来就可以了，但是还有一些问题需要说明一下。

1、赵老师课程里使用的驱动板与我们使用的TB6612不同，所以在使用ros_arduino_bridge进行电机测速，电机调速等操作时，需要对代码进行一些修改。

 （1）修改 arduino_params.yaml 文件

# 端口配置
port: /dev/ttyACM0
baud: 57600

# 机器人参数
robot_base_frame: base_footprint
# 更新为您的实际轮子参数
wheel_diameter: 0.065  # 轮子直径（米）
wheel_track: 0.26      # 两轮间距（米）
encoder_resolution: 1560  # 编码器每转脉冲数

# PID参数（需要根据实际调整）
Kp: 10
Kd: 0
Ki: 0
Ko: 50

# 电机方向（根据接线可能需要调整）
motor_left_dir: 1      # 1为正，-1为反向
motor_right_dir: -1

# 电机引脚配置 - 根据TB6612接线修改
motor_driver: TB6612
max_pwm: 255          # PWM最大值
motor_left_pwm_pin: 5
motor_left_dir1_pin: 7  # TB6612的AIN1
motor_left_dir2_pin: 6  # TB6612的AIN2
motor_right_pwm_pin: 10
motor_right_dir1_pin: 9  # TB6612的BIN1
motor_right_dir2_pin: 8  # TB6612的BIN2

# 编码器引脚配置 - 使用Mega的中断引脚
encoder_driver: Quadrature
encoder_left_pin_a: 2   # 外部中断0
encoder_left_pin_b: 22  # 普通数字引脚
encoder_right_pin_a: 3  # 外部中断1
encoder_right_pin_b: 23 # 普通数字引脚

# 启用TB6612的STBY引脚（如有使用）
enable_STBY_pin: 4      # STBY引脚

这里需要注意的是，你每次启动小车或者插拔某个USB串口时，这里的port都可能发生变化，这时你就需要根据变化修改这里的port，保证其是arduino开发板的对应串口。

确定arduino开发板对应串口的方法：你可以先打开arduino ide，然后插拔arduino开发板的USB接口，观察下图位置串口的情况，随着你插拔而出现消失的串口即为arduino开发板对应的串口，然后把他填入arduino_params.yaml的port中即可。

（2）修改 ros_arduino_firmware.ino 文件

// 在文件顶部添加TB6612引脚定义
#define MOTOR_LEFT_PWM 5
#define MOTOR_LEFT_IN1 7
#define MOTOR_LEFT_IN2 6
#define MOTOR_RIGHT_PWM 10
#define MOTOR_RIGHT_IN1 9
#define MOTOR_RIGHT_IN2 8
#define STBY_PIN 4  // 如果使用STBY引脚

// 编码器引脚
#define LEFT_ENC_PIN_A 2   // 中断0
#define LEFT_ENC_PIN_B 22
#define RIGHT_ENC_PIN_A 3  // 中断1
#define RIGHT_ENC_PIN_B 23

（3）修改 motor_driver.cpp 文件

// 修改 setMotorSpeed() 函数以支持TB6612
void setMotorSpeed(int i, int sp) {
  if (sp > MAX_PWM) sp = MAX_PWM;
  if (sp < -MAX_PWM) sp = -MAX_PWM;
  
  if (i == LEFT) {
    if (sp > 0) {
      // 正转
      digitalWrite(MOTOR_LEFT_IN1, HIGH);
      digitalWrite(MOTOR_LEFT_IN2, LOW);
      analogWrite(MOTOR_LEFT_PWM, sp);
    } else if (sp < 0) {
      // 反转
      digitalWrite(MOTOR_LEFT_IN1, LOW);
      digitalWrite(MOTOR_LEFT_IN2, HIGH);
      analogWrite(MOTOR_LEFT_PWM, -sp);
    } else {
      // 停止
      digitalWrite(MOTOR_LEFT_IN1, LOW);
      digitalWrite(MOTOR_LEFT_IN2, LOW);
      analogWrite(MOTOR_LEFT_PWM, 0);
    }
  } else {
    // 右电机同理
    if (sp > 0) {
      digitalWrite(MOTOR_RIGHT_IN1, HIGH);
      digitalWrite(MOTOR_RIGHT_IN2, LOW);
      analogWrite(MOTOR_RIGHT_PWM, sp);
    } else if (sp < 0) {
      digitalWrite(MOTOR_RIGHT_IN1, LOW);
      digitalWrite(MOTOR_RIGHT_IN2, HIGH);
      analogWrite(MOTOR_RIGHT_PWM, -sp);
    } else {
      digitalWrite(MOTOR_RIGHT_IN1, LOW);
      digitalWrite(MOTOR_RIGHT_IN2, LOW);
      analogWrite(MOTOR_RIGHT_PWM, 0);
    }
  }
}

// 添加TB6612初始化函数
void initTB6612() {
  pinMode(MOTOR_LEFT_IN1, OUTPUT);
  pinMode(MOTOR_LEFT_IN2, OUTPUT);
  pinMode(MOTOR_LEFT_PWM, OUTPUT);
  pinMode(MOTOR_RIGHT_IN1, OUTPUT);
  pinMode(MOTOR_RIGHT_IN2, OUTPUT);
  pinMode(MOTOR_RIGHT_PWM, OUTPUT);
  
  // 如果使用STBY引脚
  #ifdef STBY_PIN
    pinMode(STBY_PIN, OUTPUT);
    digitalWrite(STBY_PIN, HIGH);  // 使能电机驱动
  #endif
}

（4）修改 encoder_driver.cpp 文件

// 针对Mega 2560的中断引脚修改
void initEncoders() {
  // 左编码器 - 使用外部中断0
  pinMode(LEFT_ENC_PIN_A, INPUT_PULLUP);
  pinMode(LEFT_ENC_PIN_B, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(LEFT_ENC_PIN_A), leftEncoderISR, CHANGE);
  
  // 右编码器 - 使用外部中断1
  pinMode(RIGHT_ENC_PIN_A, INPUT_PULLUP);
  pinMode(RIGHT_ENC_PIN_B, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(RIGHT_ENC_PIN_A), rightEncoderISR, CHANGE);
}

// 修改中断服务函数以提高精度
void leftEncoderISR() {
  int val = digitalRead(LEFT_ENC_PIN_B);
  if (val == LOW) {
    if (digitalRead(LEFT_ENC_PIN_A) == HIGH) {
      left_encoder_count--;
    } else {
      left_encoder_count++;
    }
  } else {
    if (digitalRead(LEFT_ENC_PIN_A) == HIGH) {
      left_encoder_count++;
    } else {
      left_encoder_count--;
    }
  }
}

void rightEncoderISR() {
  int val = digitalRead(RIGHT_ENC_PIN_B);
  if (val == LOW) {
    if (digitalRead(RIGHT_ENC_PIN_A) == HIGH) {
      right_encoder_count--;
    } else {
      right_encoder_count++;
    }
  } else {
    if (digitalRead(RIGHT_ENC_PIN_A) == HIGH) {
      right_encoder_count++;
    } else {
      right_encoder_count--;
    }
  }
}

（5）修改 diff_controller.cpp 文件

// 更新PID控制参数
void updatePID() {
  unsigned long time = millis();
  unsigned long dt = time - lastUpdateTime;
  
  if (dt == 0) return;
  
  // 计算左右轮的目标速度（转/秒）
  float target_left = (goal_vx - goal_vy * wheel_track / 2.0) / (wheel_diameter * PI);
  float target_right = (goal_vx + goal_vy * wheel_track / 2.0) / (wheel_diameter * PI);
  
  // 转换为编码器脉冲/毫秒
  target_left = (target_left * encoder_resolution) / 1000.0;
  target_right = (target_right * encoder_resolution) / 1000.0;
  
  // 获取实际速度（脉冲数/毫秒）
  float actual_left = (left_encoder_count - left_encoder_prev_count) / dt;
  float actual_right = (right_encoder_count - right_encoder_prev_count) / dt;
  
  // PID计算
  float error_left = target_left - actual_left;
  float error_right = target_right - actual_right;
  
  integral_left += error_left * dt;
  integral_right += error_right * dt;
  
  float derivative_left = (error_left - last_error_left) / dt;
  float derivative_right = (error_right - last_error_right) / dt;
  
  // 计算PWM输出
  int pwm_left = Kp * error_left + Ki * integral_left + Kd * derivative_left;
  int pwm_right = Kp * error_right + Ki * integral_right + Kd * derivative_right;
  
  // 限制PWM值
  pwm_left = constrain(pwm_left, -max_pwm, max_pwm);
  pwm_right = constrain(pwm_right, -max_pwm, max_pwm);
  
  // 应用方向设置
  pwm_left *= motor_left_dir;
  pwm_right *= motor_right_dir;
  
  // 设置电机速度
  setMotorSpeed(LEFT, pwm_left);
  setMotorSpeed(RIGHT, pwm_right);
  
  // 更新变量
  last_error_left = error_left;
  last_error_right = error_right;
  left_encoder_prev_count = left_encoder_count;
  right_encoder_prev_count = right_encoder_count;
  lastUpdateTime = time;
}

2、在搭建电脑与树莓派的分布式框架时，可能会出现只能单向通信，即只能电脑ping通树莓派，树莓派却ping不通电脑，或者反过来的情况，这种情况笔者之前总结过，可以参考如下文章。

关于ROS分布式通信（多机通讯）中，只能单向通信问题的解决。_ros多机通信 防火墙-CSDN博客

3、在接线时，arduino mega 2560和TB6612的正负极一定要注意连接正确，千万不要马虎接反了：TB6612的正极接mega 2560的VIN，TB6612的负极接mega 2560的GND。笔者就是因为正负极接反了烧了一个开发板，导致项目延后了两三天。。。所以正负极这部分，最好每次启动前都检查一下。

接线参考链接：

Arduino_mage2560 加 TB6612稳压版驱动MG513直流电机_mg513电机-CSDN博客

# 单片机 # 嵌入式硬件
Logo
DAMO开发者矩阵

DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。

更多推荐

cover

硬件工程师必看：如何用3颗麦克风实现360°六向声源定位？

avatar DAMO开发者矩阵
cover

用 Bub 和飞书搭一个更懂群聊上下文的小机器人

avatar DAMO开发者矩阵
cover

面向可靠Sim-to-Real可预测性的MoE鲁棒四足运动控制——文献深度解读

avatar DAMO开发者矩阵