第 12 章：完整机器人示例
📂 示例代码：12_full_example
12.1 知识要点
- 多外设协同初始化的顺序与依赖关系
- 三任务双核架构的完整实现
- ADC 电压监测与查找表线性插值
- serial_tx 队列与专用 TX 任务（解耦发送与控制逻辑）
- 里程计积分（速度 × dt × 航向角）
- SBUS 遥控与串口命令的优先级仲裁
- 速度模式通道（ch[7]）：限速 15% vs 全速
- RC 超时（200 ms）与串口控制超时（500 ms）安全机制
- 电池使能 GPIO 初始化（GPIO16 置 LOW）
- IMU 加热器完整集成：warm/stable/ready 三里程碑，蜂鸣器+LED 反馈
12.2 课程内容
本章将前 10 章的所有外设整合到一个完整的机器人控制程序中，实现与 OSRCORE 固件相同的三任务架构。系统支持两种控制模式：SBUS 遥控模式（CH7 < 1500）和串口命令模式（CH7 ≥ 1500），遥控模式优先级更高。速度模式通道（CH8，index 7）控制限速：< 1500 时限速 15%，≥ 1500 时全速。RC 超时 200 ms、串口控制超时 500 ms，两者均触发自动停车。
12.3 基础学习
三任务架构
Core 1 task_imu P5 1ms QMI8658 → Madgwick AHRS → 里程计积分 → 更新 g_state
Core 1 task_control P4 20ms 编码器 → LPF → PID → ESC/舵机输出 → ADC 电压监测
Core 0 task_comm P3 1ms SBUS 解码 / USB CDC 命令 / serial_tx 队列发送另有一个低优先级 serial_tx 任务（P1，Core 0）专门负责从队列取出字符串并写入 USB CDC，将发送操作与控制逻辑完全解耦。
控制模式仲裁
SBUS CH7（index 6）< 1500 → 遥控模式：SBUS CH3 → 速度，CH1 → 转向
SBUS CH7（index 6）≥ 1500 → 串口模式：USB CDC 命令控制
SBUS CH8（index 7）< 1500 → 速度限制 15%（低速安全模式）
SBUS CH8（index 7）≥ 1500 → 全速模式
Failsafe / lost_frame 激活 → 强制停止RC 超时与串口超时安全
- RC 超时（200 ms）：遥控模式下，若 200 ms 内未收到有效 SBUS 帧，自动触发 failsafe，速度和转向复位到中立位。
- 串口超时（500 ms）：串口控制模式下，若 500 ms 内未收到新命令，自动将速度和转向复位到中立位。
ADC 电压监测
电池电压通过 GPIO4（ADC1 CH3）分压检测，使用 9 点查找表做线性插值，将 ADC 原始值映射为实际电压（V）。低于 11.3 V 时通过 serial_tx 队列发出警告。
c
static const int ADC_PTS[] = {1017,1135,1197,1257,1379,1450,1495,1555,1615};
static const float VOLT_PTS[] = {9.0f,10.0f,10.5f,11.0f,12.0f,12.6f,13.0f,13.5f,14.0f};里程计积分
task_imu 在每次 AHRS 更新后，用当前航向角（从四元数提取 yaw）和 task_control 写入的 filtered_speed 积分里程计位置：
c
g_state.odom_pos[0] += spd * dt * cosf(g_state.odom_yaw);
g_state.odom_pos[1] += spd * dt * sinf(g_state.odom_yaw);初始化顺序
1. USB CDC 控制台
2. serial_tx 队列 + TX 任务
3. 电池使能（GPIO16 置 LOW，osrcore 硬件约定）
4. NVS Flash 初始化
5. I2C + QMI8658（含启动偏置校准）
6. PCNT 编码器
7. LEDC PWM（ESC + 舵机 + 蜂鸣器）
8. UART SBUS
9. ADC 电压检测
10. Madgwick + PID 初始化
11. ESC 解锁（中立位 2 秒）
12. 创建三个任务（+ serial_tx 任务已在步骤 2 创建）12.4 程序学习
全局状态结构体（portMUX 保护）：
c
typedef struct {
float target_speed; // m/s
float filtered_speed; // m/s
float kp, ki, kd;
float odom_pos[2]; // m, x/y
float odom_yaw; // rad
float quat[4]; // w x y z
float accel[3]; // m/s²
float gyro[3]; // rad/s
float voltage; // V
float temp; // °C
int steering_pulse; // µs
int rc_ch[10];
bool remote_active;
bool failsafe;
bool speed_full_mode; // CH8 ≥ 1500 → 全速
bool serial_control_active;
unsigned long last_serial_cmd_ms;
} app_state_t;
static app_state_t g_state;
static portMUX_TYPE g_mux = portMUX_INITIALIZER_UNLOCKED;serial_tx 队列（专用 TX 任务）：
c
#define SERIAL_TX_LINE_MAX 192
#define SERIAL_TX_QUEUE_LEN 32
typedef struct { char line[SERIAL_TX_LINE_MAX]; } serial_tx_msg_t;
static QueueHandle_t s_serial_tx_queue;
static void serial_tx_task_fn(void *arg)
{
serial_tx_msg_t msg;
while (1) {
if (xQueueReceive(s_serial_tx_queue, &msg, portMAX_DELAY) == pdTRUE) {
fputs(msg.line, stdout);
fflush(stdout);
}
}
}
/* 任何任务调用此函数发送，不阻塞调用方 */
static void serial_tx_printf(const char *fmt, ...)
{
serial_tx_msg_t msg;
va_list args;
va_start(args, fmt);
vsnprintf(msg.line, SERIAL_TX_LINE_MAX, fmt, args);
va_end(args);
xQueueSend(s_serial_tx_queue, &msg, 0);
}task_comm 中的模式仲裁（核心逻辑）：
c
// SBUS 解码后的模式判断
if (rc_ch[CONTROL_MODE_CH] < 1500) {
/* 遥控模式 */
bool full = (rc_ch[SPEED_MODE_CH] >= 1500); // CH8 控制限速
float spd = /* 从 CH3 映射 */;
if (!full) spd *= 0.15f; // 限速 15%
portENTER_CRITICAL(&g_mux);
g_state.remote_active = true;
g_state.speed_full_mode = full;
g_state.target_speed = spd;
g_state.steering_pulse = /* 从 CH1 映射 */;
portEXIT_CRITICAL(&g_mux);
}
// RC 超时（200ms）
if (rc_initialized && g_state.remote_active) {
if (now_ms - last_rc_ok_ms >= RC_TIMEOUT_MS) { // RC_TIMEOUT_MS = 200
portENTER_CRITICAL(&g_mux);
g_state.failsafe = true;
g_state.remote_active = false;
g_state.target_speed = 0.0f;
g_state.steering_pulse = STEERING_CENTER;
portEXIT_CRITICAL(&g_mux);
serial_tx_printf("WARN: RC signal lost\n");
}
}
// 串口超时（500ms）
if (g_state.serial_control_active && !g_state.remote_active) {
if (g_state.last_serial_cmd_ms > 0 &&
now_ms - g_state.last_serial_cmd_ms > SERIAL_TIMEOUT_MS) { // 500ms
portENTER_CRITICAL(&g_mux);
g_state.target_speed = 0.0f;
g_state.steering_pulse = STEERING_CENTER;
portEXIT_CRITICAL(&g_mux);
}
}app_main 初始化序列：
c
void app_main(void)
{
// USB CDC
usb_serial_jtag_driver_install(&usb_cfg);
// serial_tx 队列 + TX 任务
serial_tx_init();
// 电池使能：GPIO16 置 LOW（osrcore 硬件约定）
gpio_set_direction(IO_CTRL_BAT, GPIO_MODE_OUTPUT);
gpio_set_level(IO_CTRL_BAT, 0);
ESP_ERROR_CHECK(nvs_flash_init());
init_i2c_imu(); // 含 qmi8658_calibrate_bias()
init_encoder();
init_pwm();
init_sbus();
init_adc(); // ADC1 CH3，查找表电压监测
madgwick_init(&g_ahrs, 0.1f);
pid_init(&g_pid, PID_KP_DEFAULT, PID_KI_DEFAULT, PID_KD_DEFAULT,
PID_MAX_INTEGRAL, PID_DEADBAND);
// ESC 解锁
set_throttle(THROTTLE_NEUTRAL);
set_steering(STEERING_CENTER);
vTaskDelay(pdMS_TO_TICKS(2000));
xTaskCreatePinnedToCore(task_imu, "imu", 4096, NULL, 5, NULL, 1);
xTaskCreatePinnedToCore(task_control, "ctrl", 4096, NULL, 4, NULL, 1);
xTaskCreatePinnedToCore(task_comm, "comm", 8192, NULL, 3, NULL, 0);
// serial_tx 任务（P1）已在 serial_tx_init() 中创建
}IMU 加热器集成
完整示例中加热器有三个里程碑，对应不同的系统状态：
| 里程碑 | 温度 | 蜂鸣器 | LED | 含义 |
|---|---|---|---|---|
| warm | 38°C | 880Hz 200ms | 绿 | 可以使用，启动控制任务 |
| stable | 54°C | 1200Hz+600Hz | 红 | 热稳态，偏置最优 |
| ready | 56°C 斜率稳定 | — | — | bang-bang 维持 |
启动序列：task_imu 先启动（喂温度）→ 等 warm（38°C）→ 启动 task_control 和 task_comm。
蜂鸣器和 LED 颜色切换由 imu_heater.c 内部驱动，与任务架构解耦。
12.5 课程总结
本章完成了 OSRCORE 完整机器人控制程序的实现，将 WS2812B、蜂鸣器、ESC、舵机、编码器、IMU、SBUS、NVS 和 FreeRTOS 多任务架构全部整合在一起。新增特性包括：ADC 查找表电压监测、serial_tx 队列解耦发送、里程计积分、速度模式通道（CH8）、RC 超时 200 ms 安全机制，以及电池使能 GPIO16 置 LOW 的硬件约定。三任务双核架构保证了 IMU 采样和 PID 控制的实时性，SBUS/串口双模式控制提供了灵活的操控方式。
至此，OSRCORE 开发板教程全部完成。建议按以下路径深入学习：
- 调整 PID 参数，观察速度响应曲线
- 在 task_imu 中加入姿态反馈，实现倾斜补偿
- 扩展 SBUS 通道映射，支持更多遥控功能
- 使用 ESP-IDF 的 WiFi/BLE 组件，添加无线调试界面