[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_PIN10
// 摇杆ADC引脚
#define JOYSTICK1_X3 // 左摇杆X(底座)
#define JOYSTICK1_Y4 // 左摇杆Y(大臂)
#define JOYSTICK2_X5 // 右摇杆X(小臂)
#define JOYSTICK2_Y11// 右摇杆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[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 = 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;
}
// 平滑移动到目标位置
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.minAngle,
servoLimits.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 != targetAngle) {
if (currentAngle < targetAngle) {
currentAngle++;
} else {
currentAngle--;
}
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_PIN10
#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[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.minAngle,
servoLimits.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 = 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;
}
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);
}
}
```
## 九、视频展示
[机械臂展示视频]()
页:
[1]