0 / 8 页已完成

代码实现Code Implementation

      从公式到代码From Formulas to Code：在看 C++ 代码之前，先用自然语言理解算法在做什么：: Before looking at the C++ code, understand what the algorithm does in plain language:
      读传感器Read sensors — 一次性读取竖轮、横轮、陀螺仪的当前值 — read vertical wheel, horizontal wheel, and gyroscope values all at once
算弧长Calculate arc length — 用编码器变化量 × 轮子半径，得到竖轮和横轮各走了多远 — encoder change × wheel radius gives the distance each wheel traveled
算角度变化Calculate angle change — 当前角度 - 上次角度 = Δθ — current angle - previous angle = Δθ
判断直线还是弧线Determine straight or arc — 如果 Δθ 几乎为零 → 当直线，弧长就是位移；否则 → 用圆弧模型算弦长 — if Δθ is nearly zero → treat as straight line (arc = displacement); otherwise → use arc model for chord length
坐标旋转Coordinate rotation — 用 sin/cos 把"机器人视角的前后左右"转成"场地视角的 x 和 y" — use sin/cos to convert "robot's forward/sideways" into "field x and y"
更新坐标Update coordinates — 把位移加到全局坐标上 — add displacement to global coordinates
保存当前值Save current values — 存起来给下次循环用 — store for the next loop iteration

    

3.1 准备工作：常量定义3.1 Preparation: Constant Definitions

C++

// 定位轮参数
#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 度

为什么用弧度？Why use radians? 三角函数 sin() 和 cos() 要求输入弧度，不是角度。Trig functions sin() and cos() require radians, not degrees.
弧度 = 角度 × π ÷ 180radians = degrees × π ÷ 180

3.2 先看极简版（只处理直线）3.2 Start Simple (Straight Line Only)

如果机器人只走直线不转弯，定位代码只需要 5 行：If the robot only goes straight without turning, the tracking code needs just 5 lines:

C++

// 极简版：假设不转弯
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);
}

这个版本在机器人走直线时完全正确，转弯时有误差但也能用。很多队伍一开始就用这个版本。This version is perfectly accurate for straight lines, and has some error when turning but is still usable. Many teams start with this version.

3.3 完整版：加上圆弧修正3.3 Full Version: Adding Arc Correction

在极简版基础上，加上转弯时的圆弧修正（新增部分用 // ★ 标注）：Building on the simple version, adding arc correction for turns (new parts marked with // ★):

C++

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;
}

对比极简版，完整版多了什么？What does the full version add compared to the simple one?

改进Improvement	作用Purpose	不加会怎样What happens without it
传感器一次性读取到局部变量Read sensors into local variables at once	保证计算和保存用的是同一组数据Ensures calculation and storage use the same data	偶尔数据不一致Occasional data inconsistency
`fabs(delta_angle) < 阈值threshold`	避免除以接近零的数Avoid dividing by near-zero	半径算出天文数字，坐标飞走Radius becomes astronomical, coordinates fly away
圆弧弦长计算Arc chord calculation	转弯时更精确More accurate when turning	走弧线误差大Large error on curved paths
偏移补正Offset correction	消除轮子不在旋转中心的影响Eliminates effect of wheel not being at rotation center	原地转圈坐标会漂移Coordinates drift when spinning in place
平均角度Average angle	用中间时刻的角度更准Using mid-point angle is more accurate	走弧线时 x/y 分配有偏差x/y distribution is biased on arcs

3.4 后台线程：持续更新3.4 Background Thread: Continuous Updates

定位系统需要在后台不停地跑，不能被其他代码打断：The tracking system needs to run continuously in the background without being interrupted by other code:

C++

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

在 main() 里启动它：Start it in main():

C++

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

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

3.5 逐行解读3.5 Line-by-Line Explanation

为什么编码器值要乘 `DEG_TO_RAD * (直径/2)`？Why multiply the encoder value by `DEG_TO_RAD * (diameter/2)`?

轮子走的距离 = 转过的角度（弧度） × 半径Distance traveled by wheel = angle turned (radians) × radius

编码器返回角度（度） → 乘 π/180 → 角度（弧度）Encoder returns angle (degrees) → multiply by π/180 → angle (radians)
弧度 × 半径 = 弧长（距离）Radians × radius = arc length (distance)

为什么不能用 `delta_angle == 0`？Why can't we use `delta_angle == 0`?

因为陀螺仪总有微小噪声，返回值可能是 0.00003 而不是精确的 0。用 == 0 判断几乎永远为 false，导致每次都走圆弧分支 —— 当 delta_angle 极小时，arc_v / delta_angle 会算出一个巨大的半径，坐标直接飞走。用阈值 fabs(delta_angle) < 0.001 就安全了。Because the gyroscope always has tiny noise — the value might be 0.00003 instead of exactly 0. Using == 0 is almost always false, forcing the arc branch every time. When delta_angle is extremely small, arc_v / delta_angle produces a huge radius, and coordinates fly away. Using a threshold fabs(delta_angle) < 0.001 is safe.

为什么用 `sin(avg_angle)` 和 `cos(avg_angle)`？Why use `sin(avg_angle)` and `cos(avg_angle)`?

这是坐标旋转。定位轮测到的是机器人自己视角的前后左右，我们需要转换成场地视角的 x 和 y。用平均角度做旋转最准。This is coordinate rotation. The tracking wheels measure in the robot's local frame (forward/sideways), and we need to convert to the field frame (x and y). Using the average angle gives the most accurate rotation.

试一试Try it：回到数学基础的三角函数交互组件，拖动角度从 0° 到 45° 到 90°，观察 sin 和 cos 的值怎么变化。你会发现：角度为 0° 时 cos=1、sin=0（前进只增加 y）；角度为 90° 时 cos=0、sin=1（前进只增加 x）。这就是坐标旋转在做的事。: Go back to the trig interactive in Math Foundations, drag the angle from 0° to 45° to 90°, and watch how sin and cos change. At 0°: cos=1, sin=0 (forward only increases y); at 90°: cos=0, sin=1 (forward only increases x). That's what coordinate rotation does.

检查点Checkpoint

代码里为什么用 fabs(delta_angle) < 0.001 而不是 delta_angle == 0？Why does the code use fabs(delta_angle) < 0.001 instead of delta_angle == 0?