定位轮（Tracking Wheels）

// 定位轮参数
#define WHEEL_DIAMETER_V 5.08   // 竖轮直径（cm），2 英寸全向轮
#define WHEEL_DIAMETER_H 5.08   // 横轮直径（cm）
#define OFFSET_V 4.0            // 竖轮到旋转中心的水平偏移（cm）
#define OFFSET_H 10.0           // 横轮到旋转中心的纵向偏移（cm）

// 角度转弧度
#define DEG_TO_RAD (M_PI / 180.0)

// 极小角度阈值（小于这个值视为直线）
#define ANGLE_THRESHOLD 0.001   // 约 0.06 度

// 极简版：假设不转弯
void updatePosition_simple() {
    double delta_v = (VerticalEncoder.position(deg) - last_vertical)
                     * DEG_TO_RAD * (WHEEL_DIAMETER_V / 2.0);
    double delta_h = (HorizontalEncoder.position(deg) - last_horizontal)
                     * DEG_TO_RAD * (WHEEL_DIAMETER_H / 2.0);
    double angle = IMU.rotation(deg) * DEG_TO_RAD;

    // 直接用当前角度做坐标旋转
    pos_x += delta_v * sin(angle) + delta_h * cos(angle);
    pos_y += delta_v * cos(angle) - delta_h * sin(angle);

    last_vertical = VerticalEncoder.position(deg);
    last_horizontal = HorizontalEncoder.position(deg);
}

void updatePosition() {
    // === 第一步：一次性读取所有传感器（保证数据一致性） ===

    double cur_vertical = VerticalEncoder.position(deg);     // ★ 局部变量
    double cur_horizontal = HorizontalEncoder.position(deg); // ★ 局部变量
    double angle = IMU.rotation(deg) * DEG_TO_RAD;

    // 编码器角度转成轮子走过的距离（弧长）
    double arc_v = (cur_vertical - last_vertical)
                   * DEG_TO_RAD * (WHEEL_DIAMETER_V / 2.0);
    double arc_h = (cur_horizontal - last_horizontal)
                   * DEG_TO_RAD * (WHEEL_DIAMETER_H / 2.0);

    // 陀螺仪角度变化
    double delta_angle = angle - last_angle;

    // === 第二步：算弦长（实际位移） ===

    double chord_v, chord_h;

    if (fabs(delta_angle) < ANGLE_THRESHOLD) {               // ★ 用阈值，不用 == 0
        // 几乎没转弯，弧长 ≈ 弦长
        chord_v = arc_v;
        chord_h = arc_h;
    } else {
        // 有转弯，用圆弧模型
        double radius_v = arc_v / delta_angle + OFFSET_V;    // ★ 补正偏移
        double radius_h = arc_h / delta_angle + OFFSET_H;    // ★ 补正偏移

        chord_v = 2 * radius_v * sin(delta_angle / 2.0);
        chord_h = 2 * radius_h * sin(delta_angle / 2.0);
    }

    // === 第三步：转换到全局坐标 ===

    double avg_angle = last_angle + delta_angle / 2.0;       // ★ 平均角度

    pos_x += chord_v * sin(avg_angle) + chord_h * cos(avg_angle);
    pos_y += chord_v * cos(avg_angle) - chord_h * sin(avg_angle);

    // === 第四步：保存本次值，给下次循环用 ===

    last_vertical = cur_vertical;                             // ★ 用局部变量
    last_horizontal = cur_horizontal;                         // ★ 用局部变量
    last_angle = angle;
}

int trackingTask() {
    while (true) {
        updatePosition();
        task::delay(5);  // 每 5ms 更新一次（一秒 200 次）
    }
    return 0;
}

// VEXcode 写法
task tracking = task(trackingTask);

// PROS 写法（如果你用 PROS）
// pros::Task tracking(trackingTask);

// LemLib 的核心算法（简化版）
localX = 2 * sin(deltaHeading / 2) * (deltaX / deltaHeading + horizontalOffset);
localY = 2 * sin(deltaHeading / 2) * (deltaY / deltaHeading + verticalOffset);

// 转换到全局坐标
odomPose.x += localY * sin(avgHeading) + localX * (-cos(avgHeading));
odomPose.y += localY * cos(avgHeading) + localX * sin(avgHeading);

// 角度归一化到 [-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);
}

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

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

// 每次循环功率变化不超过 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;
}

void updatePosition() {
    double cur_v = VerticalEncoder.position(deg);
    double cur_h = HorizontalEncoder.position(deg);

    double arc_v = (cur_v - last_vertical)
                   * DEG_TO_RAD * (WHEEL_DIAMETER_V / 2.0);
    double arc_h = /* (A) */;

    double angle = IMU.rotation(deg) * DEG_TO_RAD;
    double delta_angle = /* (B) */;

    double chord_v, chord_h;

    if (/* (C) */) {
        chord_v = /* (D) */;
        chord_h = /* (E) */;
    } else {
        double radius_v = /* (F) */;
        double radius_h = /* (G) */;
        chord_v = 2 * radius_v * sin(delta_angle / 2.0);
        chord_h = 2 * radius_h * sin(delta_angle / 2.0);
    }

    double avg_angle = /* (H) */;

    pos_x += /* (I) */;
    pos_y += /* (J) */;

    last_vertical = cur_v;
    last_horizontal = cur_h;
    last_angle = angle;
}

// 编码器角度转距离
double arc_v = (cur_v - last_v) * (WHEEL_DIAMETER_V / 2.0);

// 判断是否转弯
if (delta_angle == 0) {
    chord_v = arc_v;
} else {
    double radius_v = arc_v / delta_angle + OFFSET_V;
    chord_v = 2 * radius_v * sin(delta_angle / 2.0);
}

void updatePosition() {
    double arc_v = (VerticalEncoder.position(deg) - last_vertical) * DEG_TO_RAD * (WHEEL_DIAMETER_V / 2.0);
    double angle = IMU.rotation(deg) * DEG_TO_RAD;
    double delta_angle = angle - last_angle;
    // ... 计算 chord_v, chord_h, 更新 pos_x, pos_y ...
    last_vertical = VerticalEncoder.position(deg);  // 注意这一行
    last_angle = angle;
}

// 偏移补正
double radius_v = arc_v / delta_angle + OFFSET_V;
// OFFSET_V = -4.0（竖轮在旋转中心左边 4cm）
// 但实际安装时竖轮在旋转中心右边