# 真实机器人部署教程

## 1. 技术选择

本项目选择 ESP-IDF，而不是 Arduino。固件当前已经迁移到 ESP-IDF 6.0.1，并通过 `idf_component.yml` 约束为 `>=6.0.1,<6.1.0`。

原因是 ESP32S3 需要同时处理 WiFi UDP、TWAI/CAN、I2C 传感器、命令超时保护和多任务控制循环。ESP-IDF 对 FreeRTOS、WiFi、TWAI 和 I2C 的控制更直接，后续调试真实机器人会更稳。

当前闭环是：

```text
本机 PPO 模型
  -> UDP 关节命令
  -> ESP32S3
  -> TWAI/CAN
  -> RS00 关节电机
  -> CAN 反馈 + IMU/地磁遥测
  -> UDP 回本机
```

## 2. 硬件连接

你的 ESP32S3 控制板已经集成 TJA1050T CAN 总线收发器，因此控制板可以直接通过 CANH/CANL 与 RS00 关节电机总线通信。

当前默认引脚：

- ESP32S3 `GPIO5` -> TJA1050T TXD
- ESP32S3 `GPIO4` -> TJA1050T RXD
- TJA1050T CANH/CANL -> RS00 CANH/CANL 总线
- 总线两端各 120Ω 终端电阻
- ESP32S3 GND、RS00 电源 GND 共地
- RS00 电机按手册供电，额定 48V，允许 24V-60V

无屏状态提示硬件：

- `GPIO42` -> 无源蜂鸣器
- `GPIO45` -> WS2812 三色 LED

I2C 默认连接：

- `GPIO8` -> SDA
- `GPIO9` -> SCL
- QMI8658A 默认地址 `0x6B`
- QMC6309 默认地址 `0x7C`

如果你的板子引脚不同，修改：

```text
guguji_real_robot/firmware/esp32s3_espidf/main/config.h
```

## 3. 电机准备

RS00 手册说明电机默认 CAN 波特率为 1Mbps。本固件也按 1Mbps 初始化 TWAI。

建议每个电机设置唯一 CAN ID：

```text
left_hip_pitch_joint      -> 1
left_knee_pitch_joint     -> 2
left_ankle_pitch_joint    -> 3
left_ankle_joint          -> 4
right_hip_pitch_joint     -> 5
right_knee_pitch_joint    -> 6
right_ankle_pitch_joint   -> 7
right_ankle_joint         -> 8
```

对应配置在：

```text
guguji_real_robot/firmware/esp32s3_espidf/main/robot_config.h
```

如果你的电机还在私有协议或 CANopen 协议，需要先用官方上位机或 CAN 工具切到 MIT 协议，并重新上电生效。

## 4. 编译和刷写 ESP32S3

安装并在 VSCode 中选择 ESP-IDF 6.0.1 后，首次从旧 IDF 切换时先清理旧配置：

```bash
cd /home/corvin/Project/guguji_simulation/guguji_real_robot/firmware/esp32s3_espidf
rm -rf build sdkconfig sdkconfig.old managed_components dependencies.lock
idf.py set-target esp32s3
idf.py build
idf.py -p /dev/ttyUSB0 flash monitor
```

固件默认会启动 WiFi 热点：

```text
SSID: guguji-robot
Password: guguji1234
ESP32S3 IP: 192.168.4.1
```

电脑连接这个热点后，主机端配置默认即可使用。

## 5. 无屏状态提示

固件会用蜂鸣器和 WS2812 提示当前状态：

```text
蓝灯慢闪             等待本机主机连接
青色慢闪             已连接但电机失能，或处于安全保持
绿灯常亮             已 ARM，正在向电机下发控制
黄灯双闪 + 双短鸣     主机命令超时，电机已停止
红灯快闪 + 低频鸣叫   急停或 RS00 电机故障
紫灯慢闪 + 高低短鸣   传感器离线或电机反馈超时等警告
```

这些提示只表示固件判断到的状态，真正落地测试时仍然要保留物理急停和电源限流。

## 6. 本机部署模型

先安装依赖：

```bash
cd /home/corvin/Project/guguji_simulation
python3 -m venv .venv-real
source .venv-real/bin/activate
pip install -r guguji_rl/requirements.txt
pip install -r guguji_real_robot/host/requirements.txt
```

不使能电机，仅检查遥测和模型输出：

```bash
python guguji_real_robot/host/scripts/run_real_policy.py \
  --model guguji_rl/outputs/<你的训练目录>/final_model.zip \
  --deterministic
```

主机端会周期性打印 ESP32S3 遥测，例如：

```text
[ESP32S3] state=DISARMED seq=12 uptime=8.4s cmd_age=16ms faults=ok rpy=(0.3,-1.1,12.6)deg temp_max=32.5C joint_vel_mean=0.018rad/s
```

如果出现问题，会把故障拆开显示：

```text
faults=feedback_timeout=left_knee_pitch_joint; imu_offline
```

确认机器人悬空、急停开关可用、电源限流合理后，再加 `--arm`：

```bash
python guguji_real_robot/host/scripts/run_real_policy.py \
  --model guguji_rl/outputs/<你的训练目录>/final_model.zip \
  --deterministic \
  --clear-faults \
  --arm
```

可用 `--log-interval 0.5` 临时提高日志刷新率。默认值在：

```text
guguji_real_robot/host/configs/guguji_real_robot.yaml
```

## 7. 方向和零点标定

第一次不要让机器人落地，务必悬空测试。

标定步骤：

1. 使用官方工具或本固件的零偏配置，让每个关节机械零位和仿真名义站姿对应。
2. 单独给每个关节一个很小的正向目标角，观察真实转动方向。
3. 如果方向相反，在 `robot_config.h` 中把该关节 `direction` 从 `1.0f` 改成 `-1.0f`。
4. 如果零位有偏差，修改 `zero_offset_rad`。
5. 每次修改后重新编译刷写。

映射关系是：

```text
motor_angle = joint_angle * direction + zero_offset_rad
```

## 8. 安全参数

主机侧部署安全参数在：

```text
guguji_real_robot/host/configs/guguji_real_robot.yaml
```

重点参数：

- `max_joint_step_rad`：每个控制周期允许目标角变化的最大值。
- `max_roll_rad` / `max_pitch_rad`：超过后主机请求急停。
- `kp` / `kd`：RS00 MIT 运控模式的关节增益。

固件侧安全参数在 `config.h`：

- `GUGUJI_COMMAND_TIMEOUT_MS`：超过这个时间没有收到主机命令就停止电机。
- `GUGUJI_CONTROL_PERIOD_MS`：CAN 控制周期，默认 20ms，与当前训练 `control_dt=0.05s` 相比更快，主机会按训练周期发新目标，固件保持最近目标。
- `GUGUJI_MOTOR_FEEDBACK_TIMEOUT_MS`：超过这个时间没有收到某个 RS00 的反馈，就在遥测中标记该关节反馈超时。

## 9. 当前限制

现有训练观测包含：

- 关节位置
- 关节速度
- 上一时刻动作
- base height
- roll / pitch
- forward velocity
- target forward velocity

真实机器人当前只有关节反馈和 IMU，没有外部定位或测高，所以 `base height` 和 `forward velocity` 先由主机配置里的常量补齐。这能用于初步看策略摆腿效果，但要让行走策略更接近仿真，后续建议接入以下任一方案：

- 动捕或视觉里程计，用于真实 forward velocity。
- 足底接触或测距传感器，用于 base height。
- 重新训练一个不依赖外部速度和高度的部署策略。

## 10. 推荐调试顺序

1. 只刷固件，查看串口日志确认 WiFi、QMI8658A、QMC6309、蜂鸣器、WS2812 初始化。
2. 不接电机，只运行主机脚本，确认 UDP 遥测正常。
3. 只接 1 个 RS00，确认 CAN ID、使能、停止、反馈解析。
4. 悬空接 8 个电机，逐个校准方向和零位。
5. 用低 Kp/Kd、低 `max_joint_step_rad` 运行 `--arm`。
6. 确认不会突然打限位后，再逐步提高增益和落地测试。
