用定位数据做什么？ —— MoveTo 函数What to Do with Tracking Data — The MoveTo Function

知道了 (x, y, θ)，最直接的应用是：让机器人自动走到指定坐标。Knowing (x, y, θ), the most direct application is: making the robot automatically drive to a target coordinate.

6.1 思路6.1 The Approach

把底盘功率拆成两部分：Split the drivetrain power into two parts:

• 直行功率：由"离目标还有多远"决定（PID 控制）Drive power: determined by "how far from target" (PID control)
• 转弯功率：由"车头方向和目标方向差多少"决定（PID 控制）Turn power: determined by "how much the heading differs from target direction" (PID control)

                T (目标点)
               ╱|
              ╱ |
            ╱   | Δy
          ╱     |
     O ─────────┘
     (当前位置) Δx

     α = atan2(Δx, Δy)     ← 目标方向
     误差角度 = α - 当前角度  ← 车头偏了多少
     距离 = √(Δx² + Δy²)   ← 还有多远

6.2 简化代码6.2 Simplified Code

// 角度归一化到 [-180, 180]
double normalizeAngle(double angle) {
    while (angle > 180) angle -= 360;
    while (angle < -180) angle += 360;
    return angle;
}

void moveTo(double target_x, double target_y, int timeout_ms) {
    timer t;

    while (t.time(msec) < timeout_ms) {
        // 当前位置（从定位系统读取）
        double cur_x = pos_x;
        double cur_y = pos_y;
        double cur_angle = IMU.rotation(deg);

        // 计算误差
        double err_x = target_x - cur_x;
        double err_y = target_y - cur_y;
        double distance = sqrt(err_x * err_x + err_y * err_y);

        // 目标方向角度（度）
        double target_angle = atan2(err_x, err_y) / DEG_TO_RAD;

        // 角度误差，归一化防止转大圈
        double angle_error = normalizeAngle(target_angle - cur_angle);

        // 投影距离（沿车头方向还要走多远）
        double forward_error = distance * cos(angle_error * DEG_TO_RAD);

        // PID 计算功率（这里简化为 P 控制）
        double power_linear = forward_error * 2.0;   // kP = 2.0
        double power_turn = angle_error * 1.5;       // kP = 1.5

        // 限幅
        if (fabs(power_linear) > 100)
            power_linear = (power_linear > 0) ? 100 : -100;
        if (fabs(power_turn) > 100)
            power_turn = (power_turn > 0) ? 100 : -100;

        // 快到了就停止转弯（防止结尾抖动）
        if (distance < 10) power_turn = 0;

        // 分配给左右轮
        double left = power_linear + power_turn;
        double right = power_linear - power_turn;

        // 等比缩放，防止超过 100
        double max_power = fmax(fabs(left), fabs(right));
        if (max_power > 100) {
            left = left / max_power * 100;
            right = right / max_power * 100;
        }

        moveLeft(left);
        moveRight(right);

        // 到达退出
        if (distance < 3) break;

        task::delay(10);
    }

    // 停下
    moveLeft(0);
    moveRight(0);
}

为什么需要 normalizeAngle？ 假如机器人转了 350°，陀螺仪读数 350，目标方向 10°。不归一化的话 10 - 350 = -340，机器人会傻傻地转 340 度大圈。归一化后变成 +20，只转 20 度小圈。Why do we need normalizeAngle? Say the robot has turned 350°, gyro reads 350, target direction is 10°. Without normalization: 10 - 350 = -340, the robot would stupidly turn 340° the long way around. After normalization it becomes +20 — just a 20° turn.

6.3 使用方式6.3 How to Use It

void autonomous() {
    // 机器人从 (0, 0) 出发
    moveTo(60, 0, 3000);     // 往右走 60cm，3 秒超时
    moveTo(60, 60, 3000);    // 再往前走 60cm
    moveTo(0, 0, 5000);      // 回到起点
}

对比以前用 TimerForward(127, 1000) 凭时间走路 —— 现在是告诉机器人"去这个坐标"，它自己算怎么走。精确度和灵活性完全不是一个量级。Compare this to the old TimerForward(127, 1000) approach of driving by time — now we tell the robot "go to this coordinate" and it figures out how to get there. The accuracy and flexibility are on a completely different level.

6.4 实战细节6.4 Practical Details

上面的 moveTo 是最简版，实际使用中还需要注意几个问题：The moveTo above is the simplest version. In practice, there are several issues to watch for:

功率比例问题：直行功率 400 + 转弯功率 300，如果直接加起来就是 700，但电机最大 100。不做等比缩放的话，(100, 100) 变成直走，转弯完全丢失。代码里的等比缩放就是解决这个问题。Power ratio problem: Drive power 400 + turn power 300 adds up to 700, but the motor max is 100. Without proportional scaling, (100, 100) becomes straight driving with no turning. The proportional scaling in the code solves this.

起步时强制转弯：刚开始时距离很远，直行功率很大，容易把转弯功率盖过去。可以在距离远的时候给转弯功率一个最低值：Force turning at start: At the beginning, the distance is large so drive power is high, easily overpowering turn power. Give turn power a minimum value when far from target:

if (distance > 10 && fabs(power_turn) < 60) {
    power_turn = (power_turn > 0) ? 60 : -60;
}

收尾时停止转弯：快到目标时 Δx 和 Δy 都很小，任何微小变化都会导致目标方向剧烈跳动，机器人会原地左右摇摆。所以接近目标时直接把转弯功率清零。Stop turning at finish: Near the target, Δx and Δy are both tiny — any small change causes the target direction to jump wildly, making the robot wobble left and right. So near the target, set turn power to zero.

功率平滑（Slew Rate）：功率突变会让机器人抖动，限制每次循环最大功率变化：Power smoothing (Slew Rate): Sudden power changes cause the robot to jerk. Limit the maximum power change per loop:

// 每次循环功率变化不超过 10
float slew(float current, float last, float max_change) {
    float change = current - last;
    if (change > max_change) change = max_change;
    if (change < -max_change) change = -max_change;
    return last + change;
}