[ESP8266/ESP32]基于Firebeetle 2 ESP32-C5的五轴机械臂控制系...

## 一、回顾与目标

在第一篇教程中,我们完成了:
- ✅ 硬件连接与接线
- ✅ Arduino环境配置
- ✅ 基础舵机控制代码
- ✅ NRF24L01无线接收

本教程(第二部分)将实现更高级的功能:
1. **发射端遥控器代码**: 完整的控制端程序
2. **平滑运动算法**: 插值实现舵机平滑过渡
3. **动作序列录制与回放**: 记忆机械臂动作
4. **限位保护与速度控制**: 增强系统安全性
5. **优化技巧**: 性能提升与调试方法

## 二、发射端(遥控器)完整代码

### (一) 硬件需求

**发射端硬件清单**:
| 序号 | 名称 | 数量 | 用途 |
|------|------|------|------|
| 1 | Firebeetle 2 ESP32-C5 | 1 | 发射端主控 |
| 2 | NRF24L01无线模块 | 1 | 2.4GHz发射 |
| 3 | 双轴摇杆模块 | 2 | 控制4个舵机 |
| 4 | 扭子开关 | 1 | 控制夹爪 |
| 5 | 按钮开关 | 1 | 功能切换(可选) |

### (二) 发射端接线

| 模块/传感器 | ESP32-C5引脚 | 说明 |
|------------|-------------|------|
| **NRF24L01** | | |
| VCC | 3.3V | 电源 |
| GND | GND | 地线 |
| CE | GPIO9 | 使能 |
| CSN | GPIO10 | 片选 |
| SCK | GPIO6 | 时钟 |
| MOSI | GPIO7 | 数据输出 |
| MISO | GPIO2 | 数据输入 |
| **摇杆1(左)** | | |
| VRx | GPIO3 (ADC) | X轴(控制舵机1) |
| VRy | GPIO4 (ADC) | Y轴(控制舵机2) |
| VCC | 3.3V | 电源 |
| GND | GND | 地线 |
| **摇杆2(右)** | | |
| VRx | GPIO5 (ADC) | X轴(控制舵机3) |
| VRy | GPIO11 (ADC) | Y轴(控制舵机4) |
| VCC | 3.3V | 电源 |
| GND | GND | 地线 |
| **扭子开关** | | |
| 信号端 | GPIO15 | 控制舵机5(夹爪) |
| 公共端 | GND | 地线 |

### (三) 发射端完整代码

```cpp
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>

// ===== 引脚定义 =====
#define CE_PIN   9
#define CSN_PIN  10

// 摇杆ADC引脚
#define JOYSTICK1_X  3   // 左摇杆X(底座)
#define JOYSTICK1_Y  4   // 左摇杆Y(大臂)
#define JOYSTICK2_X  5   // 右摇杆X(小臂)
#define JOYSTICK2_Y  11  // 右摇杆Y(手腕)

// 开关引脚
#define GRIPPER_SWITCH 15  // 夹爪开关

// ===== 全局对象 =====
const uint64_t pipeOut = 0x123456789;  // 与接收端一致
RF24 radio(CE_PIN, CSN_PIN);

const int deadzone = 5;  // 摇杆死区
const int maxRetries = 3;  // 最大重试次数
const int retryDelay = 10;  // 重试延时(ms)

// ===== 数据结构 =====
struct Signal {
  int ele;   // 底座旋转(-512~512)
  int ail;   // 大臂抬升(-512~512)
  int thr;   // 小臂伸缩(-512~512)
  int rud;   // 手腕旋转(-512~512)
  byte aux1; // 夹爪开合(-512/512)
  byte aux2; // 预留
};
Signal data;

// ===== 辅助函数 =====
void ResetData() {
  data.ele = 0;
  data.ail = 0;
  data.thr = 0;
  data.rud = 0;
  data.aux1 = -512;  // 夹爪默认打开
  data.aux2 = 0;
}

// 死区处理函数
int applyDeadzone(int value, int deadzone, int center) {
  if (abs(value - center) <= deadzone) {
    return center;
  }
  return value;
}

// 带重试的发送函数
bool sendData(const void* data, size_t dataSize) {
  for (int retryCount = 0; retryCount < maxRetries; retryCount++) {
    if (radio.write(data, dataSize)) {
      if (retryCount > 0) {
        Serial.print("发送成功(重试次数:");
        Serial.print(retryCount);
        Serial.println(")");
      }
      return true;
    }
    delay(retryDelay);
  }
  Serial.println("发送失败!");
  return false;
}

// ===== 初始化 =====
void setup() {
  Serial.begin(115200);
  Serial.println("机械臂发射端初始化...");

  // NRF24L01配置
  if (!radio.begin()) {
    Serial.println("NRF24L01初始化失败!");
    while (1);
  }
  
  radio.openWritingPipe(pipeOut);
  radio.stopListening();  // 发射模式
  radio.setDataRate(RF24_250KBPS);  // 低速率高可靠
  radio.setPALevel(RF24_PA_HIGH);   // 高功率
  radio.setRetries(5, 10);          // 硬件重试
  
  Serial.println("NRF24L01初始化成功!");

  // 初始化数据
  ResetData();

  // 配置ADC分辨率(10位:0-1023)
  analogReadResolution(10);

  // 配置开关引脚
  pinMode(GRIPPER_SWITCH, INPUT_PULLUP);

  Serial.println("系统就绪!");
}

// ===== 主循环 =====
void loop() {
  // 1. 读取摇杆模拟值并映射到-512~512
  int raw_ele = map(analogRead(JOYSTICK1_X), 0, 1023, -512, 512);
  int raw_ail = map(analogRead(JOYSTICK1_Y), 0, 1023, -512, 512);
  int raw_thr = map(analogRead(JOYSTICK2_X), 0, 1023, -512, 512);
  int raw_rud = map(analogRead(JOYSTICK2_Y), 0, 1023, -512, 512);

  // 2. 应用死区处理
  data.ele = applyDeadzone(raw_ele, deadzone, 0);
  data.ail = applyDeadzone(raw_ail, deadzone, 0);
  data.thr = applyDeadzone(raw_thr, deadzone, 0);
  data.rud = applyDeadzone(raw_rud, deadzone, 0);

  // 3. 读取夹爪开关(LOW=闭合, HIGH=打开)
  data.aux1 = digitalRead(GRIPPER_SWITCH) ? -512 : 512;

  // 4. 发送数据
  sendData(&data, sizeof(Signal));

  // 5. 调试输出
  Serial.print("发送数据 -> ");
  Serial.print("底座:"); Serial.print(data.ele);
  Serial.print(" 大臂:"); Serial.print(data.ail);
  Serial.print(" 小臂:"); Serial.print(data.thr);
  Serial.print(" 手腕:"); Serial.print(data.rud);
  Serial.print(" 夹爪:"); Serial.println(data.aux1);

  delay(20);  // 50Hz发送频率
}
```

### (四) 代码说明

**关键设计要点**:
1. **摇杆校准**: `map(analogRead(), 0, 1023, -512, 512)`根据实际情况调整范围
2. **死区过滤**: 摇杆中位附近5个单位的抖动被过滤
3. **开关映射**: `digitalRead()`结果直接映射为-512/512
4. **重试机制**: 3次重试确保数据可靠传输

## 三、平滑运动算法

### (一) 问题分析

基础代码中舵机直接跳转到目标角度,导致:
- ❌ 动作生硬,不自然
- ❌ 瞬时电流大,可能导致电源不稳
- ❌ 机械冲击,损伤舵机齿轮

**解决方案**: 使用线性插值,让舵机以固定步长平滑移动。

### (二) 平滑运动代码

在**接收端代码**中添加以下改进:

```cpp
// ===== 全局变量(添加到代码顶部) =====
int currentAngle[5] = {90, 90, 90, 90, 90};  // 当前角度
int targetAngle[5] = {90, 90, 90, 90, 90};   // 目标角度
const int smoothStep = 2;  // 平滑步长(角度/次)

// ===== 修改controlServos函数 =====
void controlServos() {
  // 计算目标角度
  targetAngle[0] = constrain(mapToServoAngle(receivedData.ele), 0, 180);
  targetAngle[1] = constrain(mapToServoAngle(receivedData.ail), 0, 180);
  targetAngle[2] = constrain(mapToServoAngle(receivedData.thr), 0, 180);
  targetAngle[3] = constrain(mapToServoAngle(receivedData.rud), 0, 180);
  targetAngle[4] = constrain(mapAuxToServo(receivedData.aux1), 0, 180);
  
  // 平滑插值移动
  for (int i = 0; i < 5; i++) {
    if (currentAngle < targetAngle) {
      currentAngle += min(smoothStep, targetAngle - currentAngle);
    } else if (currentAngle > targetAngle) {
      currentAngle -= min(smoothStep, currentAngle - targetAngle);
    }
  }
  
  // 写入舵机
  servo1.write(currentAngle[0]);
  servo2.write(currentAngle[1]);
  servo3.write(currentAngle[2]);
  servo4.write(currentAngle[3]);
  servo5.write(currentAngle[4]);
  
  // 调试输出
  Serial.print("当前角度 -> ");
  for (int i = 0; i < 5; i++) {
    Serial.print(i+1); Serial.print(":"); Serial.print(currentAngle);
    Serial.print(" ");
  }
  Serial.println();
}
```

**参数调整**:
- `smoothStep = 1`: 非常缓慢,适合精细操作
- `smoothStep = 2`: 平衡速度与平滑度(推荐)
- `smoothStep = 5`: 快速响应,牺牲平滑度

## 四、动作序列录制与回放

### (一) 功能设计

实现类似"示教"的功能:
1. **录制模式**: 记录机械臂的位置序列
2. **回放模式**: 自动重复录制的动作
3. **存储**: 保存多组动作到EEPROM

### (二) 录制回放代码

在**接收端代码**中添加:

```cpp
#include <Preferences.h>  // ESP32持久化存储

// ===== 全局变量 =====
Preferences preferences;
const int MAX_STEPS = 50;  // 最多录制50步
int recordedAngles[MAX_STEPS][5];  // 录制的角度数据
int recordedSteps = 0;  // 已录制步数
bool recordMode = false;  // 录制模式标志
bool playbackMode = false;  // 回放模式标志

// 按钮引脚(可选:用于切换模式)
#define RECORD_BUTTON 14
#define PLAYBACK_BUTTON 22

// ===== 初始化中添加 =====
void setup() {
  // ...现有代码...
  
  pinMode(RECORD_BUTTON, INPUT_PULLUP);
  pinMode(PLAYBACK_BUTTON, INPUT_PULLUP);
  
  preferences.begin("robot", false);  // 打开命名空间
}

// ===== 录制函数 =====
void recordStep() {
  if (recordedSteps < MAX_STEPS) {
    for (int i = 0; i < 5; i++) {
      recordedAngles[recordedSteps] = currentAngle;
    }
    recordedSteps++;
    Serial.print("录制步骤 ");
    Serial.print(recordedSteps);
    Serial.println("/50");
  } else {
    Serial.println("录制已满!");
    recordMode = false;
  }
}

// ===== 回放函数 =====
void playbackSequence() {
  Serial.println("开始回放...");
  
  for (int step = 0; step < recordedSteps; step++) {
    Serial.print("执行步骤 ");
    Serial.print(step + 1);
    Serial.print("/");
    Serial.println(recordedSteps);
   
    // 设置目标角度
    for (int i = 0; i < 5; i++) {
      targetAngle = recordedAngles[step];
    }
   
    // 平滑移动到目标位置
    bool arrived = false;
    while (!arrived) {
      arrived = true;
      
      for (int i = 0; i < 5; i++) {
        if (currentAngle != targetAngle) {
          arrived = false;
         
          if (currentAngle < targetAngle) {
            currentAngle += min(smoothStep, targetAngle - currentAngle);
          } else {
            currentAngle -= min(smoothStep, currentAngle - targetAngle);
          }
        }
      }
      
      // 写入舵机
      servo1.write(currentAngle[0]);
      servo2.write(currentAngle[1]);
      servo3.write(currentAngle[2]);
      servo4.write(currentAngle[3]);
      servo5.write(currentAngle[4]);
      
      delay(20);
    }
   
    delay(500);  // 每步之间停留0.5秒
  }
  
  Serial.println("回放完成!");
  playbackMode = false;
}

// ===== 保存到EEPROM =====
void saveSequence(int slot) {
  String key = "seq" + String(slot);
  preferences.putInt(key + "_steps", recordedSteps);
  
  for (int i = 0; i < recordedSteps; i++) {
    String stepKey = key + "_" + String(i);
    preferences.putBytes(stepKey.c_str(), recordedAngles, sizeof(recordedAngles));
  }
  
  Serial.print("序列已保存到槽位 ");
  Serial.println(slot);
}

// ===== 从EEPROM加载 =====
void loadSequence(int slot) {
  String key = "seq" + String(slot);
  recordedSteps = preferences.getInt(key + "_steps", 0);
  
  for (int i = 0; i < recordedSteps; i++) {
    String stepKey = key + "_" + String(i);
    preferences.getBytes(stepKey.c_str(), recordedAngles, sizeof(recordedAngles));
  }
  
  Serial.print("从槽位 ");
  Serial.print(slot);
  Serial.print(" 加载了 ");
  Serial.print(recordedSteps);
  Serial.println(" 步动作");
}

// ===== 主循环中添加 =====
void loop() {
  // 检查录制按钮
  if (digitalRead(RECORD_BUTTON) == LOW) {
    delay(50);  // 消抖
    if (digitalRead(RECORD_BUTTON) == LOW) {
      recordMode = !recordMode;
      if (recordMode) {
        recordedSteps = 0;  // 重置
        Serial.println("开始录制!");
      } else {
        Serial.println("停止录制!");
        saveSequence(1);  // 保存到槽位1
      }
      while (digitalRead(RECORD_BUTTON) == LOW);  // 等待释放
    }
  }
  
  // 检查回放按钮
  if (digitalRead(PLAYBACK_BUTTON) == LOW) {
    delay(50);
    if (digitalRead(PLAYBACK_BUTTON) == LOW) {
      playbackMode = true;
      while (digitalRead(PLAYBACK_BUTTON) == LOW);
    }
  }
  
  // 执行模式
  if (playbackMode) {
    playbackSequence();
  } else if (radio.available()) {
    radio.read(&receivedData, sizeof(Signal));
    lastReceiveTime = millis();
    controlServos();
   
    // 录制模式:每隔一段时间记录一次
    if (recordMode) {
      static unsigned long lastRecordTime = 0;
      if (millis() - lastRecordTime > 1000) {  // 每秒记录一次
        recordStep();
        lastRecordTime = millis();
      }
    }
  }
  
  // ...其余代码...
}
```

### (三) 使用方法

1. **开始录制**: 按下录制按钮,LED提示进入录制模式
2. **操作机械臂**: 通过遥控器控制机械臂,系统每秒自动记录一次位置
3. **停止录制**: 再次按下录制按钮,序列自动保存
4. **回放**: 按下回放按钮,机械臂自动执行录制的动作

## 五、限位保护与速度控制

### (一) 软件限位

为不同舵机设置独立的角度限制:

```cpp
// ===== 舵机限位配置 =====
struct ServoLimits {
  int minAngle;
  int maxAngle;
};

ServoLimits servoLimits[5] = {
  {0, 180},    // 舵机1:底座(全范围)
  {30, 150},   // 舵机2:大臂(避免过度后仰)
  {20, 160},   // 舵机3:小臂(避免碰撞)
  {0, 180},    // 舵机4:手腕(全范围)
  {30, 150}    // 舵机5:夹爪(有效范围)
};

// ===== 限位保护函数 =====
int applyLimits(int angle, int servoIndex) {
  return constrain(angle,
                   servoLimits[servoIndex].minAngle,
                   servoLimits[servoIndex].maxAngle);
}

// ===== 在controlServos中应用 =====
void controlServos() {
  targetAngle[0] = applyLimits(mapToServoAngle(receivedData.ele), 0);
  targetAngle[1] = applyLimits(mapToServoAngle(receivedData.ail), 1);
  targetAngle[2] = applyLimits(mapToServoAngle(receivedData.thr), 2);
  targetAngle[3] = applyLimits(mapToServoAngle(receivedData.rud), 3);
  targetAngle[4] = applyLimits(mapAuxToServo(receivedData.aux1), 4);
  
  // ...其余平滑运动代码...
}
```

### (二) 速度分级控制

通过按钮切换速度档位:

```cpp
// ===== 速度档位 =====
enum SpeedMode {
  SLOW = 1,    // 慢速(精细操作)
  NORMAL = 2,  // 正常速度
  FAST = 5     // 快速(粗略移动)
};

SpeedMode currentSpeed = NORMAL;
#define SPEED_BUTTON 23  // 速度切换按钮

// ===== 初始化中添加 =====
pinMode(SPEED_BUTTON, INPUT_PULLUP);

// ===== 主循环中添加 =====
if (digitalRead(SPEED_BUTTON) == LOW) {
  delay(50);
  if (digitalRead(SPEED_BUTTON) == LOW) {
    // 循环切换速度
    if (currentSpeed == SLOW) currentSpeed = NORMAL;
    else if (currentSpeed == NORMAL) currentSpeed = FAST;
    else currentSpeed = SLOW;
   
    Serial.print("速度切换到:");
    Serial.println(currentSpeed);
   
    while (digitalRead(SPEED_BUTTON) == LOW);
  }
}

// ===== 在平滑运动中使用 =====
int effectiveStep = smoothStep * currentSpeed;
if (currentAngle < targetAngle) {
  currentAngle += min(effectiveStep, targetAngle - currentAngle);
}
```

## 六、系统优化技巧

### (一) 电源优化

**问题**: 5个舵机同时转动时电流可能超过3A,导致电压跌落。

**解决方案**:
1. **大容量电容**: 在电源端并联1000uF-2200uF电解电容
2. **分时驱动**: 避免所有舵机同时大角度转动

```cpp
// 分时驱动示例
void controlServosStaggered() {
  static int activeServo = 0;
  
  // 每次只更新一个舵机
  if (currentAngle[activeServo] != targetAngle[activeServo]) {
    if (currentAngle[activeServo] < targetAngle[activeServo]) {
      currentAngle[activeServo]++;
    } else {
      currentAngle[activeServo]--;
    }
   
    switch(activeServo) {
      case 0: servo1.write(currentAngle[0]); break;
      case 1: servo2.write(currentAngle[1]); break;
      case 2: servo3.write(currentAngle[2]); break;
      case 3: servo4.write(currentAngle[3]); break;
      case 4: servo5.write(currentAngle[4]); break;
    }
  }
  
  activeServo = (activeServo + 1) % 5;  // 轮询下一个
}
```

### (二) 通信优化

**减少数据包大小**:
```cpp
// 使用8位数据代替int(减少50%数据量)
struct CompactSignal {
  byte servo1;  // 0-180直接映射
  byte servo2;
  byte servo3;
  byte servo4;
  byte servo5;
  byte checksum;  // 校验和
};
```

**添加数据校验**:
```cpp
byte calculateChecksum(CompactSignal* sig) {
  return (sig->servo1 + sig->servo2 + sig->servo3 +
          sig->servo4 + sig->servo5) & 0xFF;
}

bool verifyData(CompactSignal* sig) {
  return sig->checksum == calculateChecksum(sig);
}
```

### (三) 调试技巧

**1. 串口可视化**:
```cpp
void printAnglesBar() {
  Serial.println("=== 舵机角度可视化 ===");
  for (int i = 0; i < 5; i++) {
    Serial.print("舵机"); Serial.print(i+1); Serial.print(" [");
   
    int bars = currentAngle / 10;  // 每10度一个字符
    for (int j = 0; j < bars; j++) Serial.print("=");
    for (int j = bars; j < 18; j++) Serial.print(" ");
   
    Serial.print("] "); Serial.print(currentAngle); Serial.println("°");
  }
  Serial.println();
}
```

**2. LED状态指示**:
```cpp
#define STATUS_LED 25

void updateStatusLED() {
  if (millis() - lastReceiveTime < 1000) {
    digitalWrite(STATUS_LED, HIGH);  // 通信正常:常亮
  } else {
    // 通信中断:闪烁
    digitalWrite(STATUS_LED, (millis() / 500) % 2);
  }
}
```

## 七、完整进阶接收端代码

将以上所有功能整合:

```cpp
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include <ESP32Servo.h>
#include <Preferences.h>

// ===== 引脚定义 =====
#define CE_PIN   9
#define CSN_PIN  10
#define SERVO1_PIN 12
#define SERVO2_PIN 13
#define SERVO3_PIN 20
#define SERVO4_PIN 21
#define SERVO5_PIN 8
#define RECORD_BUTTON 14
#define PLAYBACK_BUTTON 22
#define SPEED_BUTTON 23
#define STATUS_LED 25

// ===== 全局对象 =====
const uint64_t pipeIn = 0x123456789;
RF24 radio(CE_PIN, CSN_PIN);
Servo servo1, servo2, servo3, servo4, servo5;
Preferences preferences;

// ===== 数据结构 =====
struct Signal {
  int ele, ail, thr, rud;
  byte aux1, aux2;
};
Signal receivedData;

// ===== 状态变量 =====
int currentAngle[5] = {90, 90, 90, 90, 90};
int targetAngle[5] = {90, 90, 90, 90, 90};
const int smoothStep = 2;
unsigned long lastReceiveTime = 0;
const unsigned long timeout = 1000;

// 速度控制
enum SpeedMode { SLOW = 1, NORMAL = 2, FAST = 5 };
SpeedMode currentSpeed = NORMAL;

// 录制回放
const int MAX_STEPS = 50;
int recordedAngles[MAX_STEPS][5];
int recordedSteps = 0;
bool recordMode = false;
bool playbackMode = false;

// 限位配置
struct ServoLimits {
  int minAngle, maxAngle;
};
ServoLimits servoLimits[5] = {
  {0, 180}, {30, 150}, {20, 160}, {0, 180}, {30, 150}
};

// ===== 辅助函数 =====
int mapToServoAngle(int value) {
  return map(value, -512, 512, 0, 180);
}

int mapAuxToServo(byte value) {
  return (value == -512) ? 30 : 150;
}

int applyLimits(int angle, int servoIndex) {
  return constrain(angle,
                   servoLimits[servoIndex].minAngle,
                   servoLimits[servoIndex].maxAngle);
}

void controlServos() {
  // 计算目标角度并应用限位
  targetAngle[0] = applyLimits(mapToServoAngle(receivedData.ele), 0);
  targetAngle[1] = applyLimits(mapToServoAngle(receivedData.ail), 1);
  targetAngle[2] = applyLimits(mapToServoAngle(receivedData.thr), 2);
  targetAngle[3] = applyLimits(mapToServoAngle(receivedData.rud), 3);
  targetAngle[4] = applyLimits(mapAuxToServo(receivedData.aux1), 4);
  
  // 平滑插值(根据速度档位)
  int effectiveStep = smoothStep * currentSpeed;
  for (int i = 0; i < 5; i++) {
    if (currentAngle < targetAngle) {
      currentAngle += min(effectiveStep, targetAngle - currentAngle);
    } else if (currentAngle > targetAngle) {
      currentAngle -= min(effectiveStep, currentAngle - targetAngle);
    }
  }
  
  // 写入舵机
  servo1.write(currentAngle[0]);
  servo2.write(currentAngle[1]);
  servo3.write(currentAngle[2]);
  servo4.write(currentAngle[3]);
  servo5.write(currentAngle[4]);
}

void recordStep() {
  if (recordedSteps < MAX_STEPS) {
    for (int i = 0; i < 5; i++) {
      recordedAngles[recordedSteps] = currentAngle;
    }
    recordedSteps++;
    Serial.print("录制步骤 "); Serial.print(recordedSteps); Serial.println("/50");
  }
}

void playbackSequence() {
  Serial.println("开始回放...");
  for (int step = 0; step < recordedSteps; step++) {
    for (int i = 0; i < 5; i++) {
      targetAngle = recordedAngles[step];
    }
   
    bool arrived = false;
    while (!arrived) {
      arrived = true;
      for (int i = 0; i < 5; i++) {
        if (currentAngle != targetAngle) {
          arrived = false;
          if (currentAngle < targetAngle) currentAngle++;
          else currentAngle--;
        }
      }
      servo1.write(currentAngle[0]);
      servo2.write(currentAngle[1]);
      servo3.write(currentAngle[2]);
      servo4.write(currentAngle[3]);
      servo5.write(currentAngle[4]);
      delay(20);
    }
    delay(500);
  }
  Serial.println("回放完成!");
  playbackMode = false;
}

void updateStatusLED() {
  if (millis() - lastReceiveTime < 1000) {
    digitalWrite(STATUS_LED, HIGH);
  } else {
    digitalWrite(STATUS_LED, (millis() / 500) % 2);
  }
}

// ===== 初始化 =====
void setup() {
  Serial.begin(115200);
  Serial.println("高级机械臂系统初始化...");

  if (!radio.begin()) {
    Serial.println("NRF24L01初始化失败!");
    while (1);
  }
  radio.openReadingPipe(1, pipeIn);
  radio.startListening();
  radio.setDataRate(RF24_250KBPS);
  radio.setPALevel(RF24_PA_HIGH);
  radio.setAutoAck(true);

  servo1.attach(SERVO1_PIN, 500, 2500);
  servo2.attach(SERVO2_PIN, 500, 2500);
  servo3.attach(SERVO3_PIN, 500, 2500);
  servo4.attach(SERVO4_PIN, 500, 2500);
  servo5.attach(SERVO5_PIN, 500, 2500);
  
  for (int i = 0; i < 5; i++) {
    switch(i) {
      case 0: servo1.write(90); break;
      case 1: servo2.write(90); break;
      case 2: servo3.write(90); break;
      case 3: servo4.write(90); break;
      case 4: servo5.write(90); break;
    }
  }

  pinMode(RECORD_BUTTON, INPUT_PULLUP);
  pinMode(PLAYBACK_BUTTON, INPUT_PULLUP);
  pinMode(SPEED_BUTTON, INPUT_PULLUP);
  pinMode(STATUS_LED, OUTPUT);

  preferences.begin("robot", false);

  Serial.println("系统就绪!");
  delay(1000);
}

// ===== 主循环 =====
void loop() {
  // 按钮检测
  static unsigned long lastButtonCheck = 0;
  if (millis() - lastButtonCheck > 100) {
    // 录制按钮
    if (digitalRead(RECORD_BUTTON) == LOW) {
      delay(50);
      if (digitalRead(RECORD_BUTTON) == LOW) {
        recordMode = !recordMode;
        recordedSteps = recordMode ? 0 : recordedSteps;
        Serial.println(recordMode ? "开始录制!" : "停止录制!");
        while (digitalRead(RECORD_BUTTON) == LOW);
      }
    }
   
    // 回放按钮
    if (digitalRead(PLAYBACK_BUTTON) == LOW) {
      delay(50);
      if (digitalRead(PLAYBACK_BUTTON) == LOW) {
        playbackMode = true;
        while (digitalRead(PLAYBACK_BUTTON) == LOW);
      }
    }
   
    // 速度按钮
    if (digitalRead(SPEED_BUTTON) == LOW) {
      delay(50);
      if (digitalRead(SPEED_BUTTON) == LOW) {
        currentSpeed = (currentSpeed == SLOW) ? NORMAL :
                       (currentSpeed == NORMAL) ? FAST : SLOW;
        Serial.print("速度:"); Serial.println(currentSpeed);
        while (digitalRead(SPEED_BUTTON) == LOW);
      }
    }
   
    lastButtonCheck = millis();
  }

  // 回放模式
  if (playbackMode) {
    playbackSequence();
    return;
  }

  // 接收数据
  if (radio.available()) {
    radio.read(&receivedData, sizeof(Signal));
    lastReceiveTime = millis();
    controlServos();
   
    // 录制模式
    if (recordMode) {
      static unsigned long lastRecordTime = 0;
      if (millis() - lastRecordTime > 1000) {
        recordStep();
        lastRecordTime = millis();
      }
    }
  } else {
    // 超时保护
    if (millis() - lastReceiveTime > timeout) {
      for (int i = 0; i < 5; i++) targetAngle = 90;
      targetAngle[4] = 30;  // 夹爪打开
    }
  }

  updateStatusLED();
  delay(20);
}
```

## 八、实战应用案例

### (一) 物品抓取流程

```cpp
void pickAndPlace() {
  // 1. 初始位置
  moveToPosition(90, 90, 90, 90, 30);  // 夹爪打开
  delay(1000);
  
  // 2. 移动到物品上方
  moveToPosition(45, 60, 120, 90, 30);
  delay(1000);
  
  // 3. 下降
  moveToPosition(45, 80, 140, 90, 30);
  delay(1000);
  
  // 4. 夹取
  moveToPosition(45, 80, 140, 90, 150);  // 夹爪闭合
  delay(1000);
  
  // 5. 抬升
  moveToPosition(45, 60, 120, 90, 150);
  delay(1000);
  
  // 6. 旋转到目标位置
  moveToPosition(135, 60, 120, 90, 150);
  delay(1000);
  
  // 7. 释放
  moveToPosition(135, 60, 120, 90, 30);
  delay(1000);
  
  // 8. 复位
  moveToPosition(90, 90, 90, 90, 30);
}

void moveToPosition(int a1, int a2, int a3, int a4, int a5) {
  targetAngle[0] = a1;
  targetAngle[1] = a2;
  targetAngle[2] = a3;
  targetAngle[3] = a4;
  targetAngle[4] = a5;
  
  // 等待到位
  while (currentAngle[0] != targetAngle[0] ||
         currentAngle[1] != targetAngle[1] ||
         currentAngle[2] != targetAngle[2] ||
         currentAngle[3] != targetAngle[3] ||
         currentAngle[4] != targetAngle[4]) {
    controlServos();
    delay(20);
  }
}
```

## 九、视频展示

[机械臂展示视频]()