# 机器人趴下动作实现大纲

## 1. 目标
实现四足机器人从站立状态平滑过渡到趴下状态（降低机身高度至最低）。

## 2. 硬件与运动学基础
*   **腿部结构**：双连杆结构（大腿 L1=250mm, 小腿 L2=227mm）。
*   **电机配置**：每条腿2个电机，共8个电机（CAN1/CAN2 ID 1-4, 5-8）。
*   **坐标系**：基于 `leg.c` 的 `Inverse_Calculation`，输入为 (X, Y)。
    *   根据代码推测，Y 为垂直方向距离（机身到足端），X 为水平方向偏移。
    *   L = sqrt(X^2 + Y^2) 为髋关节到足端的直线距离。
    *   趴下即减小 Y 值，使机身贴近地面。

## 3. 实现方案

### 3.1 定义趴下姿态
*   **目标足端坐标**：
    *   `Target_X = 0`：足端保持在髋关节垂直投影线上（或根据需要微调）。
    *   `Target_Y = H_min`：最小支撑高度。需根据机械结构测量，例如 100mm - 150mm，确保大腿和小腿折叠时不发生机械干涉。
*   **目标关节角度**：
    *   通过 `Inverse_Calculation(0, H_min, ...)` 计算出对应的电机目标角度 `range1` 和 `range2`。

### 3.2 动作流程设计
1.  **初始化/获取状态**：
    *   确认当前机器人处于站立或可控状态。
    *   记录当前的设定高度 `Current_Y`。
2.  **轨迹规划（平滑过渡）**：
    *   **目的**：防止机器人直接“摔”在地上，保护机械结构和电机。
    *   **方法**：使用插值算法（线性插值或 S 曲线插值）。
    *   在时间 `T`（如 2秒）内，将高度指令从 `Current_Y` 逐步减小到 `Target_Y`。
3.  **控制循环执行**：
    *   在循环中（如 1ms 或 5ms 周期）：
        1.  计算当前时刻的目标高度 `h(t)`。
        2.  调用 `Inverse_Calculation(0, h(t), ...)` 解算四条腿的关节角度。
        3.  调用 `Posture()` 函数向 8 个电机发送位置控制指令（Position Mode 或 MIT Mode）。
4.  **到位处理**：
    *   当达到 `Target_Y` 后，机器人应已趴在地上。
    *   **节能模式**：如果机身已由地面完全支撑，可以将电机的 `Kp`, `Kd` 和 `Torque` 设为 0，进入阻尼模式或空闲模式，防止电机持续堵转过热。

## 4. 伪代码示例

```c
// 定义在 User/app/execute_task.c 或类似文件中

void Robot_LieDown(void) {
    RobotGeometry geom = { .L1 = 250.0f, .L2 = 227.0f };
    float current_h = 300.0f; // 假设初始站立高度
    float target_h = 120.0f;  // 趴下时的目标高度（需实测）
    float duration = 2000.0f; // 动作耗时 2000ms
    float dt = 5.0f;          // 控制周期 5ms
    
    float t = 0;
    while (t <= duration) {
        // 1. 计算当前高度 (线性插值)
        float h = current_h + (target_h - current_h) * (t / duration);
        
        // 2. 运动学解算 (假设 X=0)
        float angle1, angle2;
        Inverse_Calculation(0, h, &angle1, &angle2, &geom);
        
        // 3. 发送指令给四条腿 (假设腿的电机ID映射如下)
        // Leg 1 (FL)
        Posture(1, angle1, 0, 4.0f, 1.0f, 0); 
        Posture(2, angle2, 0, 4.0f, 1.0f, 0);
        // Leg 2 (FR)
        Posture(3, angle1, 0, 4.0f, 1.0f, 0); 
        Posture(4, angle2, 0, 4.0f, 1.0f, 0);
        // ... Leg 3 & 4
        
        t += dt;
        osDelay((uint32_t)dt);
    }
    
    // 4. 趴下后处理 (可选：关闭力矩输出)
    // Disable_Motor_Torque(); 
}
```

## 5. 验证与调试
1.  **静态测试**：将机器人架空，运行趴下程序，观察腿部是否缓慢收缩，角度方向是否正确。
2.  **落地测试**：在软垫上测试，观察趴下过程是否平稳，是否有冲击。
3.  **干涉检查**：确认趴到底时，机械结构不会互相碰撞或卡死（特别注意皮带传动部分是否受压或过度弯曲）。