基于Arduino的爬行机器人控制代码解析

本文将详细解析一个基于Arduino Nano的4足爬行机器人控制代码，该代码由Sunfounder设计并由Regis修改。这个机器人每条腿由3个舵机驱动，具有前进、后退、转向、挥手等多种功能。

代码来自：https://www.instructables.com/DIY-Spider-RobotQuad-robot-Quadruped/

硬件配置

舵机配置

#include <Servo.h>    // 舵机控制库
#include <FlexiTimer2.h> // 定时器库，用于管理所有舵机

// 定义12个舵机（4条腿×3个舵机）
Servo servo[4][3];

// 定义舵机引脚
const int servo_pin[4][3] = { 
  {2, 3, 4},   // 腿0的3个舵机引脚
  {5, 6, 7},   // 腿1的3个舵机引脚
  {8, 9, 10},  // 腿2的3个舵机引脚
  {11, 12, 13} // 腿3的3个舵机引脚
};

机器人尺寸参数

/* 机器人尺寸参数 ---------------------------------------------------------*/
const float length_a = 55;     // 第一段肢体长度(mm)
const float length_b = 77.5;   // 第二段肢体长度(mm)
const float length_c = 27.5;   // 第三段肢体长度(mm)
const float length_side = 71;  // 机器人侧面长度(mm)
const float z_absolute = -28;  // 绝对高度坐标(mm)

运动控制

初始化设置

void setup() {
  Serial.begin(115200);
  Serial.println("Robot starts initialization");
  
  // 初始化默认位置
  set_site(0, x_default - x_offset, y_start + y_step, z_boot);
  set_site(1, x_default - x_offset, y_start + y_step, z_boot);
  set_site(2, x_default + x_offset, y_start, z_boot);
  set_site(3, x_default + x_offset, y_start, z_boot);
  
  // 设置当前位置为期望位置
  for (int i = 0; i < 4; i++) {
    for (int j = 0; j < 3; j++) {
      site_now[i][j] = site_expect[i][j];
    }
  }
  
  // 启动舵机服务(20ms周期，50Hz)
  FlexiTimer2::set(20, servo_service);
  FlexiTimer2::start();
  
  // 初始化舵机
  servo_attach();
  
  Serial.println("Robot initialization Complete");
}

基本动作函数

站立和坐下

// 站立 - 阻塞函数
void stand(void) {
  move_speed = stand_seat_speed;
  for (int leg = 0; leg < 4; leg++) {
    set_site(leg, KEEP, KEEP, z_default); // 设置所有腿到默认高度
  }
  wait_all_reach(); // 等待所有腿到达目标位置
}

// 坐下 - 阻塞函数
void sit(void) {
  move_speed = stand_seat_speed;
  for (int leg = 0; leg < 4; leg++) {
    set_site(leg, KEEP, KEEP, z_boot); // 设置所有腿到最低高度
  }
  wait_all_reach();
}

前进和后退

// 前进 - 阻塞函数
// 参数step: 要前进的步数
void step_forward(unsigned int step) {
  move_speed = leg_move_speed;
  while (step-- > 0) {
    if (site_now[2][1] == y_start) {
      // 腿2&1移动模式
      set_site(2, x_default + x_offset, y_start, z_up); // 抬起腿2
      wait_all_reach();
      set_site(2, x_default + x_offset, y_start + 2 * y_step, z_up); // 向前移动腿2
      wait_all_reach();
      set_site(2, x_default + x_offset, y_start + 2 * y_step, z_default); // 放下腿2
      wait_all_reach();
      
      // 身体前移
      move_speed = body_move_speed;
      set_site(0, x_default + x_offset, y_start, z_default);
      set_site(1, x_default + x_offset, y_start + 2 * y_step, z_default);
      set_site(2, x_default - x_offset, y_start + y_step, z_default);
      set_site(3, x_default - x_offset, y_start + y_step, z_default);
      wait_all_reach();
      
      // 抬起并收回腿1
      move_speed = leg_move_speed;
      set_site(1, x_default + x_offset, y_start + 2 * y_step, z_up);
      wait_all_reach();
      set_site(1, x_default + x_offset, y_start, z_up);
      wait_all_reach();
      set_site(1, x_default + x_offset, y_start, z_default);
      wait_all_reach();
    } else {
      // 腿0&3移动模式
      // 类似逻辑，只是换另一组腿运动
    }
  }
}

转向控制

// 左转 - 阻塞函数
// 参数step: 要转动的步数
void turn_left(unsigned int step) {
  move_speed = spot_turn_speed;
  while (step-- > 0) {
    if (site_now[3][1] == y_start) {
      // 腿3&1移动模式
      set_site(3, x_default + x_offset, y_start, z_up);
      wait_all_reach();
      
      // 计算并设置转向位置
      set_site(0, turn_x1 - x_offset, turn_y1, z_default);
      set_site(1, turn_x0 - x_offset, turn_y0, z_default);
      set_site(2, turn_x1 + x_offset, turn_y1, z_default);
      set_site(3, turn_x0 + x_offset, turn_y0, z_up);
      wait_all_reach();
      
      // 完成转向动作
      set_site(3, turn_x0 + x_offset, turn_y0, z_default);
      wait_all_reach();
      
      // 调整到新方向
      set_site(0, turn_x1 + x_offset, turn_y1, z_default);
      set_site(1, turn_x0 + x_offset, turn_y0, z_default);
      set_site(2, turn_x1 - x_offset, turn_y1, z_default);
      set_site(3, turn_x0 - x_offset, turn_y0, z_default);
      wait_all_reach();
      
      // 抬起并调整腿1
      set_site(1, turn_x0 + x_offset, turn_y0, z_up);
      wait_all_reach();
      
      // 回到初始位置
      set_site(0, x_default + x_offset, y_start, z_default);
      set_site(1, x_default + x_offset, y_start, z_up);
      set_site(2, x_default - x_offset, y_start + y_step, z_default);
      set_site(3, x_default - x_offset, y_start + y_step, z_default);
      wait_all_reach();
      
      set_site(1, x_default + x_offset, y_start, z_default);
      wait_all_reach();
    } else {
      // 腿0&2移动模式
      // 类似逻辑，只是换另一组腿运动
    }
  }
}

数学计算与坐标转换

笛卡尔坐标到极坐标转换

/* 
  - 将笛卡尔坐标转换为极坐标
  - 数学模型2/2
   ---------------------------------------------------------------------------*/
void cartesian_to_polar(volatile float &alpha, volatile float &beta, volatile float &gamma, 
                       volatile float x, volatile float y, volatile float z) {
  // 计算w-z角度
  float v, w;
  w = (x >= 0 ? 1 : -1) * (sqrt(pow(x, 2) + pow(y, 2)));
  v = w - length_c;
  
  // 使用余弦定理计算alpha和beta角度
  alpha = atan2(z, v) + acos((pow(length_a, 2) - pow(length_b, 2) + pow(v, 2) + pow(z, 2)) 
          / 2 / length_a / sqrt(pow(v, 2) + pow(z, 2));
  beta = acos((pow(length_a, 2) + pow(length_b, 2) - pow(v, 2) - pow(z, 2)) 
         / 2 / length_a / length_b;
  
  // 计算x-y-z角度
  gamma = (w >= 0) ? atan2(y, x) : atan2(-y, -x);
  
  // 将弧度转换为角度
  alpha = alpha / pi * 180;
  beta = beta / pi * 180;
  gamma = gamma / pi * 180;
}

极坐标到舵机角度转换

/* 
  - 将极坐标转换为舵机角度
  - 数学模型映射到实际硬件
  - EEPROM中保存的误差将被添加
   ---------------------------------------------------------------------------*/
void polar_to_servo(int leg, float alpha, float beta, float gamma) {
  // 根据不同腿调整角度
  if (leg == 0) {
    alpha = 90 - alpha;
    beta = beta;
    gamma += 90;
  } else if (leg == 1) {
    alpha += 90;
    beta = 180 - beta;
    gamma = 90 - gamma;
  } else if (leg == 2) {
    alpha += 90;
    beta = 180 - beta;
    gamma = 90 - gamma;
  } else if (leg == 3) {
    alpha = 90 - alpha;
    beta = beta;
    gamma += 90;
  }

  // 设置舵机角度
  servo[leg][0].write(alpha);
  servo[leg][1].write(beta);
  servo[leg][2].write(gamma);
}

高级动作功能

挥手动作

// 挥手动作
// 参数i: 挥手次数
void hand_wave(int i) {
  float x_tmp, y_tmp, z_tmp;
  move_speed = 1;
  
  if (site_now[3][1] == y_start) {
    // 身体右移
    body_right(15);
    
    // 保存腿2当前位置
    x_tmp = site_now[2][0];
    y_tmp = site_now[2][1];
    z_tmp = site_now[2][2];
    
    // 挥手动作
    move_speed = body_move_speed;
    for (int j = 0; j < i; j++) {
      set_site(2, turn_x1, turn_y1, 50); // 抬起并前伸
      wait_all_reach();
      set_site(2, turn_x0, turn_y0, 50); // 收回
      wait_all_reach();
    }
    
    // 恢复腿2位置
    set_site(2, x_tmp, y_tmp, z_tmp);
    wait_all_reach();
    
    // 身体左移回位
    move_speed = 1;
    body_left(15);
  } else {
    // 类似逻辑，使用另一侧的腿
  }
}

舞蹈动作

// 身体舞蹈动作
// 参数i: 舞蹈循环次数
void body_dance(int i) {
  float x_tmp, y_tmp, z_tmp;
  float body_dance_speed = 2;
  
  // 先坐下
  sit();
  move_speed = 1;
  
  // 设置到默认位置
  set_site(0, x_default, y_default, KEEP);
  set_site(1, x_default, y_default, KEEP);
  set_site(2, x_default, y_default, KEEP);
  set_site(3, x_default, y_default, KEEP);
  wait_all_reach();
  
  // 半站立姿势
  set_site(0, x_default, y_default, z_default - 20);
  set_site(1, x_default, y_default, z_default - 20);
  set_site(2, x_default, y_default, z_default - 20);
  set_site(3, x_default, y_default, z_default - 20);
  wait_all_reach();
  
  // 抬头
  move_speed = body_dance_speed;
  head_up(30);
  
  // 舞蹈摆动循环
  for (int j = 0; j < i; j++) {
    if (j > i / 4) move_speed = body_dance_speed * 2;
    if (j > i / 2) move_speed = body_dance_speed * 3;
    
    // 左右摆动身体
    set_site(0, KEEP, y_default - 20, KEEP);
    set_site(1, KEEP, y_default + 20, KEEP);
    set_site(2, KEEP, y_default - 20, KEEP);
    set_site(3, KEEP, y_default + 20, KEEP);
    wait_all_reach();
    
    set_site(0, KEEP, y_default + 20, KEEP);
    set_site(1, KEEP, y_default - 20, KEEP);
    set_site(2, KEEP, y_default + 20, KEEP);
    set_site(3, KEEP, y_default - 20, KEEP);
    wait_all_reach();
  }
  
  // 低头
  move_speed = body_dance_speed;
  head_down(30);
}

总结

这段代码实现了一个完整的四足机器人控制系统，主要特点包括：

1. 模块化设计：将机器人的各种动作封装成独立函数，便于调用和扩展
2. 数学模型：使用几何和三角函数计算各关节角度，实现精确控制
3. 多线程处理：利用定时器中断实现平稳的舵机控制
4. 多种运动模式：支持前进、后退、转向、挥手、跳舞等多种动作
5. 参数化配置：所有尺寸和速度参数都可调，便于适应不同硬件

通过分析这段代码，我们可以学习到如何设计复杂的机器人控制系统，包括运动规划、坐标转换和实时控制等关键技术。