导航部分效果最好的版本，主分支

2026-04-13 23:30:22 +08:00
commit 350fd830f4
1679 changed files with 1464081 additions and 0 deletions
--- a/App/nav/track_map.c
+++ b/App/nav/track_map.c
@@ -0,0 +1,96 @@
+/**
+ * @file   track_map.c
+ * @brief  赛道地图 — S 型遍历拓扑表
+ *
+ *         正确的场地结构 (参见 Doc/map.md):
+ *           - 垄沟沿 X 轴（横向）分布，长 220cm
+ *           - 左右两端各有纵向端部通道，宽 40cm
+ *           - 启动区在左下角，入口对齐左端通道
+ *           - C1 最靠近入口(南侧)，C6 最远(北侧)
+ *
+ *         S 型路径 (以"北=Y递减=地图上方"为参考):
+ *           入场→左端通道→右转入C1(→东)→右端到端→左转(朝北)
+ *           →北行70cm→左转入C2(←西)→左端到端→右转(朝北)
+ *           →北行70cm→右转入C3(→东)→...
+ *           →C6(←西)→左端到端→左转(朝南)→南行出场
+ *
+ *         entry_turn_dir: 从纵向通道(朝北)转入垄沟的方向
+ *         exit_turn_dir:  从垄沟到端后转入纵向通道的方向
+ */
+
+#include "track_map.h"
+
+/* =========================================================
+ * 硬编码 S 型遍历表
+ *
+ * 转向方向推导 (北=机器人沿纵向通道前进方向):
+ *   C1: 左端通道朝北→右转→朝东入沟; 到右端→左转→朝北
+ *   C2: 右端通道朝北→左转→朝西入沟; 到左端→右转→朝北
+ *   C3: 左端通道朝北→右转→朝东入沟; 到右端→左转→朝北
+ *   C4: 右端通道朝北→左转→朝西入沟; 到左端→右转→朝北
+ *   C5: 左端通道朝北→右转→朝东入沟; 到右端→左转→朝北
+ *   C6: 右端通道朝北→左转→朝西入沟; 到左端→左转→朝南(出场)
+ * ========================================================= */
+static const TrackMap_t s_map = {
+    .corridors = {
+        /*  id  travel_dir       exit_turn        entry_turn       is_last
+         *  --  ----------       ---------        ----------       ------- */
+        {   0,  TRAVEL_DIR_EAST, TURN_DIR_LEFT,   TURN_DIR_RIGHT,  false  },  /* C1: →东, 从左端右转入, 到右端左转出 */
+        {   1,  TRAVEL_DIR_WEST, TURN_DIR_RIGHT,  TURN_DIR_LEFT,   false  },  /* C2: ←西, 从右端左转入, 到左端右转出 */
+        {   2,  TRAVEL_DIR_EAST, TURN_DIR_LEFT,   TURN_DIR_RIGHT,  false  },  /* C3: →东 */
+        {   3,  TRAVEL_DIR_WEST, TURN_DIR_RIGHT,  TURN_DIR_LEFT,   false  },  /* C4: ←西 */
+        {   4,  TRAVEL_DIR_EAST, TURN_DIR_LEFT,   TURN_DIR_RIGHT,  false  },  /* C5: →东 */
+        {   5,  TRAVEL_DIR_WEST, TURN_DIR_LEFT,   TURN_DIR_LEFT,   true   },  /* C6: ←西, 从右端左转入, 到左端左转出(朝南出场) */
+    },
+    .entry_corridor_id = 0,
+    .link_distance_m   = TRACK_MAP_LINK_DISTANCE_M,
+    .corridor_length_m = TRACK_MAP_CORRIDOR_LENGTH_M,
+};
+
+/* =========================================================
+ * API 实现
+ * ========================================================= */
+
+void TrackMap_Init(void)
+{
+    /* 静态表，无需运行时初始化 */
+}
+
+const TrackMap_t* TrackMap_Get(void)
+{
+    return &s_map;
+}
+
+const CorridorDescriptor_t* TrackMap_GetCorridor(uint8_t id)
+{
+    if (id >= TRACK_MAP_CORRIDOR_COUNT) {
+        return &s_map.corridors[0];
+    }
+    return &s_map.corridors[id];
+}
+
+uint8_t TrackMap_GetNextCorridorId(uint8_t current_id)
+{
+    if (current_id + 1 >= TRACK_MAP_CORRIDOR_COUNT) {
+        return current_id;  /* 已是最后一条 */
+    }
+    return current_id + 1;
+}
+
+bool TrackMap_IsLastCorridor(uint8_t id)
+{
+    if (id >= TRACK_MAP_CORRIDOR_COUNT) return true;
+    return s_map.corridors[id].is_last;
+}
+
+TurnDirection_t TrackMap_GetExitTurnDir(uint8_t id)
+{
+    if (id >= TRACK_MAP_CORRIDOR_COUNT) return TURN_DIR_LEFT;
+    return s_map.corridors[id].exit_turn_dir;
+}
+
+TurnDirection_t TrackMap_GetEntryTurnDir(uint8_t id)
+{
+    if (id >= TRACK_MAP_CORRIDOR_COUNT) return TURN_DIR_LEFT;
+    return s_map.corridors[id].entry_turn_dir;
+}