教你把 yaw / pitch / roll 三个角度实时映射到 3 只舵机（servo）

---

1️⃣ 接线速查

舵机信号线	ESP32 引脚	备注
舵机1 yaw	GPIO 25	随便改
舵机2 pitch	GPIO 26	随便改
舵机3 roll	GPIO 27	随便改
舵机 V+	5 V（外供）	不要直接用 ESP32 3.3 V
舵机 GND	GND	必须与 ESP32 共地

⚠️ 电流大时给舵机单独供电，ESP32 仅给信号线！

---

2️⃣ 代码改动（最小补丁）

① 头文件 + 舵机库

在最前面加入：

#include <ESP32Servo.h>   // 比官方 Servo 库更适合 ESP32

② 定义舵机对象与引脚

在全局区（setup 之前）加：

Servo servoYaw, servoPitch, servoRoll;

const uint8_t PIN_YAW   = 25;
const uint8_t PIN_PITCH = 26;
const uint8_t PIN_ROLL  = 27;

③ setup() 里初始化舵机

在 setup() 里（Serial.println("ESP32 + MPU6050 DMP 初始化完成！"); 之后）加：

servoYaw.attach(PIN_YAW);
servoPitch.attach(PIN_PITCH);
servoRoll.attach(PIN_ROLL);

servoYaw.write(90);      // 舵机全部回中位
servoPitch.write(90);
servoRoll.write(90);

④ loop() 里把角度写给舵机

在 loop() 里 打印完 ypr 之后 加：

// 把弧度转成角度，再映射到 0~180°
int yawDeg   = constrain((int)(ypr[0] * 180 / M_PI) + 90, 0, 180);
int pitchDeg = constrain((int)(ypr[1] * 180 / M_PI) + 90, 0, 180);
int rollDeg  = constrain((int)(ypr[2] * 180 / M_PI) + 90, 0, 180);

servoYaw.write(yawDeg);
servoPitch.write(pitchDeg);
servoRoll.write(rollDeg);

---

3️⃣ 编译上传

arduino-cli compile --fqbn esp32:esp32:esp32 esp32_mpu6050_dmp
arduino-cli upload   -p /dev/ttyUSB0 --fqbn esp32:esp32:esp32 esp32_mpu6050_dmp

---

4️⃣ 效果

• 把 ESP32 + MPU6050 一起绕 X / Y / Z 三个轴转动；
• 三只舵机会实时跟随 yaw / pitch / roll 角度变化；
• 默认 0° 时舵机在中位 90°，±90° 对应 0°~180°。

---

5️⃣ 进阶技巧（可选）

需求	做法
平滑滤波	对 ypr[i] 做 一阶低通 或 移动平均
扩大角度范围	把映射范围改成 0~270°（舵机支持的话）
无线遥控	用 ESP-NOW / WiFi UDP 把 ypr 发到另一块板子
多轴云台	改用 PWM 驱动板（PCA9685），可带十几只舵机

---

执行效果:

灯效讲解:

• 红灯: 初始化开始
• 绿灯: 初始化完成
• 蓝灯: 进入loop循环