整理季后赛仓库说明并保留本地自动路径

Claude/frc postseason refactor wepjot

README.md

@@ -0,0 +1,103 @@
+# FRC 6494 · 2026 Regional Offseason
+
+FRC 6494 的 2026 季后赛机器人 Java 项目。代码包含 Phoenix 6 Swerve 底盘、PathPlanner 自动程序、Limelight 定位，以及射击、进球、输送和灯带控制。
+
+## 当前版本与分支
+
+- 默认工作分支：`7.4`。
+- Gitea 地址：<https://git.6494.team/admin6494/FRC6494_2026Regional-offseason>。
+- 原 GitHub 仓库：<https://github.com/HarryWang163/FRC6494_2026Regional>。
+- `main`、`1`、`季后赛季后赛季后赛` 和 `手搓` 保留为原有历史分支。当前季后赛开发以 `7.4` 为准。
+- 本次整理保留原提交历史，并纳入本地已有的 `New Auto.auto` 与 `New Path.path`。
+
+## 开发环境
+
+| 项目 | 配置版本 |
+| --- | --- |
+| Java 源码与目标版本 | 17 |
+| WPILib / GradleRIO | 2026.2.1 |
+| Gradle Wrapper | 8.11 |
+| CTRE Phoenix 6 | 26.1.1 |
+| PathPlannerLib | 2026.1.2 |
+| 队号 | 6494 |
+
+建议使用 WPILib 2026 开发环境。第三方依赖由 `vendordeps/` 中的 JSON 定义，首次构建需要取得 Gradle 与相关依赖。队号保存在 `.wpilib/wpilib_preferences.json`。
+
+## 获取与构建
+
+```powershell
+git clone --branch 7.4 https://git.6494.team/admin6494/FRC6494_2026Regional-offseason.git
+cd FRC6494_2026Regional-offseason
+.\gradlew.bat build
+```
+
+Linux/macOS 构建命令为 `./gradlew build`。
+
+在具备桌面仿真依赖的 WPILib 环境中，可以运行：
+
+```powershell
+.\gradlew.bat simulateJava
+```
+
+连接队伍机器人并确认队号、网络和机构参数后，可以通过 WPILib VS Code 的部署命令部署，或手动运行 `.\gradlew.bat deploy`。仿真不能替代真实传感器、机构动作和射击参数的实机验证。
+
+## 代码结构
+
+| 位置 | 用途 |
+| --- | --- |
+| `src/main/java/frc/robot/Robot.java` | 机器人模式切换与周期调度 |
+| `src/main/java/frc/robot/RobotContainer.java` | 子系统组装、手柄绑定、自动程序选择 |
+| `src/main/java/frc/robot/Constants.java` | CAN ID、机构参数、场地坐标和状态机阈值 |
+| `src/main/java/frc/robot/controls/` | 驾驶控制、操作员意图和自动命令注册 |
+| `src/main/java/frc/robot/subsystems/Superstructure.java` | 协调射击、进球、输送等机构的状态机 |
+| `src/main/java/frc/robot/subsystems/` | 底盘、机构、Limelight 和 LED 子系统 |
+| `src/main/java/frc/robot/generated/TunerConstants.java` | Phoenix Tuner X 底盘配置 |
+| `src/main/java/frc/robot/tuning/` | 仪表盘调参入口 |
+| `src/main/deploy/pathplanner/` | 自动程序、路径、导航网格和机器人配置 |
+| `vendordeps/` | 第三方库版本与依赖描述 |
+| `elastic-layout*.json` | Elastic 仪表盘布局 |
+| `tuner-project.json` | Phoenix Tuner 项目配置 |
+
+自动程序通过 `Auto Chooser` 选择。PathPlanner NamedCommands 在 `controls/Autocommand.java` 中注册。`New Auto` 会引用 `New Path`；它们是已有的本地自动程序资源，使用前应检查路径与起始位姿。
+
+## 当前手柄绑定
+
+代码使用 0 号端口的单个 Xbox 手柄。以下绑定以 `RobotContainer.java` 为准：
+
+| 输入 | 动作 |
+| --- | --- |
+| 左摇杆 | 底盘平移 |
+| 右摇杆 X | 底盘旋转输入 |
+| Back | 重置场地坐标驾驶角度 |
+| 右扳机 | 请求向 Hub 射击 |
+| 右肩键 | 启用驾驶加速模式 |
+| 左扳机 | 请求传球 |
+| 左肩键 | 请求进球 |
+| B | 请求反转排球 |
+| Y / A | 手动抬起 / 放下进球机构 |
+| 十字键左 / 右 | 飞轮目标速度偏移 -1 / +1 |
+| 十字键上 / 下 | 背板目标位置偏移 +100 / -100 |
+| Start | 运行背板复位命令 |
+
+主要射击和进球按钮受 Teleop 模式门控。射击准备、喂球及超时回退逻辑位于 `Superstructure.java`，相关阈值位于 `Constants.Superstructure`，应结合实机检查这些行为。
+
+## 仓库维护
+
+源码、自动路径、硬件配置、Gradle Wrapper 和仪表盘布局应提交到 Git。`.gitignore` 已排除 `build/`、`.gradle/`、`bin/`、`ctre_sim/` 等生成文件和缓存。
+
+新克隆的仓库可以通过以下命令提交更新：
+
+```powershell
+git status
+git add <需要提交的文件>
+git commit -m "说明本次修改"
+git push origin 7.4
+```
+
+原本连接 GitHub 的本地副本另加了名为 `gitea` 的远程地址，上传到 Gitea 时使用 `git push gitea 7.4`。公开仓库允许匿名浏览和克隆，推送需要具有写入权限的账号或访问令牌。
+
+## 验证范围与许可
+
+本次仓库整理检查了文件、Git 历史和上传内容，并验证远程仓库的分支与匿名克隆；未执行编译、桌面仿真或机器人部署。构建命令是开发使用说明，不代表本次已通过构建或实机测试。
+
+保留项目中的 WPILib 声明与 `WPILib-License.md`。第三方库及外部代码应遵循各自原有的许可说明。

build.gradle

@@ -33,7 +33,7 @@ deploy {
                 frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
                     files = project.fileTree('src/main/deploy')
                     directory = '/home/lvuser/deploy'
-                    deleteOldFiles = false // Change to true to delete files on roboRIO that no
+                    deleteOldFiles = true // Change to true to delete files on roboRIO that no
                                            // longer exist in deploy directory of this project
                 }
             }

simgui-ds.json

@@ -0,0 +1,92 @@
+{
+  "keyboardJoysticks": [
+    {
+      "axisConfig": [
+        {
+          "decKey": 65,
+          "incKey": 68
+        },
+        {
+          "decKey": 87,
+          "incKey": 83
+        },
+        {
+          "decKey": 69,
+          "decayRate": 0.0,
+          "incKey": 82,
+          "keyRate": 0.009999999776482582
+        }
+      ],
+      "axisCount": 3,
+      "buttonCount": 4,
+      "buttonKeys": [
+        90,
+        88,
+        67,
+        86
+      ],
+      "povConfig": [
+        {
+          "key0": 328,
+          "key135": 323,
+          "key180": 322,
+          "key225": 321,
+          "key270": 324,
+          "key315": 327,
+          "key45": 329,
+          "key90": 326
+        }
+      ],
+      "povCount": 1
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 74,
+          "incKey": 76
+        },
+        {
+          "decKey": 73,
+          "incKey": 75
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 4,
+      "buttonKeys": [
+        77,
+        44,
+        46,
+        47
+      ],
+      "povCount": 0
+    },
+    {
+      "axisConfig": [
+        {
+          "decKey": 263,
+          "incKey": 262
+        },
+        {
+          "decKey": 265,
+          "incKey": 264
+        }
+      ],
+      "axisCount": 2,
+      "buttonCount": 6,
+      "buttonKeys": [
+        260,
+        268,
+        266,
+        261,
+        269,
+        267
+      ],
+      "povCount": 0
+    },
+    {
+      "axisCount": 0,
+      "buttonCount": 0,
+      "povCount": 0
+    }
+  ]
+}

src/main/deploy/pathplanner/autos/BlueLeft1.8cycle.auto

@@ -0,0 +1,130 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "intaker_down"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "start_intake_and_conveyor"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "blueLeft1"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "start_intake_and_conveyor"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "blueLeft2"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "blueLeft3"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "blueLeftTurnToHub2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "stop_intake_and_conveyor"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot_hub"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AutoAim"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "start_intake_and_conveyor"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "blueLeftTurnToHub1"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "blueLeft4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "blueLeft5"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot_hub"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AutoAim"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Blue",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/BlueRight1.8cycle.auto

@@ -0,0 +1,130 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "intaker_down"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "start_intake_and_conveyor"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "blueRight1"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "start_intake_and_conveyor"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "blueRight2"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "blueRight3"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "blueRightTurnToHub2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "stop_intake_and_conveyor"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot_hub"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AutoAim"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "start_intake_and_conveyor"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "blueRightTurnToHub1"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "blueRight4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "blueRight5"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot_hub"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AutoAim"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Blue",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Line.auto

@@ -1,19 +0,0 @@
-{
-  "version": "2025.0",
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Line"
-          }
-        }
-      ]
-    }
-  },
-  "resetOdom": false,
-  "folder": null,
-  "choreoAuto": false
-}

src/main/deploy/pathplanner/autos/LineGoAndBack.auto

@@ -1,37 +0,0 @@
-{
-  "version": "2025.0",
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Line"
-          }
-        },
-        {
-          "type": "wait",
-          "data": {
-            "waitTime": 1.0
-          }
-        },
-        {
-          "type": "path",
-          "data": {
-            "pathName": "LineBack"
-          }
-        },
-        {
-          "type": "wait",
-          "data": {
-            "waitTime": 1.0
-          }
-        }
-      ]
-    }
-  },
-  "resetOdom": false,
-  "folder": null,
-  "choreoAuto": false
-}

src/main/deploy/pathplanner/autos/New Auto.auto

@@ -3,7 +3,26 @@
   "command": {
     "type": "sequential",
     "data": {
-      "commands": []
+      "commands": [
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.0
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "New Path"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "intaker_down"
+          }
+        }
+      ]
     }
   },
   "resetOdom": true,

src/main/deploy/pathplanner/autos/RedLeft1.8cycle.auto

@@ -0,0 +1,111 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "intaker_down"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "start_intake_and_conveyor"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "red1"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "start_intake_and_conveyor"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "red2"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "red3"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "turntohub2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "stop_intake_and_conveyor"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot_hub"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AutoAim"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "start_intake_and_conveyor"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "turntohub1"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "red4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "red5"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Red",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/RedMiddle.auto

@@ -0,0 +1,81 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "intaker_down"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "start_intake_and_conveyor"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "RedGotodepot"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot_hub"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AutoAim"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "redDepotToGoudong"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "start_intake_and_conveyor"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "red4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "red5"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Red",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/RedRight1.8cycle.auto

@@ -0,0 +1,111 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "intaker_down"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "start_intake_and_conveyor"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "redRight1"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "start_intake_and_conveyor"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "redRight2"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "redRight3"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "redRightTurnToHub2"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "stop_intake_and_conveyor"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "shoot_hub"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "AutoAim"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "start_intake_and_conveyor"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "redRightTurnToHub1"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "redRight4"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "redRight5"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": "Red",
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/Rotate.auto

@@ -1,19 +0,0 @@
-{
-  "version": "2025.0",
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "Rotate"
-          }
-        }
-      ]
-    }
-  },
-  "resetOdom": false,
-  "folder": null,
-  "choreoAuto": false
-}

src/main/deploy/pathplanner/autos/RotateAndLine.auto

@@ -1,19 +0,0 @@
-{
-  "version": "2025.0",
-  "command": {
-    "type": "sequential",
-    "data": {
-      "commands": [
-        {
-          "type": "path",
-          "data": {
-            "pathName": "RotateAndLine"
-          }
-        }
-      ]
-    }
-  },
-  "resetOdom": false,
-  "folder": null,
-  "choreoAuto": false
-}

src/main/deploy/pathplanner/paths/New Path.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 14.314,
+        "x": 3.5467141162514837,
-        "y": 4.15
+        "y": 4.051138790035587
       },
       "prevControl": null,
       "nextControl": {
-        "x": 14.299415121255347,
+        "x": 4.255399988299603,
-        "y": 4.61077032810271
+        "y": 4.013414379701552
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 14.314,
+        "x": 2.944199288256229,
-        "y": 6.668017118402283
+        "y": 4.051138790035587
       },
       "prevControl": {
-        "x": 14.247660485021397,
+        "x": 2.694910022767602,
-        "y": 6.305734664764623
+        "y": 4.069976574197767
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 1.5,
+    "maxVelocity": 2.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 4.0,
     "maxAngularVelocity": 360.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 180.0
+    "rotation": 0.0
   },
   "reversed": false,
   "folder": null,
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 180.0
+    "rotation": 0.0
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/RedGotodepot.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 13.00554921540656,
+        "y": 4.054407988587732
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 13.221096992334767,
+        "y": 3.9277621996277037
+      },
+      "isLocked": false,
+      "linkedName": "middle"
+    },
+    {
+      "anchor": {
+        "x": 15.613421686746985,
+        "y": 2.2319277108433733
+      },
+      "prevControl": {
+        "x": 14.309604731740064,
+        "y": 3.053280693788945
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "depot"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 180.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 158.4590240814616
+  },
+  "reversed": false,
+  "folder": "Red Right from pwz",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/blueLeft1.path

@@ -0,0 +1,75 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.466034236804566,
+        "y": 7.454255952380953
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.426124431791342,
+        "y": 7.458578116355653
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.7055580357142865,
+        "y": 7.454255952380953
+      },
+      "prevControl": {
+        "x": 5.362545053990692,
+        "y": 7.465613159152816
+      },
+      "nextControl": {
+        "x": 6.019722228934057,
+        "y": 7.443853932944885
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.765392296718973,
+        "y": 6.784465049928674
+      },
+      "prevControl": {
+        "x": 7.765392296718973,
+        "y": 7.285812251387297
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.0,
+      "rotationDegrees": 0.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 180.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 1.5,
+    "rotation": 90.0
+  },
+  "reversed": false,
+  "folder": "Blue Left from pwz",
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/blueLeft2.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.765392296718973,
+        "y": 6.784465049928674
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.750697804389789,
+        "y": 5.401087218387813
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.765392296718973,
+        "y": 4.597831669044222
+      },
+      "prevControl": {
+        "x": 7.743897101946867,
+        "y": 5.99227817461089
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 90.0
+  },
+  "reversed": false,
+  "folder": "Blue Left from pwz",
+  "idealStartingState": {
+    "velocity": 1.5,
+    "rotation": 90.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/blueLeft3.path

@@ -0,0 +1,79 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.765392296718973,
+        "y": 4.597831669044222
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.323255476508917,
+        "y": 5.464573812325125
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.182807228915662,
+        "y": 7.247746987951809
+      },
+      "prevControl": {
+        "x": 6.705631547302918,
+        "y": 6.92851993042499
+      },
+      "nextControl": {
+        "x": 5.9062353004038926,
+        "y": 7.416616797232436
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.418096385542169,
+        "y": 7.3788795180722895
+      },
+      "prevControl": {
+        "x": 3.8149559923113934,
+        "y": 7.302711465107156
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.0,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 2.0,
+      "rotationDegrees": 0.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 1.2,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Blue Left from pwz",
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": 90.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/blueLeft4.path

@@ -0,0 +1,99 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.418096385542169,
+        "y": 7.3788795180722895
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.6528666325574033,
+        "y": 7.464803502595855
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.778710843373494,
+        "y": 7.510012048192771
+      },
+      "prevControl": {
+        "x": 3.540175760112069,
+        "y": 7.435172125334279
+      },
+      "nextControl": {
+        "x": 4.022178849802291,
+        "y": 7.586399666192227
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.33579518072289,
+        "y": 7.3788795180722895
+      },
+      "prevControl": {
+        "x": 5.948962229553457,
+        "y": 7.4850837462721405
+      },
+      "nextControl": {
+        "x": 6.584826721475427,
+        "y": 7.310508396886549
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.765392296718973,
+        "y": 6.784465049928674
+      },
+      "prevControl": {
+        "x": 6.937920174570973,
+        "y": 7.201986151309663
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.1,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 1,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 2.0,
+      "rotationDegrees": 0.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 180.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 1.5,
+    "rotation": 90.0
+  },
+  "reversed": false,
+  "folder": "Blue Left from pwz",
+  "idealStartingState": {
+    "velocity": 1.2,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/blueLeft5.path

@@ -0,0 +1,119 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.765392296718973,
+        "y": 6.784465049928674
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.733458465905914,
+        "y": 6.464214217773148
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.813229166666666,
+        "y": 4.668807228915663
+      },
+      "prevControl": {
+        "x": 8.011674331775877,
+        "y": 5.027990467000143
+      },
+      "nextControl": {
+        "x": 7.485447276626493,
+        "y": 4.075526155097174
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.33579518072289,
+        "y": 4.668807228915663
+      },
+      "prevControl": {
+        "x": 6.546160668095942,
+        "y": 4.185317251794125
+      },
+      "nextControl": {
+        "x": 6.148280431906521,
+        "y": 5.099778595689354
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.1609518072289156,
+        "y": 7.302385542168675
+      },
+      "prevControl": {
+        "x": 6.655393279169644,
+        "y": 6.772626822232179
+      },
+      "nextControl": {
+        "x": 5.9903722446389125,
+        "y": 7.485149359229394
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.29766265060241,
+        "y": 7.4553734939759035
+      },
+      "prevControl": {
+        "x": 6.100054686274564,
+        "y": 7.328414006675032
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1,
+      "rotationDegrees": 43.66778014613041
+    },
+    {
+      "waypointRelativePos": 2,
+      "rotationDegrees": -90.0
+    },
+    {
+      "waypointRelativePos": 2.4,
+      "rotationDegrees": -90.0
+    },
+    {
+      "waypointRelativePos": 3.0,
+      "rotationDegrees": 0.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 180.0,
+    "maxAngularAcceleration": 180.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Blue Left from pwz",
+  "idealStartingState": {
+    "velocity": 1.5,
+    "rotation": 90.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/blueLeftTurnToHub1.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.0028433734939757,
+        "y": 6.471879518072289
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.1230481927710834,
+        "y": 6.941771084337349
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.418096385542169,
+        "y": 7.3788795180722895
+      },
+      "prevControl": {
+        "x": 3.260671343049193,
+        "y": 7.152135161355801
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 1.2,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Blue Left from pwz",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -38.99099404250592
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/blueLeftTurnToHub2.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.418096385542169,
+        "y": 7.3788795180722895
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.175553518148792,
+        "y": 7.3182745885978715
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.0028433734939757,
+        "y": 6.471879518072289
+      },
+      "prevControl": {
+        "x": 3.540329100641909,
+        "y": 7.025981025402732
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -38.99099404250592
+  },
+  "reversed": false,
+  "folder": "Blue Left from pwz",
+  "idealStartingState": {
+    "velocity": 1.2,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/blueRight1.path

@@ -0,0 +1,75 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 4.466034236804566,
+        "y": 0.6157440476190477
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 5.426124431791342,
+        "y": 0.611421883644347
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.7055580357142865,
+        "y": 0.6157440476190477
+      },
+      "prevControl": {
+        "x": 5.362545053990692,
+        "y": 0.6043868408471844
+      },
+      "nextControl": {
+        "x": 6.019722228934057,
+        "y": 0.626146067055116
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.765392296718973,
+        "y": 1.2855349500713271
+      },
+      "prevControl": {
+        "x": 7.765392296718973,
+        "y": 0.7841877486127031
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.0,
+      "rotationDegrees": 0.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 180.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 1.5,
+    "rotation": -90.0
+  },
+  "reversed": false,
+  "folder": "Blue Right from pwz",
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/blueRight2.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.765392296718973,
+        "y": 1.2855349500713271
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.750697804389789,
+        "y": 2.668912781612187
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.765392296718973,
+        "y": 3.472168330955778
+      },
+      "prevControl": {
+        "x": 7.743897101946867,
+        "y": 2.0777218253891108
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": -90.0
+  },
+  "reversed": false,
+  "folder": "Blue Right from pwz",
+  "idealStartingState": {
+    "velocity": 1.5,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/blueRight3.path

@@ -0,0 +1,79 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.765392296718973,
+        "y": 3.472168330955778
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.323255476508917,
+        "y": 2.605426187674875
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.182807228915662,
+        "y": 0.8222530120481917
+      },
+      "prevControl": {
+        "x": 6.705631547302918,
+        "y": 1.1414800695750102
+      },
+      "nextControl": {
+        "x": 5.9062353004038926,
+        "y": 0.6533832027675645
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.418096385542169,
+        "y": 0.691120481927711
+      },
+      "prevControl": {
+        "x": 3.8149559923113934,
+        "y": 0.7672885348928444
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.0,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 2.0,
+      "rotationDegrees": 0.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 1.2,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Blue Right from pwz",
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/blueRight4.path

@@ -0,0 +1,99 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.418096385542169,
+        "y": 0.691120481927711
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.6528666325574033,
+        "y": 0.6051964974041462
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.778710843373494,
+        "y": 0.5599879518072293
+      },
+      "prevControl": {
+        "x": 3.540175760112069,
+        "y": 0.634827874665722
+      },
+      "nextControl": {
+        "x": 4.022178849802291,
+        "y": 0.48360033380777306
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.33579518072289,
+        "y": 0.691120481927711
+      },
+      "prevControl": {
+        "x": 5.948962229553457,
+        "y": 0.5849162537278598
+      },
+      "nextControl": {
+        "x": 6.584826721475427,
+        "y": 0.7594916031134517
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.765392296718973,
+        "y": 1.2855349500713271
+      },
+      "prevControl": {
+        "x": 6.937920174570973,
+        "y": 0.8680138486903368
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.1,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 1,
+      "rotationDegrees": 0.0
+    },
+    {
+      "waypointRelativePos": 2.0,
+      "rotationDegrees": 0.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 180.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 1.5,
+    "rotation": -90.0
+  },
+  "reversed": false,
+  "folder": "Blue Right from pwz",
+  "idealStartingState": {
+    "velocity": 1.2,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/blueRight5.path

@@ -0,0 +1,119 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 7.765392296718973,
+        "y": 1.2855349500713271
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.733458465905914,
+        "y": 1.6057857822268522
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.813229166666666,
+        "y": 3.4011927710843373
+      },
+      "prevControl": {
+        "x": 8.011674331775877,
+        "y": 3.042009532999857
+      },
+      "nextControl": {
+        "x": 7.485447276626493,
+        "y": 3.9944738449028265
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.33579518072289,
+        "y": 3.4011927710843373
+      },
+      "prevControl": {
+        "x": 6.546160668095942,
+        "y": 3.8846827482058757
+      },
+      "nextControl": {
+        "x": 6.148280431906521,
+        "y": 2.9702214043106463
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 6.1609518072289156,
+        "y": 0.7676144578313244
+      },
+      "prevControl": {
+        "x": 6.655393279169644,
+        "y": 1.2973731777678212
+      },
+      "nextControl": {
+        "x": 5.9903722446389125,
+        "y": 0.584850640770607
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.29766265060241,
+        "y": 0.6146265060240967
+      },
+      "prevControl": {
+        "x": 6.100054686274564,
+        "y": 0.7415859933249683
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1,
+      "rotationDegrees": -43.66778014613044
+    },
+    {
+      "waypointRelativePos": 2,
+      "rotationDegrees": 90.0
+    },
+    {
+      "waypointRelativePos": 2.4,
+      "rotationDegrees": 90.0
+    },
+    {
+      "waypointRelativePos": 3.0,
+      "rotationDegrees": 0.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 180.0,
+    "maxAngularAcceleration": 180.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Blue Right from pwz",
+  "idealStartingState": {
+    "velocity": 1.5,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/blueRightTurnToHub1.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.0028433734939757,
+        "y": 1.5981204819277117
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.1230481927710834,
+        "y": 1.128228915662652
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.418096385542169,
+        "y": 0.691120481927711
+      },
+      "prevControl": {
+        "x": 3.260671343049193,
+        "y": 0.9178648386441994
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 1.2,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": "Blue Right from pwz",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 38.99099404250589
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/blueRightTurnToHub2.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.418096385542169,
+        "y": 0.691120481927711
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.175553518148792,
+        "y": 0.7517254114021283
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.0028433734939757,
+        "y": 1.5981204819277117
+      },
+      "prevControl": {
+        "x": 3.540329100641909,
+        "y": 1.0440189745972686
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 38.99099404250589
+  },
+  "reversed": false,
+  "folder": "Blue Right from pwz",
+  "idealStartingState": {
+    "velocity": 1.2,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/red1.path

@@ -0,0 +1,75 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 12.073965763195433,
+        "y": 0.6157440476190477
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 11.113875568208657,
+        "y": 0.6114218836443469
+      },
+      "isLocked": false,
+      "linkedName": "trench"
+    },
+    {
+      "anchor": {
+        "x": 10.834441964285713,
+        "y": 0.6157440476190477
+      },
+      "prevControl": {
+        "x": 11.177454946009307,
+        "y": 0.6043868408471843
+      },
+      "nextControl": {
+        "x": 10.520277771065942,
+        "y": 0.626146067055116
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.774607703281026,
+        "y": 1.2855349500713271
+      },
+      "prevControl": {
+        "x": 8.774607703281026,
+        "y": 0.7841877486127031
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "1"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.0,
+      "rotationDegrees": 180.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 180.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 1.5,
+    "rotation": -90.0
+  },
+  "reversed": false,
+  "folder": "Red Right from pwz",
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/red2.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.774607703281026,
+        "y": 1.2855349500713271
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 8.78930219561021,
+        "y": 2.668912781612187
+      },
+      "isLocked": false,
+      "linkedName": "1"
+    },
+    {
+      "anchor": {
+        "x": 8.774607703281026,
+        "y": 3.472168330955778
+      },
+      "prevControl": {
+        "x": 8.796102898053132,
+        "y": 2.0777218253891108
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "2"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": -90.0
+  },
+  "reversed": false,
+  "folder": "Red Right from pwz",
+  "idealStartingState": {
+    "velocity": 1.5,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/red3.path

@@ -0,0 +1,79 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.774607703281026,
+        "y": 3.472168330955778
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 9.216744523491082,
+        "y": 2.605426187674875
+      },
+      "isLocked": false,
+      "linkedName": "2"
+    },
+    {
+      "anchor": {
+        "x": 10.357192771084337,
+        "y": 0.8222530120481917
+      },
+      "prevControl": {
+        "x": 9.834368452697081,
+        "y": 1.1414800695750102
+      },
+      "nextControl": {
+        "x": 10.633764699596107,
+        "y": 0.6533832027675643
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 13.12190361445783,
+        "y": 0.691120481927711
+      },
+      "prevControl": {
+        "x": 12.725044007688606,
+        "y": 0.7672885348928444
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "3"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.0,
+      "rotationDegrees": 180.0
+    },
+    {
+      "waypointRelativePos": 2.0,
+      "rotationDegrees": 180.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 1.2,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "Red Right from pwz",
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/red4.path

@@ -0,0 +1,99 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 13.12190361445783,
+        "y": 0.691120481927711
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 12.887133367442596,
+        "y": 0.6051964974041462
+      },
+      "isLocked": false,
+      "linkedName": "3"
+    },
+    {
+      "anchor": {
+        "x": 12.761289156626505,
+        "y": 0.5599879518072293
+      },
+      "prevControl": {
+        "x": 12.99982423988793,
+        "y": 0.634827874665722
+      },
+      "nextControl": {
+        "x": 12.517821150197708,
+        "y": 0.48360033380777306
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 10.204204819277109,
+        "y": 0.691120481927711
+      },
+      "prevControl": {
+        "x": 10.591037770446542,
+        "y": 0.5849162537278597
+      },
+      "nextControl": {
+        "x": 9.955173278524573,
+        "y": 0.7594916031134517
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.774607703281026,
+        "y": 1.2855349500713271
+      },
+      "prevControl": {
+        "x": 9.602079825429026,
+        "y": 0.868013848690337
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "1"
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.1,
+      "rotationDegrees": 180.0
+    },
+    {
+      "waypointRelativePos": 1,
+      "rotationDegrees": 180.0
+    },
+    {
+      "waypointRelativePos": 2.0,
+      "rotationDegrees": 180.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 180.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 1.5,
+    "rotation": -90.0
+  },
+  "reversed": false,
+  "folder": "Red Right from pwz",
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/red5.path

@@ -0,0 +1,119 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.774607703281026,
+        "y": 1.2855349500713271
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 8.806541534094086,
+        "y": 1.6057857822268522
+      },
+      "isLocked": false,
+      "linkedName": "1"
+    },
+    {
+      "anchor": {
+        "x": 8.726770833333333,
+        "y": 3.4011927710843373
+      },
+      "prevControl": {
+        "x": 8.528325668224122,
+        "y": 3.042009532999857
+      },
+      "nextControl": {
+        "x": 9.054552723373506,
+        "y": 3.9944738449028265
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 10.204204819277109,
+        "y": 3.4011927710843373
+      },
+      "prevControl": {
+        "x": 9.993839331904057,
+        "y": 3.8846827482058757
+      },
+      "nextControl": {
+        "x": 10.391719568093478,
+        "y": 2.9702214043106463
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 10.379048192771084,
+        "y": 0.7676144578313244
+      },
+      "prevControl": {
+        "x": 9.884606720830355,
+        "y": 1.2973731777678212
+      },
+      "nextControl": {
+        "x": 10.549627755361087,
+        "y": 0.584850640770607
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 11.242337349397589,
+        "y": 0.6146265060240967
+      },
+      "prevControl": {
+        "x": 10.439945313725435,
+        "y": 0.7415859933249683
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1,
+      "rotationDegrees": -136.3322198538696
+    },
+    {
+      "waypointRelativePos": 2,
+      "rotationDegrees": 90.0
+    },
+    {
+      "waypointRelativePos": 2.4,
+      "rotationDegrees": 90.0
+    },
+    {
+      "waypointRelativePos": 3.0,
+      "rotationDegrees": 180.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 180.0,
+    "maxAngularAcceleration": 180.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "Red Right from pwz",
+  "idealStartingState": {
+    "velocity": 1.5,
+    "rotation": -90.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/redDepotToGoudong.path

@@ -3,29 +3,29 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 14.314,
+        "x": 15.613421686746985,
-        "y": 6.668017118402283
+        "y": 2.2319277108433733
       },
       "prevControl": null,
       "nextControl": {
-        "x": 14.299415121255347,
+        "x": 15.36208433734939,
-        "y": 7.128787446504993
+        "y": 2.2319277108433733
       },
       "isLocked": false,
-      "linkedName": "Point1"
+      "linkedName": "depot"
     },
     {
       "anchor": {
-        "x": 14.314,
+        "x": 13.12190361445783,
-        "y": 4.15
+        "y": 0.691120481927711
       },
       "prevControl": {
-        "x": 14.299415121255347,
+        "x": 13.37190361445783,
-        "y": 4.597831669044222
+        "y": 0.691120481927711
       },
       "nextControl": null,
       "isLocked": false,
-      "linkedName": "StartingPointRed2"
+      "linkedName": "3"
     }
   ],
   "rotationTargets": [],
@@ -41,14 +41,14 @@
     "unlimited": false
   },
   "goalEndState": {
-    "velocity": 0,
+    "velocity": 0.0,
     "rotation": 180.0
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Red Right from pwz",
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 180.0
+    "rotation": 158.4590240814616
   },
-  "useDefaultConstraints": true
+  "useDefaultConstraints": false
 }

src/main/deploy/pathplanner/paths/redRight1.path

@@ -0,0 +1,75 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 12.073965763195433,
+        "y": 7.454255952380953
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 11.113875568208657,
+        "y": 7.458578116355653
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 10.834441964285713,
+        "y": 7.454255952380953
+      },
+      "prevControl": {
+        "x": 11.177454946009307,
+        "y": 7.465613159152816
+      },
+      "nextControl": {
+        "x": 10.520277771065942,
+        "y": 7.443853932944885
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.774607703281026,
+        "y": 6.784465049928674
+      },
+      "prevControl": {
+        "x": 8.774607703281026,
+        "y": 7.285812251387297
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.0,
+      "rotationDegrees": 180.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 180.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 1.5,
+    "rotation": 90.0
+  },
+  "reversed": false,
+  "folder": "Red Right from pwz",
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/redRight2.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.774607703281026,
+        "y": 6.784465049928674
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 8.78930219561021,
+        "y": 5.401087218387813
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.774607703281026,
+        "y": 4.597831669044222
+      },
+      "prevControl": {
+        "x": 8.796102898053132,
+        "y": 5.99227817461089
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 90.0
+  },
+  "reversed": false,
+  "folder": "Red Right from pwz",
+  "idealStartingState": {
+    "velocity": 1.5,
+    "rotation": 90.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/redRight3.path

@@ -0,0 +1,79 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.774607703281026,
+        "y": 4.597831669044222
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 9.216744523491082,
+        "y": 5.464573812325125
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 10.357192771084337,
+        "y": 7.247746987951809
+      },
+      "prevControl": {
+        "x": 9.834368452697081,
+        "y": 6.92851993042499
+      },
+      "nextControl": {
+        "x": 10.633764699596107,
+        "y": 7.416616797232436
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 13.12190361445783,
+        "y": 7.3788795180722895
+      },
+      "prevControl": {
+        "x": 12.725044007688606,
+        "y": 7.302711465107156
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.0,
+      "rotationDegrees": 180.0
+    },
+    {
+      "waypointRelativePos": 2.0,
+      "rotationDegrees": 180.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 1.2,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "Red Right from pwz",
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": 90.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/redRight4.path

@@ -0,0 +1,99 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 13.12190361445783,
+        "y": 7.3788795180722895
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 12.887133367442596,
+        "y": 7.464803502595855
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 12.761289156626505,
+        "y": 7.510012048192771
+      },
+      "prevControl": {
+        "x": 12.99982423988793,
+        "y": 7.435172125334279
+      },
+      "nextControl": {
+        "x": 12.517821150197708,
+        "y": 7.586399666192227
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 10.204204819277109,
+        "y": 7.3788795180722895
+      },
+      "prevControl": {
+        "x": 10.591037770446542,
+        "y": 7.4850837462721405
+      },
+      "nextControl": {
+        "x": 9.955173278524573,
+        "y": 7.310508396886549
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.774607703281026,
+        "y": 6.784465049928674
+      },
+      "prevControl": {
+        "x": 9.602079825429026,
+        "y": 7.201986151309663
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.1,
+      "rotationDegrees": 180.0
+    },
+    {
+      "waypointRelativePos": 1,
+      "rotationDegrees": 180.0
+    },
+    {
+      "waypointRelativePos": 2.0,
+      "rotationDegrees": 180.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 180.0,
+    "maxAngularAcceleration": 360.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 1.5,
+    "rotation": 90.0
+  },
+  "reversed": false,
+  "folder": "Red Right from pwz",
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/redRight5.path

@@ -0,0 +1,119 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.774607703281026,
+        "y": 6.784465049928674
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 8.806541534094086,
+        "y": 6.464214217773148
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.726770833333333,
+        "y": 4.668807228915663
+      },
+      "prevControl": {
+        "x": 8.528325668224122,
+        "y": 5.027990467000143
+      },
+      "nextControl": {
+        "x": 9.054552723373506,
+        "y": 4.075526155097174
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 10.204204819277109,
+        "y": 4.668807228915663
+      },
+      "prevControl": {
+        "x": 9.993839331904057,
+        "y": 4.185317251794125
+      },
+      "nextControl": {
+        "x": 10.391719568093478,
+        "y": 5.099778595689354
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 10.379048192771084,
+        "y": 7.302385542168675
+      },
+      "prevControl": {
+        "x": 9.884606720830355,
+        "y": 6.772626822232179
+      },
+      "nextControl": {
+        "x": 10.549627755361087,
+        "y": 7.485149359229394
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 11.242337349397589,
+        "y": 7.4553734939759035
+      },
+      "prevControl": {
+        "x": 10.439945313725435,
+        "y": 7.328414006675032
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1,
+      "rotationDegrees": 136.33221985386956
+    },
+    {
+      "waypointRelativePos": 2,
+      "rotationDegrees": -90.0
+    },
+    {
+      "waypointRelativePos": 2.4,
+      "rotationDegrees": -90.0
+    },
+    {
+      "waypointRelativePos": 3.0,
+      "rotationDegrees": 180.0
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 1.5,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 180.0,
+    "maxAngularAcceleration": 180.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "Red Right from pwz",
+  "idealStartingState": {
+    "velocity": 1.5,
+    "rotation": 90.0
+  },
+  "useDefaultConstraints": false
+}

src/main/deploy/pathplanner/paths/redRightTurnToHub1.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 14.314,
+        "x": 13.537156626506023,
-        "y": 4.15
+        "y": 6.471879518072289
       },
       "prevControl": null,
       "nextControl": {
-        "x": 14.312353780313838,
+        "x": 13.416951807228916,
-        "y": 4.74015691868759
+        "y": 6.941771084337349
       },
       "isLocked": false,
-      "linkedName": "StartingPointRed1"
+      "linkedName": null
     },
     {
       "anchor": {
-        "x": 14.314,
+        "x": 13.12190361445783,
-        "y": 6.357489300998574
+        "y": 7.3788795180722895
       },
       "prevControl": {
-        "x": 14.312353780313838,
+        "x": 13.279328656950806,
-        "y": 6.098716119828817
+        "y": 7.152135161355801
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,22 +33,22 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 1.5,
+    "maxVelocity": 2.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 4.0,
     "maxAngularVelocity": 360.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
     "unlimited": false
   },
   "goalEndState": {
-    "velocity": 0,
+    "velocity": 0.0,
-    "rotation": 0.0
+    "rotation": 180.0
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Red Right from pwz",
   "idealStartingState": {
     "velocity": 0,
-    "rotation": 180.0
+    "rotation": -141.0090059574941
   },
   "useDefaultConstraints": true
 }

src/main/deploy/pathplanner/paths/redRightTurnToHub2.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 14.314,
+        "x": 13.12190361445783,
-        "y": 4.15
+        "y": 7.3788795180722895
       },
       "prevControl": null,
       "nextControl": {
-        "x": 14.064,
+        "x": 13.364446481851207,
-        "y": 4.15
+        "y": 7.3182745885978715
       },
       "isLocked": false,
-      "linkedName": "StartingPointRed1"
+      "linkedName": null
     },
     {
       "anchor": {
-        "x": 14.314,
+        "x": 13.537156626506023,
-        "y": 4.15
+        "y": 6.471879518072289
       },
       "prevControl": {
-        "x": 14.650181888846735,
+        "x": 12.99967089935809,
-        "y": 4.15
+        "y": 7.025981025402732
       },
       "nextControl": null,
       "isLocked": false,
@@ -33,8 +33,8 @@
   "pointTowardsZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 1.5,
+    "maxVelocity": 2.0,
-    "maxAcceleration": 3.0,
+    "maxAcceleration": 4.0,
     "maxAngularVelocity": 360.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,
@@ -42,13 +42,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 0.0
+    "rotation": -141.0090059574941
   },
   "reversed": false,
-  "folder": null,
+  "folder": "Red Right from pwz",
   "idealStartingState": {
-    "velocity": 0,
+    "velocity": 1.2,
     "rotation": 180.0
   },
   "useDefaultConstraints": true
-}
+}

src/main/deploy/pathplanner/paths/turntohub1.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 13.537156626506023,
+        "y": 1.5981204819277117
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 13.416951807228916,
+        "y": 1.128228915662652
+      },
+      "isLocked": false,
+      "linkedName": "sssss"
+    },
+    {
+      "anchor": {
+        "x": 13.12190361445783,
+        "y": 0.691120481927711
+      },
+      "prevControl": {
+        "x": 13.279328656950806,
+        "y": 0.9178648386441994
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "3"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0.0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": "Red Right from pwz",
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 141.0090059574941
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/turntohub2.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 13.12190361445783,
+        "y": 0.691120481927711
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 13.364446481851207,
+        "y": 0.7517254114021283
+      },
+      "isLocked": false,
+      "linkedName": "3"
+    },
+    {
+      "anchor": {
+        "x": 13.537156626506023,
+        "y": 1.5981204819277117
+      },
+      "prevControl": {
+        "x": 12.99967089935809,
+        "y": 1.0440189745972686
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": "sssss"
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 2.0,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 360.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 141.0090059574941
+  },
+  "reversed": false,
+  "folder": "Red Right from pwz",
+  "idealStartingState": {
+    "velocity": 1.2,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/settings.json

@@ -2,14 +2,22 @@
   "robotWidth": 0.85,
   "robotLength": 0.85,
   "holonomicMode": true,
-  "pathFolders": [],
+  "pathFolders": [
-  "autoFolders": [],
+    "Blue Left from pwz",
-  "defaultMaxVel": 1.5,
+    "Red Right from pwz",
-  "defaultMaxAccel": 3.0,
+    "Blue Right from pwz"
+  ],
+  "autoFolders": [
+    "Blue",
+    "New Folder",
+    "Red"
+  ],
+  "defaultMaxVel": 2.0,
+  "defaultMaxAccel": 4.0,
   "defaultMaxAngVel": 360.0,
   "defaultMaxAngAccel": 720.0,
   "defaultNominalVoltage": 12.0,
-  "robotMass": 30.0,
+  "robotMass": 50.0,
   "robotMOI": 3.6,
   "robotTrackwidth": 0.546,
   "driveWheelRadius": 0.045,

src/main/java/frc/robot/Constants.java

@@ -2,70 +2,91 @@ package frc.robot;
 
 import com.ctre.phoenix6.configs.Slot0Configs;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
 
 public final class Constants {
+    private Constants() {}
+
     public static final boolean LEDUsing = false;
-    public enum RobotStatus{
+
-        Stopped,
+    public static class Shooter {
-        AllTelop,
+        // Placeholder mechanism CAN IDs; update here after final wiring.
-        PassingBall,
+        public static final int leftFlywheelID = 38;
-        AutoAimming,
+        public static final int rightFlywheelID = 39;
-        Climbing,
+        public static final int leftConveyorID = 40;
-        CrossingBump,
+        public static final int rightConveyorID = 37;
-        CrossingTrench
+        public static final int backboardMotorID = 44;
-    }
+
-    public class Shooter {
+        public static final double flywheelNominalKv = 0.12;
-        public static final double conveyorSpeed = 20;
+        public static final double defaultFlywheelVoltage = 3.34;
+        public static final double defaultFlywheelVelocity = defaultFlywheelVoltage / flywheelNominalKv;
+        public static final double flywheelMaxVelocity = 120.0;
+        public static final double flywheelReadyVelocityThreshold = 5.0;
+        public static final double flywheelReadyVelocityTolerance = 2.0;
+        public static final double flywheelReadyDelaySeconds = 0.5;
+
+        // Distance (meters) -> flywheel velocity (rotations per second).
+
+        public static final double shooterConveyorFeedVelocity = 40;
+        public static final double shooterConveyorReverseVelocity = -10;
+        public static final double shooterConveyorMaxVelocity = 120.0;
+
         public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs()
-            .withKP(0.0).withKI(0).withKD(0)
+            .withKP(0.21)
-            .withKV(0.0088).withKA(0.0).withKS(0.0);
+            .withKI(0)
+            .withKD(0.004)
+            .withKS(0.25)
+            .withKA(0.0)
+            .withKV(flywheelNominalKv);
 
         public static final Slot0Configs conveyorSlot0Configs = new Slot0Configs()
-            .withKP(0.0).withKI(0).withKD(0)
+            .withKP(0.19).withKI(0).withKD(0.001)
-            .withKV(0.02).withKA(0.15).withKS(0);
+            .withKV(0.125).withKA(0.0).withKS(0.07);
 
         public static final Slot0Configs backboardSlot0Configs = new Slot0Configs()
             .withKP(0.01).withKI(0).withKD(0)
-            .withKV(0.008).withKA(0.0).withKS(0.010);
+            .withKV(0.03).withKA(0.0).withKS(0.010);
 
-        public class backboardPositionPID {
+        public static class backboardPositionPID {
-            public static final double kP = 0.1;
+            public static final double kP = 0.007;
             public static final double kI = 0.0;
             public static final double kD = 0.0;
-            
         }
-        public static final double backboardSpeedMax = 30; //也就是output
+
-        // 背板的上下限位角度
+        public static final double backboardSpeedMax = 3.0;
-        public static final double backboardUpLimit = 3000.0;  // 背板上限角度（根据实际需求设置）
+        public static final double backboardSpeedMin = 0;
-        public static final double backboardDownLimit = 0.0;  // 背板下限角度（根据实际需求设置）
+        public static final double backboardGravityFeedforwardMinVolts = 0.2;
+        public static final double backboardGravityFeedforwardMaxVolts = 0.5;
+        public static final double backboardGravityFeedforwardMinPosition = 0.0;
+        public static final double backboardGravityFeedforwardMaxPosition = 2200.0;
+        public static final double backboardGravityFeedforwardDirection = 1.0;
+        public static final double backboardUpLimit = 2200.0;
+        public static final double backboardDownLimit = 0.0;
     }
-    public class ShooterCalculation{
+
-        public static final double positionNear = 0;  // 近距离背板角度
+    public static class AutoPositioning {
-        public static final double positionMid  = 1600;  // 中距离背板角度
+        public static final double autoRotationkP = 0.07;
-        public static final double postionFar  = 3200;  // 远距离背板角度
-    }
-    public class AutoPositioning {
-        public static final double autoRotationkP = 0.1;
         public static final double autoRotationForBumpTargetDegrees = 45;
         public static final double autoRotationForTrenchTargetDegrees = 0;
         public static final boolean teleopOffsetEnabled = true;
         public static final double teleopOffsetkP = 15;
 
         public static final double autoPositioningkP = 1.0;
+        public static final double autoPositioningAngleError = 1.0;
 
-        public static final double[] bumpY = {2.51,5.556};
+        public static final double[] bumpY = {2.51, 5.556};
-        public static final double[] trenchY = {0.639,7.43};
+        public static final double[] trenchY = {0.639, 7.43};
 
         public static final double TrenchtargetAngle = 0;
         public static final double TurningkP = 1.5;
     }
 
-    public class StartingPoints{
+    public static class StartingPoints {
-        public class Red{
+        public static class Red {
-            public static final Pose2d Point1 = new Pose2d(12.93, 7.47, edu.wpi.first.math.geometry.Rotation2d.fromDegrees(0));
+            public static final Pose2d Point1 = new Pose2d(12.93, 7.47, Rotation2d.fromDegrees(0));
-            public static final Pose2d Point2 = new Pose2d(14.314, 4.15, edu.wpi.first.math.geometry.Rotation2d.fromDegrees(-180));
+            public static final Pose2d Point2 = new Pose2d(14.314, 4.15, Rotation2d.fromDegrees(-180));
         }
-        
     }
 
     public enum DriveMode {
@@ -74,68 +95,81 @@ public final class Constants {
         SYSID,
         OTHER
     }
-    public class Pigeon {
+
+    public static class Pigeon {
         public static final int CANID = 0;
     }
 
-    public class Limelight {
+    public static class Limelight {
-        public static final String LIMELIGHT_NAME_Shooter = "limelight-shooter";
+        public static final String LIMELIGHT_NAME_Shooter = "limelight-left";
-        public static final String LIMELIGHT_NAME_Intaker = "limelight-intaker";
+        public static final String LIMELIGHT_NAME_Intaker = "limelight-front";
         public static final Boolean UsingMetaTag2 = true;
+        public static final int locatePipelineIndex = 0;
     }
 
-    public class Field {
+    public static class Field {
         public static final double RedHubPositionX = 11.914;
         public static final double RedHubPositionY = 4.034;
-        public static final double PassingBallPosX = 0;
+        public static final double BlueHubPositionX = 4.625;
-        public static final double PassingBallPosY1 = 2.51;
+        public static final double BlueHubPositionY = 4.034;
-        public static final double PassingBallPosY2 = 5.556;
+        public static final double PassingBallPosY1 = 1.405;
+        public static final double PassingBallPosY2 = 6.664;
+        public static final double RedPassingBallPosX = 13.355;
+        public static final double BluePassingBallPosX = 3.184;
     }
-    
 
-    public class Intaker {
+    public static class Intaker {
+        // Placeholder intake CAN IDs; update here after wiring is confirmed.
+        public static final int leftIntakeRollerID = 20;
+        public static final int rightIntakeRollerID = 47;
+        public static final int leftIntakeRotaterID = 48;
+        public static final int rightIntakeRotaterID = 43;
 
-    public static final double intakeRotaterUpLimit = 0.0;  // rotator上限角度
+        public static final double intakeRollerIntakeVoltage = -6.0;
-    public static final double intakeRotaterDownLimit = 0.0;  // rotator下限角度
+        public static final double intakeRollerOuttakeVoltage = 3.0;
 
-    public static final Slot0Configs intakeGetterSlot0Configs = new Slot0Configs()
+        public static final double intakeRotaterRaiseVoltage = 0.8;
-        .withKP(0.00).withKI(0.00).withKD(0.00)
+        public static final double intakeRotaterLowerVoltage = -1.5;
-        .withKV(0.00).withKA(0.00).withKS(0.00);
-    // getter 如果你只用固定速度开关，也可以先不启闭环；若用闭环速度，这里 kP/kV/KS 等需要实测调参(这句fromAI)
 
-    public static final Slot0Configs intakeRotaterSlot0Configs = new Slot0Configs()
+        public static final Slot0Configs intakeRollerSlot0Configs = new Slot0Configs()
-        .withKP(0.00).withKI(0.00).withKD(0.00)
+            .withKP(0.00).withKI(0.00).withKD(0.00)
-        .withKV(0.00).withKA(0.00).withKS(0.00);
+            .withKV(0.00).withKA(0.00).withKS(0.00);
-    //如果 rotater 受重力影响明显，可能需要用 withKG/withGravityType/withGravityArmPositionOffset（看机构形式）(这句fromAI)
+        public static final Slot0Configs intakeGetterSlot0Configs = intakeRollerSlot0Configs;
 
-    public static final double IntakeGetterSpeed = 0.0;
+        public static final Slot0Configs intakeRotaterSlot0Configs = new Slot0Configs()
-    // 单位RPS
+            .withKP(0.00).withKI(0.00).withKD(0.00)
-    public static final double positionToleranceRotations = 0.00;
+            .withKV(0.00).withKA(0.00).withKS(0.00);
-    //rotater容差
-    }   
-    public static class Climber {
 
-    public static final int motorID = 49;
+        public static final double IntakeGetterSpeedforAuto = 0.5;
+        // Placeholder values; tighten after the real mechanism is tuned.
+    }
 
-    public static final boolean inverted = false;
+    public static class Conveyor {
+        public static final int mainConveyorID = 42;
+        public static final double feedVoltage = 2.5;
+        public static final double intakeFeedVoltage = 1.5;
+        public static final double reverseVoltage = -2.0;
 
-    public static final double climbUpTicks = 94; // TODO: 实测后调整
+        public static final Slot0Configs mainConveyorSlot0Configs = new Slot0Configs()
-    public static final double climbDownTicks = 0; // TODO: 实测后调整
+            .withKP(0.00).withKI(0.00).withKD(0.00)
-    public static final double holdToleranceRot = 0.5; 
+            .withKV(0.00).withKA(0.00).withKS(0.00);
-    public static final double deltaDownTicks = 1;
+    }
 
-    public static final Slot0Configs slot0Configs = new Slot0Configs();
+    public static class Superstructure {
+        // State-machine timing and gate tolerances; tune on the real robot.
 
-    static {
+        public static final double shooterFeedVelocity = Shooter.shooterConveyorFeedVelocity;
-        // TODO: PID/F 值你们实测后填
+        public static final double shooterReverseVelocity = Shooter.shooterConveyorReverseVelocity;
-        slot0Configs.kP = 0.2;
+        public static final double shootTimeoutSeconds = 4.0;
-        slot0Configs.kI = 0.0;
+        public static final double prepShootFallbackDelaySeconds = 0.5;
-        slot0Configs.kD = 0.0;
+        public static final double aimToleranceDegrees = 1.5;
+        public static final double stationarySpeedToleranceMetersPerSecond = 0.5;
+        public static final double passBallFlywheelVoltage = Shooter.defaultFlywheelVoltage;
+        public static final double passBallFlywheelVelocity = Shooter.defaultFlywheelVelocity;
+        public static final double passBallBackboardPosition = 175.0;
 
-        // TODO: 如果要前馈（速度闭环一般建议有 kV）
+        // Shoot backboard target position in external encoder counts.
-        slot0Configs.kS = 0.0;
+        public static final double backboardShootPosition = 175.0;
-        slot0Configs.kV = 0.1;
+        public static final double backboardReadyPositionTolerance = 100.0;
-        slot0Configs.kA = 0.0;
     }
 }
-}

src/main/java/frc/robot/Main.java

@@ -1,6 +1,6 @@
-// Copyright (c) FIRST and other WPILib contributors.
+// 版权所有 (c) FIRST 和其他 WPILib 贡献者。
-// Open Source Software; you can modify and/or share it under the terms of
+// 本项目为开源软件；你可以依据项目根目录中的 WPILib BSD license 文件
-// the WPILib BSD license file in the root directory of this project.
+// 修改和/或分享本代码。
 
 package frc.robot;
 

src/main/java/frc/robot/Robot.java

@@ -1,6 +1,6 @@
-// Copyright (c) FIRST and other WPILib contributors.
+// 版权所有 (c) FIRST 和其他 WPILib 贡献者。
-// Open Source Software; you can modify and/or share it under the terms of
+// 本项目为开源软件；你可以依据项目根目录中的 WPILib BSD license 文件
-// the WPILib BSD license file in the root directory of this project.
+// 修改和/或分享本代码。
 
 package frc.robot;
 import edu.wpi.first.wpilibj.TimedRobot;
@@ -20,11 +20,21 @@ public class Robot extends TimedRobot {
     public void robotPeriodic() {
         CommandScheduler.getInstance().run(); 
         m_robotContainer.tunerPeriodic();
+        m_robotContainer.dashboardPeriodic();
 
     }
 
     @Override
-    public void disabledInit() {}
+    public void disabledInit() {
+        System.out.println("Robot disabledInit called");
+        // 进入 disabled 时强制所有重构后的机构回到安全状态，
+        // 即使上一周期还有手动或自动请求也不能继续动作。
+        m_robotContainer.superstructure.requestIdle();
+        m_robotContainer.shooterSubsystem.stopAll();
+        m_robotContainer.intakeRollerSubsystem.stop();
+        m_robotContainer.intakeRotaterSubsystem.stop();
+        m_robotContainer.conveyorSubsystem.stop();
+    }
 
     @Override
     public void disabledPeriodic() {}
@@ -35,6 +45,7 @@ public class Robot extends TimedRobot {
     @Override
     public void autonomousInit() {
         m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+        System.out.println("Selected auto: " + (m_autonomousCommand == null ? "null" : m_autonomousCommand.getName()));
 
         if (m_autonomousCommand != null) {
             CommandScheduler.getInstance().schedule(m_autonomousCommand);
@@ -42,7 +53,9 @@ public class Robot extends TimedRobot {
     }
 
     @Override
-    public void autonomousPeriodic() {}
+    public void autonomousPeriodic() {
+        m_robotContainer.driveControls.pushDistanceData();
+    }
 
     @Override
     public void autonomousExit() {}

src/main/java/frc/robot/RobotContainer.java

@@ -12,66 +12,93 @@ import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc.robot.Constants.RobotStatus;
+import frc.robot.controls.Autocommand;
 import frc.robot.controls.DriveControls;
-import frc.robot.controls.ShooterControls;
+import frc.robot.controls.OperatorControls;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
+import frc.robot.subsystems.ConveyorSubsystem;
+import frc.robot.subsystems.IntakeRollerSubsystem;
+import frc.robot.subsystems.IntakeRotaterSubsystem;
 import frc.robot.subsystems.LEDSubsystem;
+import frc.robot.subsystems.LimelightSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.subsystems.Superstructure;
 import frc.robot.tuning.ConfigTalonFXMotorTuner;
 import frc.robot.tuning.ConfigTalonFXSMotorTuner;
 import frc.robot.tuning.DriveGainsTuner;
+import frc.robot.utils.GameData2026;
 import frc.robot.utils.LedBindings;
-import frc.robot.controls.ClimberControls;
+
-import frc.robot.subsystems.ClimberSubsystem;
 public class RobotContainer {
 
   /* ====================== */
   /*         手柄            */
   /* ====================== */
 
-  // 只使用一个 Xbox 手柄（端口 0）
+  // 使用一个 Xbox 手柄：0 号口同时负责底盘驾驶和机构操作。
   private final CommandXboxController controller = new CommandXboxController(0);
 
   /* ====================== */
   /*         子系统           */
   /* ====================== */
 
-  // Phoenix TunerX 生成的底盘
+  // 这里继续使用 Phoenix TunerX 生成的底盘，避免改动已经验证过的 swerve 底层。
   public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
-  public final ClimberSubsystem climberSubsystem = new ClimberSubsystem();
   public final ShooterSubsystem shooterSubsystem = new ShooterSubsystem();
+  public final IntakeRollerSubsystem intakeRollerSubsystem = new IntakeRollerSubsystem();
+  public final IntakeRotaterSubsystem intakeRotaterSubsystem = new IntakeRotaterSubsystem();
+  public final ConveyorSubsystem conveyorSubsystem = new ConveyorSubsystem();
+  public final LimelightSubsystem limelightSubsystem = new LimelightSubsystem();
 
-  // LED 子系统
+  // Superstructure 是所有得分机构的唯一调度器。
-  private final LEDSubsystem leds = new LEDSubsystem(0, 72 ,0); // 总长度=72+72         
+  // 控制层和自动命令只能向它请求状态，不能直接命令电机。
+  public final Superstructure superstructure = new Superstructure(
+      shooterSubsystem,
+      intakeRollerSubsystem,
+      intakeRotaterSubsystem,
+      conveyorSubsystem,
+      drivetrain,
+      limelightSubsystem
+  );
+
+  // LED 子系统仍然只负责灯带显示，不参与机构状态判断。
+  private final LEDSubsystem leds = new LEDSubsystem(0, 72, 0); // 总长度 = 72 + 72
 
-  private final RobotStatusManager robotStatusManager = new RobotStatusManager();
   /* ====================== */
-  /*     控制封装/日志        */
+  /*     控制封装/调参        */
   /* ====================== */
 
-  private final DriveControls driveControls = new DriveControls(drivetrain, controller,robotStatusManager );
+  public final DriveControls driveControls = new DriveControls(drivetrain, controller, superstructure);
-  private final ShooterControls shooterControls = new ShooterControls(shooterSubsystem, controller,robotStatusManager);
+
-  private final ClimberControls climberControls = new ClimberControls(climberSubsystem, controller);
+  // 操作员意图层替代旧的射击/进球分离控制类，让复杂动作判断集中留在 Superstructure。
+  private final OperatorControls operatorControls = new OperatorControls(superstructure, shooterSubsystem);
   private final DriveGainsTuner driveGainsTuner = new DriveGainsTuner(drivetrain);
-  private ConfigTalonFXMotorTuner configFlywheelTuner = new ConfigTalonFXMotorTuner(shooterSubsystem.flywheelMotorLeft, "flywheel", Constants.Shooter.flyWheelSlot0Configs);
+  private final GameData2026 gameData2026 = new GameData2026();
-  private ConfigTalonFXMotorTuner configConveyorTuner = new ConfigTalonFXMotorTuner(shooterSubsystem.conveyorMotor, "conveyor", Constants.Shooter.conveyorSlot0Configs);
+  private ConfigTalonFXMotorTuner configFlywheelTuner =
-  private ConfigTalonFXSMotorTuner configBackboardTuner = new ConfigTalonFXSMotorTuner(shooterSubsystem.backboardMotor, "backboard", Constants.Shooter.backboardSlot0Configs);
+      new ConfigTalonFXMotorTuner(shooterSubsystem.leftFlywheel, "flywheel", Constants.Shooter.flyWheelSlot0Configs, false);
+  private ConfigTalonFXSMotorTuner configBackboardTuner =
+      new ConfigTalonFXSMotorTuner(shooterSubsystem.backboardMotor, "backboard", Constants.Shooter.backboardSlot0Configs);
 
   private final SendableChooser<Command> autoChooser;
-  
   private final Trigger enableTrigger = new Trigger(DriverStation::isEnabled);
 
-  
   public RobotContainer() {
+    Pose2d startingPose = Constants.StartingPoints.Red.Point2;
+    drivetrain.resetPose(startingPose);
+    configueSwerve();
+    configueShooter();
+    bingdingLED();
+    configueIntaker();
+    Autocommand.preNameCommands(
+        superstructure,
+        intakeRotaterSubsystem,
+        driveControls,
+        operatorControls
+    );
     SignalLogger.enableAutoLogging(false);
     autoChooser = AutoBuilder.buildAutoChooser();
     SmartDashboard.putData("Auto Chooser", autoChooser);
-    configueSwerve();
-    configueShooter();
-    configueClimber();
-    bingdingLED();
   }
 
   /* ====================== */
@@ -79,84 +106,97 @@ public class RobotContainer {
   /* ====================== */
 
   private void configueSwerve() {
+    enableTrigger.onTrue(driveControls.getAllianceColorCommand());
     Pose2d startingPose = Constants.StartingPoints.Red.Point2;
     drivetrain.resetPose(startingPose);
 
-    // 默认驾驶命令 
+    // 默认驾驶命令说明：
-    /*
+    // 左摇杆 X 控制底盘左右平移。
-     * LeftX 底盘左右
+    // 左摇杆 Y 控制底盘前后平移，越障/壕沟偏移逻辑目前仍沿用旧状态。
-     * LeftY 底盘前后  Bump/Trench偏移未完成
+    // 右摇杆 X 在自动瞄准模式下作为偏移量，在全手动模式下控制底盘旋转。
-     * RightX Aiming偏移    AllTelop模式下 底盘旋转
-     * 
-    */
     drivetrain.setDefaultCommand(driveControls.defaultDriveCommand());
 
-    // Disabled 时进入 idle（防止模块乱动）
+    // 禁用状态下进入空闲命令，防止模块继续响应上一周期的运动请求。
     RobotModeTriggers.disabled().whileTrue(driveControls.idleCommand());
 
-    
+    // Back 键：重置场地坐标系角度。
-    // //左保险：按住刹车
-    // controller.leftBumper()
-    //   .onTrue(Commands.runOnce(driveControls::emergencyStop))
-    //   .whileTrue(driveControls.brakeWhileHeld());
-    
-    // // 右保险：按住加速（Boost）
-    // controller.rightBumper()
-    //     .onTrue(Commands.runOnce(() -> driveControls.setBoostEnabled(true)))
-    //     .onFalse(Commands.runOnce(() -> driveControls.setBoostEnabled(false)));
-
-    // Back：重置场地坐标系角度
     controller.back().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric));
-    
-    // Start：强行使用metaTag2全场定位
-    controller.start().whileTrue(drivetrain.run(drivetrain::forceUsingLimelightMT2));
 
-    //LM 按下时使用机器人坐标系
+    // 预留的左摇杆自动瞄准切换，当前保持注释，避免改变驾驶员已习惯的按键。
-    controller.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming));
+    // controller.leftStick().onTrue(operatorControls.requestAimHubCommand());
-    controller.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
+    // controller.leftStick().onFalse(operatorControls.requestIdleCommand());
 
-    controller.rightStick().and(controller.leftBumper())
-    .onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingBump))
-    .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
-    
-    controller.rightStick().and(controller.rightBumper())
-    .onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingTrench))
-    .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
   }
 
-  private void configueShooter(){
+  private void configueShooter() {
-    shooterSubsystem.setDefaultCommand(shooterControls.defaultShooterCommand());
+    // 射击相关按钮只请求 Superstructure 状态；是否喂球由 Superstructure
-    controller.povLeft().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(-1)));
+    // 根据飞轮速度和底盘瞄准统一判断。
-    controller.povRight().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(1)));
+    // 这些命令 require Superstructure，与自动程序共用需求；
-    controller.povUp().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(100)));
+    // 用 teleop 门控避免自动赛阶段误触直接取消整个 PathPlanner 自动。
-    controller.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-100)));
+    Trigger teleopOnly = RobotModeTriggers.teleop();
+    controller.rightTrigger().and(teleopOnly)
+        .onTrue(operatorControls.requestShootHubCommand())
+        .onFalse(operatorControls.requestIdleCommand());
+    controller.rightBumper().and(teleopOnly)
+        .onTrue(Commands.runOnce(() -> driveControls.setBoostEnabled(true)))
+        .onFalse(Commands.runOnce(() -> driveControls.setBoostEnabled(false)));
+    controller.b().and(teleopOnly)
+        .onTrue(operatorControls.requestEjectCommand())
+        .onFalse(operatorControls.requestIdleCommand());
+    controller.povLeft().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVelocityOffset(-1.0)));
+    controller.povRight().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVelocityOffset(1.0)));
+    controller.povUp().onTrue(Commands.runOnce(() -> operatorControls.adjustBackboardRateOffset(100)));
+    controller.povDown().onTrue(Commands.runOnce(() -> operatorControls.adjustBackboardRateOffset(-100)));
+    controller.start().onTrue(operatorControls.resetBackboardCommand());
   }
-  /* ====================== */
-  /*        LED 绑定          */
-  /* ====================== */
 
   private void bingdingLED() {
     LedBindings.bindModeIndicators(leds);
   }
-  
-  public Command getAutonomousCommand() {
 
+  public Command getAutonomousCommand() {
     return autoChooser.getSelected();
   }
 
   public void tunerPeriodic() {
+    boolean allowMotorOutputTuning = DriverStation.isTest();
+
     driveGainsTuner.periodic();
     configFlywheelTuner.periodic(() -> shooterSubsystem.applyLeftConfigurationToRight());
-    configConveyorTuner.periodic(null);
     configBackboardTuner.periodic();
-    shooterSubsystem.setRightFollowLeft();
+    shooterSubsystem.debugFlywheelVelocityPeriodic(allowMotorOutputTuning);
+    shooterSubsystem.debugShooterConveyorVelocityPeriodic(allowMotorOutputTuning);
+    conveyorSubsystem.debugMainConveyorVoltagePeriodic(allowMotorOutputTuning);
+    shooterSubsystem.debugBackboardTargetPeriodic(allowMotorOutputTuning);
+    shooterSubsystem.setRightFlywheelFollowLeft();
+    shooterSubsystem.setRightConveyorFollowLeft();
   }
 
-  private void configueClimber() {
+  public void dashboardPeriodic() {
-    
+    gameData2026.periodic();
-    enableTrigger.onTrue(climberControls.enableInitCommand());
-
-    climberSubsystem.setDefaultCommand(climberControls.defaultClimberCommand());
   }
 
+  private void configueIntaker() {
+    // 进球相关按钮也只请求 Superstructure 状态；intaker 开赛后默认保持下放。
+    Trigger teleopOnly = RobotModeTriggers.teleop();
+    controller.leftTrigger().and(teleopOnly)
+        .onTrue(operatorControls.requestPassBallCommand())
+        .onFalse(operatorControls.requestIdleCommand());
+
+    controller.leftBumper().and(teleopOnly)
+        .onTrue(operatorControls.requestIntakeCommand())
+        .onFalse(operatorControls.requestIdleCommand());
+
+    Trigger manualRaiseIntaker = controller.y().and(teleopOnly);
+    manualRaiseIntaker
+        .onTrue(operatorControls.requestManualCommand())
+        .whileTrue(operatorControls.manualRaiseIntakeRotaterCommand(intakeRotaterSubsystem))
+        .onFalse(operatorControls.requestIdleCommand());
+
+    Trigger manualLowerIntaker = controller.a().and(teleopOnly);
+    manualLowerIntaker
+        .onTrue(operatorControls.requestManualCommand())
+        .whileTrue(operatorControls.manualLowerIntakeRotaterCommand(intakeRotaterSubsystem))
+        .onFalse(operatorControls.requestIdleCommand());
+  }
 }

src/main/java/frc/robot/RobotStatusManager.java

@@ -1,25 +0,0 @@
-package frc.robot;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import frc.robot.Constants.RobotStatus;
-
-public class RobotStatusManager {
-    private RobotStatus robotStatus;
-    
-    public RobotStatusManager(){
-        this.robotStatus = RobotStatus.AllTelop;
-    }
-
-    public RobotStatus getStatus(){
-        return this.robotStatus;
-    }
-
-    public void setStatus(RobotStatus rs){
-        this.robotStatus = rs;
-    }
-
-    public Command setStatusCommand(RobotStatus rs){
-        return Commands.runOnce(()->setStatus(rs));
-    }
-}

src/main/java/frc/robot/controls/Autocommand.java

@@ -0,0 +1,67 @@
+package frc.robot.controls;
+
+import com.pathplanner.lib.auto.NamedCommands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.Constants;
+import frc.robot.subsystems.IntakeRotaterSubsystem;
+import frc.robot.subsystems.Superstructure;
+
+/**
+ * 注册 PathPlanner 的 NamedCommands。
+ *
+ * 自动路径只请求机器人级动作，不直接设置 shooter、intake 或 conveyor 电机；
+ * 这样自动和手动都会走同一套 Superstructure 状态机。
+ */
+public class Autocommand extends SequentialCommandGroup {
+    public static void preNameCommands(
+        Superstructure superstructure,
+        IntakeRotaterSubsystem intakeRotater,
+        DriveControls driveControls,
+        OperatorControls operatorControls
+    ) {
+        NamedCommands.registerCommand("start_intake", startIntake(superstructure));
+        NamedCommands.registerCommand("start_intake_and_conveyor", startIntake(superstructure));
+        NamedCommands.registerCommand("stop_intake", idle(superstructure));
+        NamedCommands.registerCommand("stop_intake_", idle(superstructure));
+        NamedCommands.registerCommand("stop_intake_and_conveyor", idle(superstructure));
+        NamedCommands.registerCommand("init", init(superstructure));
+        NamedCommands.registerCommand("intaker_down", init(superstructure));
+        NamedCommands.registerCommand("intaker_up", raiseIntaker(intakeRotater));
+        NamedCommands.registerCommand("shoot_hub", shootHub(superstructure));
+        NamedCommands.registerCommand("reset_backboard0", operatorControls.resetBackboard0Command());
+        NamedCommands.registerCommand("AutoAimForShoot", autoAimForShoot(driveControls));
+        NamedCommands.registerCommand("AutoAim", autoAimForShoot(driveControls));
+        NamedCommands.registerCommand("Shake", driveControls.shakeCommand());
+    }
+
+    public static Command startIntake(Superstructure superstructure) {
+        return superstructure.requestIntakeCommand();
+    }
+
+    public static Command idle(Superstructure superstructure) {
+        return superstructure.requestIdleCommand();
+    }
+
+    public static Command init(Superstructure superstructure) {
+        return superstructure.requestInitCommand();
+    }
+
+    public static Command raiseIntaker(IntakeRotaterSubsystem intakeRotater) {
+        return intakeRotater.raiseForMatchCommand();
+    }
+
+    public static Command shootHub(Superstructure superstructure) {
+        return Commands.sequence(
+            superstructure.requestShootHubCommand(),
+            Commands.waitSeconds(Constants.Superstructure.shootTimeoutSeconds),
+            superstructure.requestIdleCommand()
+        );
+    }
+    public static Command autoAimForShoot(DriveControls driveControls) {
+        return driveControls.autoAimForShootCommand();
+    }
+
+}

src/main/java/frc/robot/controls/ClimberControls.java

@@ -1,53 +0,0 @@
-package frc.robot.controls;
-
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import frc.robot.subsystems.ClimberSubsystem;
-
-public class ClimberControls {
-
-    private final ClimberSubsystem climber;
-    private final CommandXboxController controller;
-
-    private final NetworkTable table;
-
-    public ClimberControls(ClimberSubsystem climber, CommandXboxController controller) {
-        this.climber = climber;
-        this.controller = controller;
-        this.table = NetworkTableInstance.getDefault().getTable("ClimberControls");
-    }
-
-    public Command defaultClimberCommand() {
-        return Commands.run(()->{
-            //climber.climbUp();
-                // 优先级：如果同时按，优先上升（你也可以反过来）
-                if(controller.y().getAsBoolean()&&controller.x().getAsBoolean()){
-                    climber.zeroEncoder();
-                }
-                else if (controller.y().getAsBoolean()) {
-                    climber.climbUp();
-                } else if (controller.x().getAsBoolean()) {
-                    climber.climbDownNonStop();
-                } else {
-                    // 松开：记录当前位置 + 锁死
-                    climber.captureHoldPosition();
-                    climber.holdPosition();
-                }
-                
-
-                // 调试用
-                table.getEntry("xHeld_up").setBoolean(controller.x().getAsBoolean());
-                table.getEntry("yHeld_down").setBoolean(controller.y().getAsBoolean());
-        }, climber);
-
-    }
-
-    public Command enableInitCommand() {
-        return Commands.run(climber::initialize, climber)
-                .withTimeout(0.5)
-                .andThen(Commands.runOnce(climber::zeroEncoder, climber));
-    }
-}

src/main/java/frc/robot/controls/DriveControls.java

@@ -11,48 +11,60 @@ import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Constants;
-import frc.robot.RobotStatusManager;
 import frc.robot.Constants.DriveMode;
-import frc.robot.Constants.RobotStatus;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
+import frc.robot.subsystems.Superstructure;
+import frc.robot.subsystems.Superstructure.WantedState;
+
+import java.util.Optional;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.Timer; 
 
 public class DriveControls {
+
+    public boolean isRedAlliance;
+
     private final CommandSwerveDrivetrain drivetrain;
     private final CommandXboxController driver;
 
     private final double maxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
     private final double maxAngularRate = RotationsPerSecond.of(0.75).in(edu.wpi.first.units.Units.RadiansPerSecond);
 
-    // 默认速度比例（平时开车）
+    // 默认速度比例，普通驾驶使用。
-    private double normalDriveScale = 0.3;
+    private double normalDriveScale = 0.6;
-    private double normalTurnScale  = 0.7;
+    private double normalTurnScale  = 0.8;
 
-    // 加速模式比例（按住 RB）
+    // 加速模式比例，按住 RB 时使用。
     private double boostDriveScale = 1.0;
     private double boostTurnScale  = 1.0;
+    // 自动瞄准时降低平移速度。
+    private double slowDriveScale = 0.3;
 
     private boolean boostEnabled = false;
 
-    private final SlewRateLimiter vxLimiter = new SlewRateLimiter(5.0);   // m/s^2 等效（调）
+    private final SlewRateLimiter vxLimiter = new SlewRateLimiter(3.5);   // m/s^2 等效加速度限制
     private final SlewRateLimiter vyLimiter = new SlewRateLimiter(5.0);
-    private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(9.0); // rad/s^2 等效（调）
+    private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(9.0); // rad/s^2 等效角加速度限制
 
-    // ====== 类成员变量里新增：记录上一次输出 ======
+    // 记录上一周期输出，用于只限制加速、不限制减速。
     private double lastVx = 0.0;
     private double lastVy = 0.0;
     private double lastOmega = 0.0;
 
     private NetworkTable autoControlNetworkTable;
     
-    private final RobotStatusManager robotStatusManager;
+    private final Superstructure superstructure;
     public void setBoostEnabled(boolean enabled) {
         boostEnabled = enabled;
     }
 
-    // 新增：当前是否使用场地坐标系（默认 true）
+    // 当前是否使用场地坐标系，默认启用。
     private boolean fieldCentricEnabled = true;
 
     private final SwerveRequest.FieldCentric fieldCentric =
@@ -74,10 +86,10 @@ public class DriveControls {
         .withRotationalDeadband(maxAngularRate * 0.05)
         .withDriveRequestType(DriveRequestType.Velocity);
     
-    public DriveControls(CommandSwerveDrivetrain drivetrain, CommandXboxController driver, RobotStatusManager rsm) {
+    public DriveControls(CommandSwerveDrivetrain drivetrain, CommandXboxController driver, Superstructure superstructure) {
         this.drivetrain = drivetrain;
         this.driver = driver;
-        this.robotStatusManager = rsm;
+        this.superstructure = superstructure;
         autoControlNetworkTable = NetworkTableInstance.getDefault().getTable("AutoControl");
     }
     
@@ -89,16 +101,16 @@ public class DriveControls {
         return fieldCentricEnabled;
     }
 
-    /** 底盘的默认驾驶命令（teleop 期间持续执行） */
+    /** 底盘默认驾驶命令，在 teleop 期间持续执行。 */
     public Command defaultDriveCommand() {
         return drivetrain.applyRequest(() -> {
-        SmartDashboard.putString("robotStatus",robotStatusManager.getStatus().name());
+        WantedState wantedState = superstructure.getWantedState();
+        SmartDashboard.putString("wantedState", wantedState.name());
         double driveScale = boostEnabled ? boostDriveScale : normalDriveScale;
         double turnScale  = boostEnabled ? boostTurnScale  : normalTurnScale;
         double vx = -driver.getLeftY() * maxSpeed * driveScale;
         double vy = -driver.getLeftX() * maxSpeed * driveScale;
         double vomega = -driver.getRightX() * maxAngularRate * turnScale;
-        // ====== execute 内替换 limiter：只限加速、不限减速 ======
         double limitedVx = limitAccelOnly(vx, lastVx, vxLimiter);
         double limitedVy = limitAccelOnly(vy, lastVy, vyLimiter);
         double limitedOmega = limitAccelOnly(vomega, lastOmega, omegaLimiter);
@@ -111,108 +123,40 @@ public class DriveControls {
         vy = limitedVy;
         vomega = limitedOmega;
 
-        double[] distanceAndRotation = calculateDistanceAndRotationToHub();
+        double[] hubAim = calculateDistanceAndRotationToHub();
-        // 将距离值写入 NetworkTable
+        double[] passAim = calculateDistanceAndRotationToPassBall();
-        autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceAndRotation[0]);
+        autoControlNetworkTable.getEntry("distanceToHub").setDouble(hubAim[0]);
-        autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(distanceAndRotation[1]);
+        autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(hubAim[1]);
+        autoControlNetworkTable.getEntry("distanceToPassball").setDouble(passAim[0]);
+        autoControlNetworkTable.getEntry("angleDifferenceToPassball").setDouble(passAim[1]);
 
-        switch (robotStatusManager.getStatus()) {
+        double[] activeAim = wantedState == WantedState.PASS_BALL ? passAim : hubAim;
-            case Stopped:
+        drivetrain.setTargetHeading(
-                vx = 0;
+            drivetrain.getRotation().plus(Rotation2d.fromDegrees(activeAim[1])));
-                vy = 0;
+
-                vomega = 0;
+        switch (wantedState) {
+            case SHOOT_HUB:
+            case AIM_HUB:
+                vomega = calculateRotationSpeedFromRotationAngle(hubAim[1]);
+                autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
+                vx = -driver.getLeftY() * maxSpeed * slowDriveScale;
+                vy = -driver.getLeftX() * maxSpeed * slowDriveScale;
                 break;
-            case PassingBall:
+            case PASS_BALL:
-                vomega = calculateRotationSpeedFromRotationAngle(calculateDistanceAndRotationToPassBall()[1]);
+                vomega = calculateRotationSpeedFromRotationAngle(passAim[1]);
                 autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
                 break;
-
+            default:
-            case AutoAimming:
-                vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]);
-                autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
-                break;
-            case AllTelop:
                 autoControlNetworkTable.getEntry("autoRotationRate").setDouble(Double.NaN);
                 break;
-            case Climbing:
+        }
-                break;
-            case CrossingBump:
-                double differenceBump = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.bumpY[0], Constants.AutoPositioning.bumpY[1]);
-                if (Math.abs(differenceBump) < 0.1) {
-                    vy = 0;
-                } else {
-                    double raw = differenceBump * Constants.AutoPositioning.autoPositioningkP;
-                    vy = Math.copySign(
-                        Math.max(Math.abs(raw), 0.4),
-                        raw
-                    );
-                }
-                autoControlNetworkTable.getEntry("BumpDifference").setDouble(differenceBump);
-
-                Rotation2d BumpcurrentAngle = drivetrain.getState().Pose.getRotation();
-                double BumpcurrentRad = BumpcurrentAngle.getRadians();
-                double targetA = Math.toRadians(45);
-                double targetB = Math.toRadians(-45);
-                // 计算误差
-                double errorA = targetA - BumpcurrentRad;
-                double errorB = targetB - BumpcurrentRad;
-                // 角度归一化到 [-pi, pi]
-                errorA = Math.atan2(Math.sin(errorA), Math.cos(errorA));
-                errorB = Math.atan2(Math.sin(errorB), Math.cos(errorB));
-                // 选误差绝对值更小的那个
-                double chosenError = (Math.abs(errorA) <= Math.abs(errorB)) ? errorA : errorB;
-
-                if (Math.abs(chosenError) < Math.toRadians(2.0)) {
-                    vomega = 0.0;
-                } else {
-                    vomega = chosenError * Constants.AutoPositioning.TurningkP;
-                }
-                // 限制最大角速度
-                vomega = Math.max(-maxAngularRate, Math.min(maxAngularRate, vomega));
-
-                //vomega = calculateRotationSpeedForBump();
-                break;
-            case CrossingTrench:
-                double differenceTrench = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.trenchY[0], Constants.AutoPositioning.trenchY[1]);
-                if (Math.abs(differenceTrench) < 0.1) {
-                    vy = 0;
-                } else {
-                    double raw = differenceTrench * Constants.AutoPositioning.autoPositioningkP;
-                    vy = Math.copySign(
-                        Math.max(Math.abs(raw), 0.4),
-                        raw
-                    );
-                }
-                autoControlNetworkTable.getEntry("TrenchDifference").setDouble(differenceTrench);
-                Rotation2d currentAngle = drivetrain.getState().Pose.getRotation();
-                double currentRad = currentAngle.getRadians();
-                double targetRad = Math.toRadians(Constants.AutoPositioning.TrenchtargetAngle);
-                double TrenchAngleerror = targetRad - currentRad;
-                if (Math.abs(TrenchAngleerror) < Math.toRadians(1.0)) {
-                    vomega = 0.0;
-                } else {
-                    vomega = TrenchAngleerror * Constants.AutoPositioning.TurningkP;
-                }
-                // 限幅（防止转太快）
-                vomega = Math.max(-maxAngularRate, Math.min(maxAngularRate, vomega));
-
-                //vomega = calculateRotationSpeedForTrench();
-                break;
-        }        
 
         autoControlNetworkTable.getEntry("ActualVX").setDouble(vx);
         autoControlNetworkTable.getEntry("ActualVY").setDouble(vy);
         autoControlNetworkTable.getEntry("ActualVOmega").setDouble(vomega);
-        
-        boolean isAutoLike =
-            robotStatusManager.getStatus() == RobotStatus.CrossingBump ||
-            robotStatusManager.getStatus() == RobotStatus.CrossingTrench;
 
-        if (fieldCentricEnabled || isAutoLike) {
+        if (fieldCentricEnabled) {
-
+        return fieldCentric
-    // 自动状态用 BlueAlliance（不翻转），手柄 field-centric 仍用原 fieldCentric（OperatorPerspective）
-        var chosen = isAutoLike ? fieldCentricAuto : fieldCentric;
-        return chosen
                 .withVelocityX(vx)
                 .withVelocityY(vy)
                 .withRotationalRate(vomega);
@@ -222,60 +166,26 @@ public class DriveControls {
                 .withVelocityY(vy)
                 .withRotationalRate(vomega);
         }
-        }, () -> fieldCentricEnabled || 
+        }, () -> fieldCentricEnabled ? DriveMode.FIELD_CENTRIC : DriveMode.ROBOT_CENTRIC);
-            robotStatusManager.getStatus() == RobotStatus.CrossingBump || 
-            robotStatusManager.getStatus() == RobotStatus.CrossingTrench
-             ? DriveMode.FIELD_CENTRIC : DriveMode.ROBOT_CENTRIC);
     }
-
+    // 返回当前位置到两个目标点中更近目标的位移。
-    /**
+    // 返回到更近目标的带符号位移。
-     * 
-         * 输出移动到距离两个目标之间较近的那个点的位移
-         * @return 位移 有正负
-         */
     public double calculateDifferenceToTwoTarget(double current, double target1, double target2){
           double x1 = target1 - current;
           double x2 = target2 - current;
           return Math.abs(x1) < Math.abs(x2) ? x1 : x2;
     }
-    public double calculateRotationSpeedForTrench(){
-        Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
-        double currentAngle = currentPose.getRotation().getDegrees();
-        double targetAngle = Constants.AutoPositioning.autoRotationForTrenchTargetDegrees; // 转换为度
-        double angleDifference = targetAngle - currentAngle;
-        // Normalize angle difference to the range [-180, 180]
-        if (angleDifference > 180) {
-            angleDifference -= 360;
-        } else if (angleDifference < -180) {
-            angleDifference += 360;
-        }
-        return calculateRotationSpeedFromRotationAngle(angleDifference);
-    }
-    public double calculateRotationSpeedForBump(){
-        Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
-        double currentAngle = currentPose.getRotation().getDegrees();
-        double targetAngle = Constants.AutoPositioning.autoRotationForBumpTargetDegrees; // 转换为度
-        double angleDifference = targetAngle - currentAngle;
-        // Normalize angle difference to the range [-180, 180]
-        if (angleDifference > 180) {
-            angleDifference -= 360;
-        } else if (angleDifference < -180) {
-            angleDifference += 360;
-        }
-        return calculateRotationSpeedFromRotationAngle(angleDifference);
-    }
-    // Calculate distance and rotation (angle difference)
     public double[] calculateDistanceAndRotationToHub() {
-        Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
+        Pose2d currentPose = drivetrain.getState().Pose; // 当前机器人位姿
-        double targetX = Constants.Field.RedHubPositionX;  // 目标 X 坐标
+        double targetX = isRedAlliance ? Constants.Field.RedHubPositionX : Constants.Field.BlueHubPositionX;
-        double targetY = Constants.Field.RedHubPositionY;  // 目标 Y 坐标
+        double targetY = isRedAlliance ? Constants.Field.RedHubPositionY : Constants.Field.BlueHubPositionY;
 
-        // 计算目标角度（相对于场地坐标系）
+        // 计算目标角度，相对于场地坐标系。
         double deltaX = targetX - currentPose.getX();
         double deltaY = targetY - currentPose.getY();
-        double targetAngle = Math.toDegrees(Math.atan2(deltaY, deltaX)); // 转换为度
+        double targetAngle = Math.toDegrees(Math.atan2(deltaY, deltaX)); // 转换为角度
 
-        // 当前机器人角度
+        // 当前机器人朝向。
         double currentAngle = currentPose.getRotation().getDegrees();
         double angleDifference = targetAngle - currentAngle;
         // Normalize angle difference to the range [-180, 180]
@@ -285,27 +195,26 @@ public class DriveControls {
             angleDifference += 360;
         }
 
-        // 计算距离
+        // 计算距离。
         double distance = Math.sqrt(deltaX * deltaX + deltaY * deltaY);
-                // 计算距离和旋转角度
+        // 返回距离和旋转角度误差。
         double[] distanceAndRotation = new double[]{distance, angleDifference};
 
-        // 返回距离和角度差
+        // 距离单位为米，角度单位为度。
         return distanceAndRotation;
     }
     public double[] calculateDistanceAndRotationToPassBall() {
-        Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
+        Pose2d currentPose = drivetrain.getState().Pose; // 当前机器人位姿
-        double targetX = Constants.Field.PassingBallPosX;  // 目标 X 坐标
+        double targetX = isRedAlliance ? Constants.Field.RedPassingBallPosX : Constants.Field.BluePassingBallPosX;  // 传球目标 X 坐标
-        double targetY1 = Constants.Field.PassingBallPosY1;  // 目标 Y 坐标
+        double targetY1 = Constants.Field.PassingBallPosY1;  // 传球目标 Y 坐标
-        double targetY2 = Constants.Field.PassingBallPosY2;  // 目标 Y 坐标
+        double targetY2 = Constants.Field.PassingBallPosY2;  // 传球目标 Y 坐标
        
-
+        // 计算目标角度，相对于场地坐标系。
-        // 计算目标角度（相对于场地坐标系）
         double deltaX = targetX - currentPose.getX();
         double deltaY = calculateDifferenceToTwoTarget(currentPose.getY(), targetY1, targetY2);
-        double targetAngle = Math.toDegrees(Math.atan2(deltaY, deltaX)); // 转换为度
+        double targetAngle = Math.toDegrees(Math.atan2(deltaY, deltaX)); // 转换为角度
 
-        // 当前机器人角度
+        // 当前机器人朝向。
         double currentAngle = currentPose.getRotation().getDegrees();
         double angleDifference = targetAngle - currentAngle;
         // Normalize angle difference to the range [-180, 180]
@@ -315,16 +224,15 @@ public class DriveControls {
             angleDifference += 360;
         }
 
-        // 计算距离
+        // 计算距离。
         double distance = Math.sqrt(deltaX * deltaX + deltaY * deltaY);
-                // 计算距离和旋转角度
+        // 返回距离和旋转角度误差。
         double[] distanceAndRotation = new double[]{distance, angleDifference};
 
-        // 返回距离和角度差
+        // 距离单位为米，角度单位为度。
         return distanceAndRotation;
     }
 
-    // Calculate rotation speed from rotation angle difference
     public double calculateRotationSpeedFromRotationAngle(double angleDifference) {
         double calDifference = angleDifference;
 
@@ -336,29 +244,43 @@ public class DriveControls {
 
         // If angle difference is within threshold, don't rotate
         if (Math.abs(calDifference) > 1.0) {
-            return calDifference  * Constants.AutoPositioning.autoRotationkP;
+            double rotationRate = calDifference * Constants.AutoPositioning.autoRotationkP;
+            if (Math.abs(rotationRate) < 0.24) {
+                return Math.copySign(0.24, rotationRate);
+            }
+            return rotationRate;
         } else {
             return 0;
         }
     }
+    
+    private double calculatevomegaFromRotationAngle(double error) {
+        double vomega;
+        if (Math.abs(error) < Math.toRadians(Constants.AutoPositioning.autoPositioningAngleError)) {
+                vomega = 0.0;
+            } else {
+                vomega = error * Constants.AutoPositioning.TurningkP;
+            }
+            // 限制最大角速度。
+            vomega = Math.max(-maxAngularRate, Math.min(maxAngularRate, vomega));
+        return vomega;
+    }
 
-
+    /**
-
+     * 只限制加速，也就是输出幅值变大时；不限制减速。
-        /**
+     * 这样松杆或刹车能更及时，同时 boost 仍能抑制电流冲击。
-     * 只限制“加速”（幅值变大），不限制“减速”（幅值变小）
-     * 这样松杆/刹车会更及时，同时 boost 仍然能抑制电流冲击
      */
     private double limitAccelOnly(double target, double last, SlewRateLimiter limiter) {
-        // 如果正在减速（幅值变小），直接放行到目标值
+        // 如果正在减速，直接放行到目标值。
         if (Math.abs(target) < Math.abs(last)) {
-        limiter.reset(target);  // 同步 limiter 内部状态，防止下一次突变
+            limiter.reset(target);  // 同步 limiter 内部状态，防止下一次突跳。
-        return target;
+            return target;
         }
-        // 否则是加速，使用 limiter
+        // 否则是加速，使用 limiter。
         return limiter.calculate(target);
     }
 
-        /** 紧急停止：清零限速器，让速度输出立刻归零 */
+    /** 紧急停止：清零限速器，让速度输出立刻归零。 */
     public void emergencyStop() {
         vxLimiter.reset(0.0);
         vyLimiter.reset(0.0);
@@ -395,5 +317,117 @@ public class DriveControls {
     public double getMaxAngularRate() {
         return maxAngularRate;
     }
+    public void setTeamColors() {
+        Optional<Alliance> ally = DriverStation.getAlliance(); // 获取联盟颜色
+        if (ally.isPresent()) {
+            // 根据联盟颜色更新自动瞄准目标。
+            if (ally.get() == Alliance.Red) {
+                isRedAlliance = true; // 红队
+            } else if (ally.get() == Alliance.Blue) {
+                isRedAlliance = false; // 蓝队
+            }
+        } else {
+            isRedAlliance = true;
+        }
+        if (isRedAlliance) {
+            System.out.println("Robot is on the Red Alliance.");
+        } else {
+            System.out.println("Robot is on the Blue Alliance.");
+        }
+    }
+    public Command getAllianceColorCommand() {
+        return Commands.runOnce(this::setTeamColors);
+    }
 
+    // DriveControls.java
+    public Command autoAimForShootCommand() {
+        return drivetrain.applyRequest(() -> {
+            double[] hubAim = calculateDistanceAndRotationToHub();
+            double distanceToHub = hubAim[0];
+            double angleDifferenceToHub = hubAim[1];
+
+            autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceToHub);
+            autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(angleDifferenceToHub);
+
+            drivetrain.setTargetHeading(
+                drivetrain.getRotation().plus(Rotation2d.fromDegrees(angleDifferenceToHub))
+            );
+
+            double vomega = calculateRotationSpeedFromRotationAngle(angleDifferenceToHub);
+
+            autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
+            autoControlNetworkTable.getEntry("ActualVX").setDouble(0.0);
+            autoControlNetworkTable.getEntry("ActualVY").setDouble(0.0);
+            autoControlNetworkTable.getEntry("ActualVOmega").setDouble(vomega);
+
+            return fieldCentric
+                .withVelocityX(0.0)
+                .withVelocityY(0.0)
+                .withRotationalRate(vomega);
+        }, () -> DriveMode.FIELD_CENTRIC)
+        .withTimeout(Constants.Superstructure.shootTimeoutSeconds)
+        .andThen(new InstantCommand(() -> drivetrain.stop(), drivetrain));
+    }
+
+    public Command shakeCommand(double amplitude, double switchPeriod, double totalTime) {
+    Timer timer = new Timer();
+
+    return Commands.sequence(
+        new InstantCommand(timer::restart),
+        drivetrain.applyRequest(() -> {
+            double t = timer.get();
+            int phase = ((int) (t / switchPeriod)) % 4;
+
+            double vx = 0.0;
+            double vy = 0.0;
+            double[] distanceAndRotation = calculateDistanceAndRotationToHub();
+            double vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]);
+
+
+            switch (phase) {
+                case 0:
+                    vx = amplitude;   // 前
+                    vy = 0.0;
+                    break;
+                case 1:
+                    vx = 0.0;
+                    vy = amplitude;   // 左
+                    break;
+                case 2:
+                    vx = -amplitude;  // 后
+                    vy = 0.0;
+                    break;
+                case 3:
+                    vx = 0.0;
+                    vy = -amplitude;  // 右
+                    break;
+            }
+
+            return robotCentric
+                .withVelocityX(vx)
+                .withVelocityY(vy)
+                .withRotationalRate(vomega);
+        }, () -> DriveMode.ROBOT_CENTRIC).withTimeout(totalTime),
+        new InstantCommand(() -> {
+            timer.stop();
+            drivetrain.stop();
+        }, drivetrain)
+    );
+}
+
+        public Command shakeCommand() {
+            return shakeCommand(0.3, 0.15, 5.0);
+        }
+
+    public void pushDistanceData(){
+        double[] distanceAndRotation = calculateDistanceAndRotationToHub();
+        // 将距离数据写入 NetworkTable。
+        autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceAndRotation[0]);
+        autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(distanceAndRotation[1]);
+        // 自动阶段默认驾驶命令不运行，这里同样保持目标朝向最新，
+        // 让自动程序里的射击门控 isAimed() 有效。
+        drivetrain.setTargetHeading(
+            drivetrain.getRotation().plus(Rotation2d.fromDegrees(distanceAndRotation[1])));
+    }
+    
 }

src/main/java/frc/robot/controls/IntakerControls.java

@@ -1,71 +0,0 @@
-package frc.robot.controls;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import frc.robot.subsystems.IntakerSubsystem;
-
-public class IntakerControls {
-    private final IntakerSubsystem intakerSubsystem;
-    private final CommandXboxController controller;
-
-    private static final double ROTATER_STEP_PER_CYCLE = 0.00; // 需要你按实际速度调：越大越快（fromAI）
-
-    public IntakerControls(IntakerSubsystem intakerSubsystem, CommandXboxController controller) {
-        this.intakerSubsystem = intakerSubsystem;
-        this.controller = controller;
-    }
-
-    public void configureBindings() {
-
-        //B.A：按住下降 + getter转；松开后 rotater保持当前位置，getter继续转
-        controller.a()
-            .whileTrue(
-                Commands.run(() -> {
-                    // rotater下降：范围会被 subsystem 限制
-                    double cur = intakerSubsystem.getIntakerotaterPosition();
-                    intakerSubsystem.setIntakerRotaterPosition(cur - ROTATER_STEP_PER_CYCLE);
-
-                    // getter 开
-                    intakerSubsystem.setIntakeGetterOn(true);
-                }, intakerSubsystem)
-            )
-            .onFalse(
-                Commands.runOnce(() -> {
-                    // 松：把当前位置作为target保持
-                    double cur = intakerSubsystem.getIntakerotaterPosition();
-                    intakerSubsystem.setIntakerRotaterPosition(cur);
-
-                    // gtter继续转
-                    intakerSubsystem.setIntakeGetterOn(true);
-                }, intakerSubsystem)
-            );
-
-        // === B.B：按住上升+getter停；松开后rotater保持当前位置
-        controller.b()
-            .whileTrue(
-                Commands.run(() -> {
-                    // rotater升
-                    double cur = intakerSubsystem.getIntakerotaterPosition();
-                    intakerSubsystem.setIntakerRotaterPosition(cur + ROTATER_STEP_PER_CYCLE);
-
-                    // getter关
-                    intakerSubsystem.setIntakeGetterOn(false);
-                }, intakerSubsystem)
-            )
-            .onFalse(
-                Commands.runOnce(() -> {
-                    // 松：保持当前位置
-                    double cur = intakerSubsystem.getIntakerotaterPosition();
-                    intakerSubsystem.setIntakerRotaterPosition(cur);
-
-                    // getter仍然停止
-                    intakerSubsystem.setIntakeGetterOn(false);
-                }, intakerSubsystem)
-            );
-    }
-
-    public Command defaultIntakerCommand() {
-        return Commands.none();
-    }
-}

src/main/java/frc/robot/controls/OperatorControls.java

@@ -0,0 +1,119 @@
+package frc.robot.controls;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.Constants;
+import frc.robot.subsystems.IntakeRollerSubsystem;
+import frc.robot.subsystems.IntakeRotaterSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
+import frc.robot.subsystems.Superstructure;
+
+/**
+ * 上操作员手柄的“意图层”。
+ *
+ * 这里刻意不直接设置任何机构电机。按钮、自动辅助命令和调参快捷键都会
+ * 转换成 Superstructure 请求，由 Superstructure 统一处理多机构动作顺序。
+ */
+public class OperatorControls {
+    private final Superstructure superstructure;
+    private final ShooterSubsystem shooterSubsystem;
+
+    public OperatorControls(Superstructure superstructure, ShooterSubsystem shooterSubsystem) {
+        this.superstructure = superstructure;
+        this.shooterSubsystem = shooterSubsystem;
+    }
+
+    public Command requestIdleCommand() {
+        return superstructure.requestIdleCommand();
+    }
+
+    public Command requestIntakeCommand() {
+        return superstructure.requestIntakeCommand();
+    }
+
+    public Command requestShootHubCommand() {
+        return superstructure.requestShootHubCommand();
+    }
+
+    public Command requestAimHubCommand() {
+        return superstructure.requestAimHubCommand();
+    }
+
+    public Command requestPassBallCommand() {
+        return superstructure.requestPassBallCommand();
+    }
+
+    public Command requestEjectCommand() {
+        return superstructure.requestEjectCommand();
+    }
+
+    public Command requestManualCommand() {
+        return superstructure.requestManualCommand();
+    }
+
+    public void adjustFlywheelVoltageOffset(double offset) {
+        superstructure.adjustFlywheelVoltageOffset(offset);
+    }
+
+    public void adjustFlywheelVelocityOffset(double offset) {
+        superstructure.adjustFlywheelVelocityOffset(offset);
+    }
+
+    public void adjustBackboardRateOffset(double offset) {
+        superstructure.adjustBackboardPositionOffset(offset);
+    }
+
+    public void resetOffsets() {
+        superstructure.resetOffsets();
+    }
+
+    public Command resetBackboardCommand() {
+        return Commands.runOnce(superstructure::resetBackboardEncoder, shooterSubsystem);
+    }
+
+    public Command manualRaiseIntakeRotaterCommand(IntakeRotaterSubsystem intakeRotater) {
+        return Commands.runEnd(intakeRotater::manualRaise, intakeRotater::stop, intakeRotater);
+    }
+
+    public Command manualRaiseIntakeWithRollerCommand(
+        IntakeRotaterSubsystem intakeRotater,
+        IntakeRollerSubsystem intakeRoller
+    ) {
+        return Commands.runEnd(
+            () -> {
+                intakeRotater.manualRaise();
+                intakeRoller.intake();
+            },
+            () -> {
+                intakeRotater.stop();
+                intakeRoller.stop();
+            },
+            intakeRotater,
+            intakeRoller
+        );
+    }
+
+    public Command manualLowerIntakeRotaterCommand(IntakeRotaterSubsystem intakeRotater) {
+        return Commands.runEnd(intakeRotater::manualLower, intakeRotater::stop, intakeRotater);
+    }
+
+    public Command lowerIntakeRotaterForMatchCommand(IntakeRotaterSubsystem intakeRotater) {
+        return intakeRotater.lowerForMatchCommand();
+    }
+
+    // PathPlanner 自动射击辅助：先预备到所有射击条件满足，再请求喂球，
+    // 最后回到空闲状态。真正的“是否能射”判断仍然放在 Superstructure。
+    public Command autoShootToHubCommand() {
+        return Commands.sequence(
+            superstructure.requestShootHubCommand(),
+            Commands.waitSeconds(Constants.Superstructure.shootTimeoutSeconds),
+            superstructure.requestIdleCommand()
+        );
+    }
+
+    // 保留旧背板回零动作，方便调参和现场恢复；
+    // 具体电机操作在 ShooterSubsystem 内实现，这里只转发命令。
+    public Command resetBackboard0Command() {
+        return shooterSubsystem.homeBackboardCommand();
+    }
+}

src/main/java/frc/robot/controls/ShooterControls.java

@@ -1,222 +0,0 @@
-package frc.robot.controls;
-
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import frc.robot.Constants;
-import frc.robot.RobotStatusManager;
-import frc.robot.Constants.Intaker;
-import frc.robot.subsystems.IntakerSubsystem;
-import frc.robot.subsystems.ShooterSubsystem;
-
-public class ShooterControls {
-    private final ShooterSubsystem shooterSubsystem;
-    private final CommandXboxController controller;
-
-    private double flywheelSpeedOffset = 0.0;
-    private double backboardPositionOffset = 0.0;
-    private NetworkTable shooterControlTable;
-    private double conveyorSpeed = 0.0;
-    private double flywheelSpeed = 0;
-    private double backboardPosition = 0;
-    private final RobotStatusManager robotStatusManager;
-    public ShooterControls(ShooterSubsystem shooterSubsystem, CommandXboxController controller,RobotStatusManager rsm) {
-        this.shooterSubsystem = shooterSubsystem;
-        this.controller = controller;
-        this.robotStatusManager = rsm;
-        shooterControlTable = NetworkTableInstance.getDefault().getTable("ShooterControl");
-    }
-    
-    public Command defaultShooterCommand() {
-        return Commands.run(() -> {
-            switch (robotStatusManager.getStatus()) {
-                case Stopped:
-                    shooterSubsystem.stopMotors();
-                    break;
-                case AllTelop:
-                case PassingBall:
-                    var xP = shooterSubsystem.getDistance();
-                    //xP = Math.max(1.2, Math.min(4.5, xP));
-                    flywheelSpeed = 0.6956596811733782*xP*xP*xP*xP*xP*xP*xP-14.164408860183267*xP*xP*xP*xP*xP*xP+119.7115375240918*xP*xP*xP*xP*xP-543.0984876677477*xP*xP*xP*xP+1425.1928543240738*xP*xP*xP-2156.199296490315*xP*xP+1736.568139600292*xP-516.097598057351;
-                    
-                    flywheelSpeed += flywheelSpeedOffset;
-                    backboardPosition = xP*xP;//todo:实测传球的时候的背板位置 打表
-                    backboardPosition += backboardPositionOffset;
-                    conveyorSpeed = Constants.Shooter.conveyorSpeed;
-                    if (controller.getRightTriggerAxis() < 0.1) {
-                        flywheelSpeed = 0;
-                        conveyorSpeed = 0;
-                    }
-                    if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.7){
-                        conveyorSpeed = 0;
-                    }
-                    // flywheelSpeed = calculateFlywheelSpeedOnlywithOffset(60);
-                    // double flywheelSpeed = calculateFlywheelSpeed(0);
-                    // double BackboardPosition = calculateBackboardPosition(0);
-                    shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed);
-                    shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed);
-                    shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition);
-                    shooterSubsystem.setFlywheelSpeedByRPS(flywheelSpeed);
-                    shooterSubsystem.setConveyorSpeedByRPS(conveyorSpeed);
-                    shooterSubsystem.setBackboardPosition(backboardPosition);  
-                    if(shooterSubsystem.isBackboardAtTarget()){
-                        shooterSubsystem.backboardMotor.set(0);
-                    }else{
-                        shooterSubsystem.outputBackboard();
-                    }  
-                    break;
-                case AutoAimming:
-                
-                    var x = shooterSubsystem.getDistance();
-                    x = Math.max(1.2, Math.min(4.5, x));
-                    flywheelSpeed = 0.6956596811733782*x*x*x*x*x*x*x-14.164408860183267*x*x*x*x*x*x+119.7115375240918*x*x*x*x*x-543.0984876677477*x*x*x*x+1425.1928543240738*x*x*x-2156.199296490315*x*x+1736.568139600292*x-516.097598057351;
-                    flywheelSpeed += flywheelSpeedOffset;
-                    backboardPosition = -122.11889023593778*x*x*x*x*x*x*x+2453.385583841308*x*x*x*x*x*x-20553.05413872581*x*x*x*x*x+92963.33175750206*x*x*x*x-244905.25916147238*x*x*x+375150.1363997868*x*x-308031.51715612336*x+104203.52690842684;
-                    backboardPosition += backboardPositionOffset;
-                    conveyorSpeed = Constants.Shooter.conveyorSpeed;
-                    if (controller.getRightTriggerAxis() < 0.1) {
-                        flywheelSpeed = 0;
-                        conveyorSpeed = 0;
-                    }
-                    if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.7){
-                        conveyorSpeed = 0;
-                    }
-                    // flywheelSpeed = calculateFlywheelSpeedOnlywithOffset(60);
-                    // double flywheelSpeed = calculateFlywheelSpeed(0);
-                    // double BackboardPosition = calculateBackboardPosition(0);
-                    shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed);
-                    shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed);
-                    shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition);
-                    shooterSubsystem.setFlywheelSpeedByRPS(flywheelSpeed);
-                    shooterSubsystem.setConveyorSpeedByRPS(conveyorSpeed);
-                    shooterSubsystem.setBackboardPosition(backboardPosition);  
-                    if(shooterSubsystem.isBackboardAtTarget()){
-                        shooterSubsystem.backboardMotor.set(0);
-                    }else{
-                        shooterSubsystem.outputBackboard();
-                    }  
-                    break;
-                case Climbing:
-                case CrossingBump:
-                case CrossingTrench:
-                    if(shooterSubsystem.getBackboardPosition() <= 20)
-                        shooterSubsystem.stopMotors();
-                    else{
-                        shooterSubsystem.setBackboardPosition(0);    
-                        shooterSubsystem.outputBackboard();
-                    }
-                    break;
-                    
-            }
-                
-
-        }, shooterSubsystem);
-    }
-
-    // public Command resetBackboardCommand(){
-    //     return 
-    //         Commands.runOnce(()->{
-    //             shooterSubsystem.backboardMotor.set(-0.1);
-    //             shooterSubsystem.currentBackboardInalyzeMode = Mode.Inalyzing;}, shooterSubsystem)
-    //         .andThen(
-    //             Commands.waitSeconds(0.5))
-    //         .andThen(Commands.runOnce(()->{
-    //             shooterSubsystem.backboardMotor.set(0);
-    //             shooterSubsystem.backboardEncoder.reset();
-    //             shooterSubsystem.backboardPID.reset(0);}, shooterSubsystem))
-    //         .andThen(
-    //             Commands.waitSeconds(1))
-    //         .andThen(Commands.runOnce(()->{
-    //             shooterSubsystem.currentBackboardInalyzeMode = Mode.Inalyzed;}, shooterSubsystem));
-    // }
-
-    public void outputInfo(){
-        double currentConveyorSpeed = shooterSubsystem.conveyorMotor.getVelocity().getValueAsDouble();
-        double currentFlywheelSpeed = shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble();
-        double currentBackboardRate = shooterSubsystem.getBackboardPosition();
-        System.out.println("Conveyor: " + currentConveyorSpeed + "\t Flywheel: " + currentFlywheelSpeed + "\t Backboard:" + currentBackboardRate);
-    }
-    public Command resetBackboardCommand(){
-        return 
-            Commands.runOnce(()->{
-                shooterSubsystem.backboardEncoder.reset();
-                shooterSubsystem.backboardPID.reset(0);}, shooterSubsystem);
-    }
-
-    //打表参数
-    // private static final double[][] FLYWHEEL_TABLE = {
-    // {1.5, 0.42},
-    // {2.0, 0.46},
-    // {2.5, 0.51},
-    // {3.0, 0.57},
-    // {3.5, 0.64},
-    // {4.0, 0.72}
-    // };
-
-    // //线性插值函数
-    // private static double lerp(double x0, double y0, double x1, double y1, double x)  {
-    // if (Math.abs(x1 - x0) < 1e-9) {
-    //     return y0;
-    // }
-    // double t = (x - x0) / (x1 - x0);
-    // return y0 + t * (y1 - y0);
-    // }
-    // private double calculateFlywheelSpeedOnlywithOffset(double defaultSpeed) {
-    //     return defaultSpeed + flywheelSpeedOffset;
-    // }
-    // private double calculateBackboardPositionOnlywithOffset(double defaultPosition) {
-    //     return defaultPosition + backboardPositionOffset;
-    // }
-
-
-    // private double calculateFlywheelSpeed(double distance) {
-    //     // 小于最小距离：用第一个点
-    //     if (distance <= FLYWHEEL_TABLE[0][0]) {
-    //     return FLYWHEEL_TABLE[0][1] + flywheelSpeedOffset;
-    //     }
-    //     // 大于最大距离：用最后一个点
-    //     int last = FLYWHEEL_TABLE.length - 1;
-    //     if (distance >= FLYWHEEL_TABLE[last][0]) {
-    //     return FLYWHEEL_TABLE[last][1] + flywheelSpeedOffset;
-    //     }
-    //     // 查找区间并插值
-    //     for (int i = 0; i < FLYWHEEL_TABLE.length - 1; i++) {
-    //         double d0 = FLYWHEEL_TABLE[i][0];
-    //         double s0 = FLYWHEEL_TABLE[i][1];
-    //         double d1 = FLYWHEEL_TABLE[i + 1][0];
-    //         double s1 = FLYWHEEL_TABLE[i + 1][1];
-    //         if (distance >= d0 && distance <= d1) {
-    //             return lerp(d0, s0, d1, s1, distance) + flywheelSpeedOffset;
-    //         }
-    //     }
-    //     return 0;
-    // }
-    
-    // private double calculateBackboardPosition(double distance) {
-    //     double BackboardPosition;
-    //     // 距离阈值（单位自己按distance进行修改）
-    //     final double DISTANCE_NEAR = 2.0;   // 近距离阈值
-    //     final double DISTANCE_MID  = 4.0;   // 中距离阈值
-    //     if (distance < DISTANCE_NEAR) {
-    //     BackboardPosition = Constants.ShooterCalculation.positionNear;
-    //     } 
-    //     else if (distance < DISTANCE_MID) {
-    //     BackboardPosition = Constants.ShooterCalculation.positionMid;
-    //     } 
-    //     else {
-    //     BackboardPosition = Constants.ShooterCalculation.postionFar;
-    //     }
-    //     BackboardPosition += backboardPositionOffset;
-    //     return BackboardPosition;
-    // }
-
-    public void adjustFlywheelSpeedOffset(double offset) {
-        flywheelSpeedOffset += offset;
-    }
-
-    public void adjustBackboardRateOffset(double offset) {
-        backboardPositionOffset += offset;
-    }
-}

src/main/java/frc/robot/generated/TunerConstants.java

@@ -42,12 +42,12 @@ public class TunerConstants {
 
     // The type of motor used for the drive motor
     private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated;
-    // The type of motor used for the drive motor
+    // The type of motor used for the steer motor
     private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated;
 
     // The remote sensor feedback type to use for the steer motors;
-    // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote*
+    // Use RemoteCANcoder so the steer TalonFXs do not require Phoenix Pro licenses.
-    private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder;
+    private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.RemoteCANcoder;
 
     // The stator current at which the wheels start to slip;
     // This needs to be tuned to your individual robot
@@ -55,7 +55,14 @@ public class TunerConstants {
 
     // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null.
     // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation.
-    private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration();
+    private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration()
+        .withCurrentLimits(
+            new CurrentLimitsConfigs()
+                // Default supply current limit is 70 A, but it can be lowered to avoid brownouts.
+                // Supply current limits can be larger than the breaker current rating.
+                .withSupplyCurrentLimit(Amps.of(70))
+                .withSupplyCurrentLimitEnable(true)
+        );
     private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration()
         .withCurrentLimits(
             new CurrentLimitsConfigs()
@@ -66,13 +73,23 @@ public class TunerConstants {
         );
     private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration();
     // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
-    private static final Pigeon2Configuration pigeonConfigs = null;
+    private static final Pigeon2Configuration pigeonConfigs =
+    new Pigeon2Configuration()
+        .withGyroTrim(
+            new GyroTrimConfigs()
+                .withGyroScalarZ(-6.7)
+        )
+        .withPigeon2Features(
+            new Pigeon2FeaturesConfigs()
+                .withEnableCompass(false)
+        );
 
     // CAN bus that the devices are located on;
     // All swerve devices must share the same CAN bus
     public static final CANBus kCANBus = new CANBus("", "./logs/example.hoot");
 
-    // Theoretical free speed (m/s) at 12 V applied output;
+    // Measured robot speed (m/s) at 12 V applied output;
+    // This is NOT the desired max robot speed - see MaxSpeed in RobotContainer instead;
     // This needs to be tuned to your individual robot
     public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.73);
 
@@ -126,10 +143,10 @@ public class TunerConstants {
 
 
     // Front Left
-    private static final int kFrontLeftDriveMotorId = 11;
+    private static final int kFrontLeftDriveMotorId = 1;
-    private static final int kFrontLeftSteerMotorId = 12;
+    private static final int kFrontLeftSteerMotorId = 2;
-    private static final int kFrontLeftEncoderId = 13;
+    private static final int kFrontLeftEncoderId = 3;
-    private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.412841796875);
+    private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.18115234375);
     private static final boolean kFrontLeftSteerMotorInverted = false;
     private static final boolean kFrontLeftEncoderInverted = false;
 
@@ -137,10 +154,10 @@ public class TunerConstants {
     private static final Distance kFrontLeftYPos = Inches.of(11.375);
 
     // Front Right
-    private static final int kFrontRightDriveMotorId = 21;
+    private static final int kFrontRightDriveMotorId = 11;
-    private static final int kFrontRightSteerMotorId = 22;
+    private static final int kFrontRightSteerMotorId = 12;
-    private static final int kFrontRightEncoderId = 23;
+    private static final int kFrontRightEncoderId = 13;
-    private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.012939453125);
+    private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.16162109375);
     private static final boolean kFrontRightSteerMotorInverted = false;
     private static final boolean kFrontRightEncoderInverted = false;
 
@@ -148,10 +165,10 @@ public class TunerConstants {
     private static final Distance kFrontRightYPos = Inches.of(-11.375);
 
     // Back Left
-    private static final int kBackLeftDriveMotorId = 1;
+    private static final int kBackLeftDriveMotorId = 21;
-    private static final int kBackLeftSteerMotorId = 2;
+    private static final int kBackLeftSteerMotorId = 22;
-    private static final int kBackLeftEncoderId = 3;
+    private static final int kBackLeftEncoderId = 23;
-    private static final Angle kBackLeftEncoderOffset = Rotations.of(0.07275390625);
+    private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.017822265625);
     private static final boolean kBackLeftSteerMotorInverted = false;
     private static final boolean kBackLeftEncoderInverted = false;
 
@@ -162,7 +179,7 @@ public class TunerConstants {
     private static final int kBackRightDriveMotorId = 31;
     private static final int kBackRightSteerMotorId = 32;
     private static final int kBackRightEncoderId = 33;
-    private static final Angle kBackRightEncoderOffset = Rotations.of(-0.4892578125);
+    private static final Angle kBackRightEncoderOffset = Rotations.of(-0.490234375);
     private static final boolean kBackRightSteerMotorInverted = false;
     private static final boolean kBackRightEncoderInverted = false;
 
@@ -193,7 +210,7 @@ public class TunerConstants {
 
     /**
      * Creates a CommandSwerveDrivetrain instance.
-     * This should only be called once in your robot program,.
+     * This should only be called once in your robot program.
      */
     public static CommandSwerveDrivetrain createDrivetrain() {
         return new CommandSwerveDrivetrain(

src/main/java/frc/robot/subsystems/ClimberSubsystem.java

@@ -1,103 +0,0 @@
-package frc.robot.subsystems;
-
-import com.ctre.phoenix6.configs.MotorOutputConfigs;
-import com.ctre.phoenix6.controls.DutyCycleOut;
-import com.ctre.phoenix6.controls.PositionVoltage;
-import com.ctre.phoenix6.controls.VelocityDutyCycle;
-import com.ctre.phoenix6.controls.VelocityVoltage;
-import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.InvertedValue;
-
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.Constants;
-
-
-public class ClimberSubsystem extends SubsystemBase {
-
-    private final TalonFX climberMotor;
-    private double targetPositionTicks;
-
-    private final PositionVoltage positionHoldRequest =
-            new PositionVoltage(0).withSlot(0);
-
-    private final VelocityVoltage velocityRequest = new VelocityVoltage(0).withSlot(0);
-
-    private double holdPositionTicks = 0.0;
-    private double deltaPositionTicks = 0.0;
-
-    private final NetworkTable climberTable;
-    
-
-    public ClimberSubsystem() {
-        climberMotor = new TalonFX(Constants.Climber.motorID);
-
-        climberMotor.getConfigurator().apply(Constants.Climber.slot0Configs);
-
-        var cfg = new MotorOutputConfigs();
-        cfg.Inverted = Constants.Climber.inverted
-                ? InvertedValue.Clockwise_Positive
-                : InvertedValue.CounterClockwise_Positive;
-        climberMotor.getConfigurator().apply(cfg);
-
-        climberTable = NetworkTableInstance.getDefault().getTable("Climber");
-
-        holdPositionTicks = getPositionTicks();
-    }
-    public void initialize() {
-        
-        climberMotor.set(-0.07);
-
-    }
-    public void zeroEncoder(){
-
-        climberMotor.setPosition(0);
-        stopMotor();
-    }
-
-    public double getPositionTicks() {
-        return climberMotor.getPosition().getValueAsDouble();
-    }
-
-    public double getVelocityRps() {
-        return climberMotor.getVelocity().getValueAsDouble();
-    }
-
-    public void climbUp() {
-        targetPositionTicks = Constants.Climber.climbUpTicks;  
-        climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks));
-    }
-
-    public void climbDown() {
-        targetPositionTicks = Constants.Climber.climbDownTicks;  
-        climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks));
-        
-    }
-    public void climbDownNonStop(){
-
-        deltaPositionTicks = Constants.Climber.deltaDownTicks;  
-        climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks-deltaPositionTicks));
-    }
-
-    public void captureHoldPosition() {
-        holdPositionTicks = getPositionTicks();
-    }
-
-    public void holdPosition() {
-        climberMotor.setControl(
-                positionHoldRequest.withPosition(holdPositionTicks)
-        );
-    }
-
-    public void stopMotor() {
-        climberMotor.set(0);
-    }
-
-    @Override
-    public void periodic() {
-        climberTable.getEntry("velocityRps").setDouble(getVelocityRps());
-        climberTable.getEntry("positionTicks").setDouble(getPositionTicks());
-        climberTable.getEntry("holdPositionTicks").setDouble(holdPositionTicks);
-    }
-}

src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java

@@ -1,4 +1,5 @@
 package frc.robot.subsystems;
+
 import java.util.Optional;
 import java.util.function.Supplier;
 
@@ -18,46 +19,48 @@ import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.StructPublisher;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Subsystem;
+import frc.robot.Constants;
 import frc.robot.Constants.DriveMode;
 import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain;
-import frc.robot.Constants;
-
-import edu.wpi.first.networktables.*;
-
 
 /**
- * Class that extends the Phoenix 6 SwerveDrivetrain class and implements
+ * Phoenix 6 Tuner X 生成底盘的命令式封装。
- * Subsystem so it can easily be used in command-based projects.
  *
- * Generated by the 2026 Tuner X Swerve Project Generator
+ * 底层 swerve、模块控制和里程计保持原结构；本类只在外层补充
- * https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
+ * Command-Based、PathPlanner、Limelight 融合和上层状态读取接口。
  */
 public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem {
 
-    /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */
+    /* 蓝方视角下，正前方为 0 度。 */
     private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero;
-    /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */
+    /* 红方视角下，正前方为 180 度。 */
     private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg;
-    /* Keep track if we've ever applied the operator perspective before or not */
+    /* 记录是否已经应用过操作员视角。 */
     private boolean m_hasAppliedOperatorPerspective = false;
 
+    // 上层状态机射击门控使用的目标朝向；正常底盘控制仍由手动驾驶掌控。
+    private Rotation2d targetHeading = Rotation2d.kZero;
+
     private DriveMode m_driveMode = DriveMode.OTHER;
-    
+
     private NetworkTable positioningNetworkTable;
     private NetworkTable ll_shooter_NT;
     private NetworkTable ll_intaker_NT;
     private NetworkTable driveNetworkTable;
-    
+
     private final StructPublisher<Pose2d> posePub =
-    NetworkTableInstance.getDefault()
+        NetworkTableInstance.getDefault()
-        .getStructTopic("/Odometry/RobotPose", Pose2d.struct)
+            .getStructTopic("/Odometry/RobotPose", Pose2d.struct)
-        .publish();
+            .publish();
 
     private Field2d m_field;
     private Field2d shooter_ll_field;
@@ -74,20 +77,17 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         SmartDashboard.putData("Field", m_field);
         SmartDashboard.putData("PositioningShooterll", shooter_ll_field);
         SmartDashboard.putData("PositioningIntakerll", intaker_ll_field);
-        LimelightHelpers.SetIMUMode(Constants.Limelight.LIMELIGHT_NAME_Shooter, 1);
+        LimelightHelpers.SetIMUMode(Constants.Limelight.LIMELIGHT_NAME_Shooter, 0);
-        LimelightHelpers.SetIMUMode(Constants.Limelight.LIMELIGHT_NAME_Intaker, 1);
+        LimelightHelpers.SetIMUMode(Constants.Limelight.LIMELIGHT_NAME_Intaker, 0);
         positioningNetworkTable = NetworkTableInstance.getDefault().getTable("Positioning");
         driveNetworkTable = NetworkTableInstance.getDefault().getTable("Drive");
-        ll_shooter_NT = NetworkTableInstance.getDefault().getTable("Positioning/limelight-shooter");
+        ll_shooter_NT = NetworkTableInstance.getDefault().getTable("Positioning/limelight-left");
-        ll_intaker_NT = NetworkTableInstance.getDefault().getTable("Positioning/limelight-intaker");
+        ll_intaker_NT = NetworkTableInstance.getDefault().getTable("Positioning/limelight-front");
 
         configueAutoBuilder();
-
     }
 
-
+    private void configueAutoBuilder() {
-    private void configueAutoBuilder(){
-        
         RobotConfig config;
         try {
             config = RobotConfig.fromGUISettings();
@@ -96,20 +96,20 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         }
 
         AutoBuilder.configure(
-            this::getPose,                  // 当前机器人 Pose
+            this::getPose,                 // 当前机器人位姿。
-            this::resetPose,                // 重置里程计
+            this::resetPose,               // 重置里程计。
-            this::getRobotRelativeSpeeds,   // ROBOT RELATIVE speeds
+            this::getRobotRelativeSpeeds,  // 机器人坐标系速度。
-            (speeds, feedforwards) -> driveRobotRelative(speeds),      // 用 speeds 驱动机器人
+            (speeds, feedforwards) -> driveRobotRelative(speeds), // 按目标速度驱动机器人。
             new PPHolonomicDriveController(
-                new PIDConstants(5.0, 0.0, 0.0),   // X/Y PID
+                new PIDConstants(5.0, 0.0, 0.0), // X/Y 位置 PID。
-                new PIDConstants(5.0, 0.0, 0.0)    // Theta PID
+                new PIDConstants(5.0, 0.0, 0.0)  // 朝向 PID。
             ),
             config,
             () -> false,
             this
         );
-
     }
+
     public Pose2d getPose() {
         return getState().Pose;
     }
@@ -118,11 +118,37 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         return getState().Speeds;
     }
 
+    public ChassisSpeeds getChassisSpeeds() {
+        // 给上层代码使用的速度别名；原机器人坐标系方法继续留给 PathPlanner。
+        return getRobotRelativeSpeeds();
+    }
+
+    public ChassisSpeeds getFieldRelativeSpeeds() {
+        return ChassisSpeeds.fromRobotRelativeSpeeds(getRobotRelativeSpeeds(), getRotation());
+    }
+
+    public Rotation2d getRotation() {
+        return getState().Pose.getRotation();
+    }
+
+    public void setTargetHeading(Rotation2d targetHeading) {
+        this.targetHeading = targetHeading;
+    }
+
+    public double getHeadingError() {
+        return targetHeading.minus(getRotation()).getDegrees();
+    }
+
+    public boolean isAimed() {
+        // 上层状态机只读取这个门控，不直接控制 swerve 模块。
+        return Math.abs(getHeadingError()) <= Constants.Superstructure.aimToleranceDegrees;
+    }
+
     public void driveRobotRelative(ChassisSpeeds speeds) {
         ChassisSpeeds scaled = new ChassisSpeeds(
-            speeds.vxMetersPerSecond,                 // X 不变
+            speeds.vxMetersPerSecond,          // X 方向速度保持原值。
-            speeds.vyMetersPerSecond,                 // Y 不变
+            speeds.vyMetersPerSecond,          // Y 方向速度保持原值。
-            speeds.omegaRadiansPerSecond * 1.3        // 只放大旋转
+            speeds.omegaRadiansPerSecond * 1.3 // 只放大旋转输出，沿用原驾驶手感。
         );
 
         setControl(
@@ -130,17 +156,32 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         );
     }
 
+    public void stop() {
+        setControl(
+            new SwerveRequest.ApplyRobotSpeeds().withSpeeds(new ChassisSpeeds(0, 0, 0))
+        );
+    }
 
+    public void lockWheels() {
+        setControl(new SwerveRequest.SwerveDriveBrake());
+    }
+
+    public void aimToTarget() {
+        // 给未来命令预留的高层辅助接口，仍走现有底盘请求路径，不触碰模块底层逻辑。
+        double headingErrorDegrees = getHeadingError();
+        double omega = Math.toRadians(headingErrorDegrees) * Constants.AutoPositioning.TurningkP;
+        setControl(
+            new SwerveRequest.ApplyRobotSpeeds().withSpeeds(new ChassisSpeeds(0, 0, omega))
+        );
+    }
 
-        
-    private static final double kMaxVisionJumpMeters = 1.0;
     private static final double kMaxOmegaDegPerSec = 360.0;
 
     /**
-     * Returns a command that applies the specified control request to this swerve drivetrain.
+     * 返回一个持续下发指定 swerve 请求的命令。
      *
-     * @param request Function returning the request to apply
+     * @param request 生成待下发请求的函数。
-     * @return Command to run
+     * @return 可调度执行的 Command。
      */
     public Command applyRequest(Supplier<SwerveRequest> request) {
         return run(() -> this.setControl(request.get()));
@@ -153,10 +194,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
             this.setControl(requestSupplier.get());
         });
     }
-    
+
-    public void forceUsingLimelightMT2() {
+    public void forceUsingLimelightmt2() {
         LimelightHelpers.PoseEstimate mt2 =
-        LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Shooter);
+            LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Shooter);
 
         if (mt2 == null || mt2.tagCount == 0) {
             return;
@@ -165,15 +206,27 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         var stdDevs = VecBuilder.fill(0.7, 0.7, 9999999);
         addVisionMeasurement(mt2.pose, mt2.timestampSeconds, stdDevs);
     }
-    
+
-    private void fuseLimelightMT2() {
+    public void forceUsingLimelightmt2WithllName(String llString) {
+        LimelightHelpers.PoseEstimate mt2 =
+            LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(llString);
+
+        if (mt2 == null || mt2.tagCount == 0) {
+            return;
+        }
+
+        var stdDevs = VecBuilder.fill(0.7, 0.7, 9999999);
+        addVisionMeasurement(mt2.pose, mt2.timestampSeconds, stdDevs);
+    }
+
+    private void fuseLimelightmt2() {
         Pose2d current = getState().Pose;
 
         LimelightHelpers.PoseEstimate mt2_shooter =
-        getLimelightPoseEstimate(Constants.Limelight.LIMELIGHT_NAME_Shooter);
+            getLimelightPoseEstimate(Constants.Limelight.LIMELIGHT_NAME_Shooter);
-        
+
         LimelightHelpers.PoseEstimate mt2_intaker =
-        getLimelightPoseEstimate(Constants.Limelight.LIMELIGHT_NAME_Intaker);
+            getLimelightPoseEstimate(Constants.Limelight.LIMELIGHT_NAME_Intaker);
 
         boolean shooterAvailable = ifLimelightPositioningAvailable(mt2_shooter, current);
         boolean intakerAvailable = ifLimelightPositioningAvailable(mt2_intaker, current);
@@ -181,35 +234,34 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         ll_intaker_NT.getEntry("available").setBoolean(intakerAvailable);
         ll_shooter_NT.getEntry("TagCount").setNumber(mt2_shooter != null ? mt2_shooter.tagCount : 0);
         ll_intaker_NT.getEntry("TagCount").setNumber(mt2_intaker != null ? mt2_intaker.tagCount : 0);
-        
+
         LimelightHelpers.PoseEstimate mt2_final = null;
 
-        if(!shooterAvailable && !intakerAvailable) {
+        if (!shooterAvailable && !intakerAvailable) {
             positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(false);
             positioningNetworkTable.getEntry("Usingll").setString("None");
-            //positioningNetworkTable.getEntry("MetaTag2DistanceDifference").setDouble(Double.NaN);
+            // 未使用视觉时不发布距离差，避免仪表盘误读旧值。
-            
             return;
-        }else if (shooterAvailable && !intakerAvailable) {
+        } else if (shooterAvailable && !intakerAvailable) {
             double shooterDistance = current.getTranslation().getDistance(mt2_shooter.pose.getTranslation());
             ll_shooter_NT.getEntry("Distance").setDouble(shooterDistance);
             ll_shooter_NT.getEntry("X").setDouble(mt2_shooter.pose.getX());
             ll_shooter_NT.getEntry("Y").setDouble(mt2_shooter.pose.getY());
             positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(true);
             positioningNetworkTable.getEntry("Usingll").setString("Shooter");
-            //positioningNetworkTable.getEntry("MetaTag2DistanceDifference").setDouble(Double.NaN);
+            // 单个 Limelight 可用时直接采用该测量。
             mt2_final = mt2_shooter;
-        }else if (!shooterAvailable && intakerAvailable) {
+        } else if (!shooterAvailable && intakerAvailable) {
             double intakerDistance = current.getTranslation().getDistance(mt2_intaker.pose.getTranslation());
             ll_intaker_NT.getEntry("Distance").setDouble(intakerDistance);
             ll_intaker_NT.getEntry("X").setDouble(mt2_intaker.pose.getX());
             ll_intaker_NT.getEntry("Y").setDouble(mt2_intaker.pose.getY());
             positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(true);
             positioningNetworkTable.getEntry("Usingll").setString("Intaker");
-            //positioningNetworkTable.getEntry("MetaTag2DistanceDifference").setDouble(Double.NaN);
+            // 单个 Limelight 可用时直接采用该测量。
             mt2_final = mt2_intaker;
-        }else {
+        } else {
-            // 两个都可用时的策略：这里简单选更近的，你也
+            // 两个 Limelight 都可用时，先选择与当前位姿更接近的测量。
             double shooterDistance = current.getTranslation().getDistance(mt2_shooter.pose.getTranslation());
             double intakerDistance = current.getTranslation().getDistance(mt2_intaker.pose.getTranslation());
             double difference = mt2_shooter.pose.getTranslation().getDistance(mt2_intaker.pose.getTranslation());
@@ -227,7 +279,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
 
             mt2_final = shooterDistance <= intakerDistance ? mt2_shooter : mt2_intaker;
         }
-        
+
         Pose2d mt2Pose = mt2_final.pose;
 
         double distance = current.getTranslation().getDistance(mt2Pose.getTranslation());
@@ -240,11 +292,11 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         positioningNetworkTable.getEntry("PoseDistance").setDouble(distance);
         positioningNetworkTable.getEntry("RotationDifference").setDouble(Math.abs(deltaRotation.getDegrees()));
 
-        // 4) 设置测量噪声（示例：theta 基本不信任就给极大）
+        // 视觉测量噪声：theta 基本不信任，只融合平移。
-        // 你也可以根据 mt2.avgTagDist / mt2.tagSpan 动态调
+        // 后续可以根据 mt2.avgTagDist / mt2.tagSpan 动态调整。
         var stdDevs = VecBuilder.fill(0.7, 0.7, 9999999);
 
-        // 5) 融合：注意 timestampSeconds 的处理
+        // 融合视觉测量，注意 timestampSeconds 的时间基转换。
         addVisionMeasurement(mt2_final.pose, mt2_final.timestampSeconds, stdDevs);
         positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(true);
     }
@@ -255,30 +307,28 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         }
         return LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName);
     }
+
     private boolean ifLimelightPositioningAvailable(LimelightHelpers.PoseEstimate mt2, Pose2d currentPose) {
-        // 1) 没看到 tag：拒绝
+        // 没看到 tag 时拒绝该帧视觉定位。
         if (mt2 == null || mt2.tagCount == 0) {
             return false;
         }
-        // 2) 转太快：拒绝（角速度你可以从 Pigeon2 或 drivetrain state 拿）
-        double omegaDegPerSec = getPigeon2().getAngularVelocityZWorld().getValueAsDouble(); // 若你有这个信号
-        if (Math.abs(omegaDegPerSec) > kMaxOmegaDegPerSec) {
-            System.out.println("NoUsingMetaTag2ForTooFastRotationSpeed");
-            return false;
-        }      
 
-        // 3) 跳变太大：拒绝（Phoenix6 推荐的稳定策略）
+        // 旋转太快时拒绝该帧，避免高速转动时的视觉跳变。
-        //Pose2d current = getState().Pose;
+        double omegaDegPerSec = getPigeon2().getAngularVelocityZWorld().getValueAsDouble();
-        if (currentPose.getTranslation().getDistance(mt2.pose.getTranslation()) > kMaxVisionJumpMeters) {
+        if (Math.abs(omegaDegPerSec) > kMaxOmegaDegPerSec) {
-            System.out.println("NoUsingMetaTag2ForTooChangingPose");
             return false;
         }
 
         return true;
     }
+
+    private double clamp(double value, double min, double max) {
+        return Math.max(min, Math.min(max, value));
+    }
+
     @Override
     public void periodic() {
-
         Pose2d pose = getState().Pose;
         m_field.setRobotPose(pose);
         posePub.set(pose);
@@ -288,19 +338,22 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
         driveNetworkTable.getEntry("DriveMode").setString(m_driveMode.name());
         driveNetworkTable.getEntry("IsFieldCentric").setBoolean(m_driveMode == DriveMode.FIELD_CENTRIC);
 
-        // 1) 每帧喂给 LL：机器人当前 yaw（度），其余先全 0
+        // 保留 Positioning 表中的基础状态输出。
+        positioningNetworkTable.getEntry("interestPointsCl").setBoolean(false);
+
+        // 每帧向 Limelight 写入当前 yaw，供 MegaTag2 使用。
         LimelightHelpers.SetRobotOrientation(
             Constants.Limelight.LIMELIGHT_NAME_Shooter,
-            pose.getRotation().getDegrees(), // 关键：要符合 wpiBlue 约定
+            pose.getRotation().getDegrees(), // 符合 wpiBlue 坐标约定。
             0, 0, 0, 0, 0
         );
         LimelightHelpers.SetRobotOrientation(
             Constants.Limelight.LIMELIGHT_NAME_Intaker,
-            pose.getRotation().getDegrees(), // 关键：要符合 wpiBlue 约定
+            pose.getRotation().getDegrees(), // 符合 wpiBlue 坐标约定。
             0, 0, 0, 0, 0
         );
 
-        fuseLimelightMT2();
+        fuseLimelightmt2();
 
         if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) {
             DriverStation.getAlliance().ifPresent(allianceColor -> {
@@ -312,15 +365,13 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
                 m_hasAppliedOperatorPerspective = true;
             });
         }
-    }        
+    }
-        
+
-        
+    /**
-            /**
+     * 添加一帧视觉位姿测量，用于修正里程计估计。
-     * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
-     * while still accounting for measurement noise.
      *
-     * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+     * @param visionRobotPoseMeters 视觉测得的机器人位姿。
-     * @param timestampSeconds The timestamp of the vision measurement in seconds.
+     * @param timestampSeconds 视觉测量时间戳，单位秒。
      */
     @Override
     public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
@@ -328,17 +379,11 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
     }
 
     /**
-     * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
+     * 添加一帧带标准差的视觉位姿测量。
-     * while still accounting for measurement noise.
-     * <p>
-     * Note that the vision measurement standard deviations passed into this method
-     * will continue to apply to future measurements until a subsequent call to
-     * {@link #setVisionMeasurementStdDevs(Matrix)} or this method.
      *
-     * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+     * @param visionRobotPoseMeters 视觉测得的机器人位姿。
-     * @param timestampSeconds The timestamp of the vision measurement in seconds.
+     * @param timestampSeconds 视觉测量时间戳，单位秒。
-     * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement
+     * @param visionMeasurementStdDevs 视觉测量标准差，格式为 [x, y, theta]。
-     *     in the form [x, y, theta]ᵀ, with units in meters and radians.
      */
     @Override
     public void addVisionMeasurement(
@@ -350,10 +395,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
     }
 
     /**
-     * Return the pose at a given timestamp, if the buffer is not empty.
+     * 从位姿缓存中读取指定时间戳的机器人位姿。
      *
-     * @param timestampSeconds The timestamp of the pose in seconds.
+     * @param timestampSeconds 位姿时间戳，单位秒。
-     * @return The pose at the given timestamp (or Optional.empty() if the buffer is empty).
+     * @return 指定时间的位姿；缓存为空时返回 Optional.empty()。
      */
     @Override
     public Optional<Pose2d> samplePoseAt(double timestampSeconds) {

src/main/java/frc/robot/subsystems/ConveyorSubsystem.java

@@ -0,0 +1,90 @@
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
+import com.ctre.phoenix6.controls.NeutralOut;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.math.MathUtil;
+import frc.robot.Constants;
+
+/**
+ * 主 conveyor 的开环控制封装。
+ */
+public class ConveyorSubsystem extends SubsystemBase {
+    public final TalonFX mainConveyor;
+    private final VoltageOut voltageRequest = new VoltageOut(0);
+    private final NeutralOut neutralRequest = new NeutralOut();
+    private final NetworkTable table = NetworkTableInstance.getDefault().getTable("Conveyor");
+    private final NetworkTable tuningTable = NetworkTableInstance.getDefault().getTable("Tuning/mainConveyor");
+    private double targetOutputVolts = 0.0;
+    private double commandedDebugVolts = Constants.Conveyor.feedVoltage;
+    private double clampedDebugVolts = Constants.Conveyor.feedVoltage;
+    private boolean voltageTuningActive = false;
+
+    public ConveyorSubsystem() {
+        mainConveyor = new TalonFX(Constants.Conveyor.mainConveyorID);
+        mainConveyor.getConfigurator().apply(Constants.Conveyor.mainConveyorSlot0Configs);
+        var mainConveyorMotorOutputConfigs = new MotorOutputConfigs();
+        mainConveyorMotorOutputConfigs.NeutralMode = NeutralModeValue.Coast;
+        mainConveyor.getConfigurator().apply(mainConveyorMotorOutputConfigs);
+        tuningTable.getEntry("mainConveyorVoltageEnable").setDefaultBoolean(false);
+        tuningTable.getEntry("mainConveyorTargetVolts").setDefaultDouble(Constants.Conveyor.feedVoltage);
+    }
+    public void feedToShooter() {
+        setVoltage(Constants.Conveyor.feedVoltage);
+    }
+    public void feedForIntake() {
+        setVoltage(Constants.Conveyor.intakeFeedVoltage);
+    }
+    public void reverse() {
+        setVoltage(Constants.Conveyor.reverseVoltage);
+    }
+    public void stop() {
+        targetOutputVolts = 0.0;
+        mainConveyor.setControl(neutralRequest);
+    }
+    public void setVoltage(double outputVolts) {
+        targetOutputVolts = MathUtil.clamp(outputVolts, -12.0, 12.0);
+        mainConveyor.setControl(voltageRequest.withOutput(targetOutputVolts));
+    }
+
+    public boolean isMainConveyorTuningControlActive() {
+        return DriverStation.isTestEnabled()
+            && tuningTable.getEntry("mainConveyorVoltageEnable").getBoolean(false);
+    }
+
+    public void debugMainConveyorVoltagePeriodic(boolean allowMotorOutput) {
+        voltageTuningActive =
+            allowMotorOutput
+                && DriverStation.isTestEnabled()
+                && tuningTable.getEntry("mainConveyorVoltageEnable").getBoolean(false);
+        commandedDebugVolts = tuningTable.getEntry("mainConveyorTargetVolts")
+            .getDouble(Constants.Conveyor.feedVoltage);
+        clampedDebugVolts = MathUtil.clamp(commandedDebugVolts, -12.0, 12.0);
+
+        tuningTable.getEntry("mainConveyorVoltageEnableActive").setBoolean(voltageTuningActive);
+        tuningTable.getEntry("mainConveyorCommandedTargetVolts").setDouble(commandedDebugVolts);
+        tuningTable.getEntry("mainConveyorClampedTargetVolts").setDouble(clampedDebugVolts);
+
+        if (voltageTuningActive) {
+            setVoltage(clampedDebugVolts);
+        }
+    }
+
+    @Override
+    public void periodic() {
+        table.getEntry("velocity").setDouble(mainConveyor.getVelocity().getValueAsDouble());
+        table.getEntry("targetOutputVolts").setDouble(targetOutputVolts);
+        table.getEntry("motorOutputVolts").setDouble(mainConveyor.getMotorVoltage().getValueAsDouble());
+        table.getEntry("statorCurrent").setDouble(mainConveyor.getStatorCurrent().getValueAsDouble());
+        table.getEntry("voltageTuningActive").setBoolean(voltageTuningActive);
+        table.getEntry("commandedDebugVolts").setDouble(commandedDebugVolts);
+        table.getEntry("clampedDebugVolts").setDouble(clampedDebugVolts);
+    }
+}

src/main/java/frc/robot/subsystems/IntakeRollerSubsystem.java

@@ -0,0 +1,81 @@
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.controls.Follower;
+import com.ctre.phoenix6.controls.NeutralOut;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.MotorAlignmentValue;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+/**
+ * 进球滚轮的硬件封装。
+ *
+ * 何时 intake、outtake、hold 或 stop 由 Superstructure 决定；
+ * 本类只负责成对 TalonFX 输出和 follower 关系。
+ */
+public class IntakeRollerSubsystem extends SubsystemBase {
+    public final TalonFX leftIntakeRoller;
+    private final TalonFX rightIntakeRoller;
+
+    private final VoltageOut voltageRequest = new VoltageOut(0);
+    private final NeutralOut neutralRequest = new NeutralOut();
+    private final Follower rightFollower;
+    private final NetworkTable table = NetworkTableInstance.getDefault().getTable("IntakeRoller");
+    private double targetOutputVolts = 0.0;
+
+    public IntakeRollerSubsystem() {
+        leftIntakeRoller = new TalonFX(Constants.Intaker.leftIntakeRollerID);
+        rightIntakeRoller = new TalonFX(Constants.Intaker.rightIntakeRollerID);
+        leftIntakeRoller.getConfigurator().apply(Constants.Intaker.intakeRollerSlot0Configs);
+        rightIntakeRoller.getConfigurator().apply(Constants.Intaker.intakeRollerSlot0Configs);
+        rightFollower = new Follower(leftIntakeRoller.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50);
+        followLeft();
+    }
+
+    public void intake() {
+        setVoltage(Constants.Intaker.intakeRollerIntakeVoltage);
+    }
+
+    public void intakeforshoot() {
+        setVoltage(-1.0);
+    }  
+
+    public void outtake() {
+        setVoltage(Constants.Intaker.intakeRollerOuttakeVoltage);
+    }
+
+    public void stop() {
+        targetOutputVolts = 0.0;
+        leftIntakeRoller.setControl(neutralRequest);
+        rightIntakeRoller.setControl(neutralRequest);
+    }
+
+    public void setVoltage(double outputVolts) {
+        targetOutputVolts = MathUtil.clamp(outputVolts, -12.0, 12.0);
+        leftIntakeRoller.setControl(voltageRequest.withOutput(targetOutputVolts));
+        followLeft();
+    }
+
+    private void followLeft() {
+        rightIntakeRoller.setControl(rightFollower);
+    }
+
+    private double getVelocityDifference() {
+    double leftVelocity = leftIntakeRoller.getVelocity().getValueAsDouble();
+    double rightVelocity = rightIntakeRoller.getVelocity().getValueAsDouble();
+    return Math.abs(leftVelocity) - Math.abs(rightVelocity);
+    }
+
+    @Override
+    public void periodic() {
+        table.getEntry("leftVelocity").setDouble(leftIntakeRoller.getVelocity().getValueAsDouble());
+        table.getEntry("rightVelocity").setDouble(rightIntakeRoller.getVelocity().getValueAsDouble());
+        table.getEntry("velocityDifference").setDouble(getVelocityDifference());
+        table.getEntry("targetOutputVolts").setDouble(targetOutputVolts);
+    }
+}

src/main/java/frc/robot/subsystems/IntakeRotaterSubsystem.java

@@ -0,0 +1,139 @@
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.controls.Follower;
+import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.MotorAlignmentValue;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+/**
+ * Intaker rotation control for the 2026 fuel path.
+ *
+ * The rotater uses timed voltage actions only. Position is published for
+ * telemetry, but it is not used for closed-loop control.
+ */
+public class IntakeRotaterSubsystem extends SubsystemBase {
+    public final TalonFX leftIntakeRotater;
+    private final TalonFX rightIntakeRotater;
+
+    private final VoltageOut voltageRequest = new VoltageOut(0);
+    private final Follower rightFollower;
+    private final NetworkTable table = NetworkTableInstance.getDefault().getTable("IntakeRotater");
+
+    private double outputVoltage = 0.0;
+
+    public IntakeRotaterSubsystem() {
+        leftIntakeRotater = new TalonFX(Constants.Intaker.leftIntakeRotaterID);
+        rightIntakeRotater = new TalonFX(Constants.Intaker.rightIntakeRotaterID);
+        leftIntakeRotater.getConfigurator().apply(Constants.Intaker.intakeRotaterSlot0Configs);
+        rightIntakeRotater.getConfigurator().apply(Constants.Intaker.intakeRotaterSlot0Configs);
+        rightFollower = new Follower(leftIntakeRotater.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50);
+        followLeft();
+
+        leftIntakeRotater.setPosition(0.0);
+        rightIntakeRotater.setPosition(0.0);
+    }
+
+    public void lowerForMatch() {
+        setVoltage(0.05);
+    }
+
+    public Command lowerForMatchCommand() {
+        return Commands.sequence(
+            Commands.runOnce(() -> setVoltage(-1.5), this),
+            Commands.waitSeconds(0.7),
+            Commands.runOnce(() -> setVoltage(0.05), this),
+            Commands.waitSeconds(0.7),
+            Commands.runOnce(this::stop, this)
+        );
+    }
+
+    public void raiseForMatch() {
+        setVoltage(0.8);
+    }
+
+    public Command raiseForMatchCommand() {
+        return Commands.sequence(
+            Commands.runOnce(this::raiseForMatch, this),
+            Commands.waitSeconds(0.5),
+            Commands.runOnce(this::stop, this)
+        );
+    }
+
+    public void shootAssist() {
+        double cycleTime = Timer.getFPGATimestamp() % 1.6;
+
+        if (cycleTime < 0.6) {
+            setVoltage(1);
+        } else if (cycleTime < 0.9) {
+            setVoltage(0.0);
+        } else {
+            setVoltage(0.03);
+        }
+    }
+
+    public void postShootIdleAssist(double elapsedSeconds) {
+        double cycleTime = elapsedSeconds % 0.2;
+        setVoltage(cycleTime < 0.1 ? 0.0 : 0.0);
+    }
+
+    public void manualRaise() {
+        setVoltage(Constants.Intaker.intakeRotaterRaiseVoltage);
+    }
+
+    public void manualLower() {
+        setVoltage(Constants.Intaker.intakeRotaterLowerVoltage);
+    }
+
+    public void stop() {
+        setVoltage(0.0);
+    }
+
+    public boolean isRunning() {
+        return Math.abs(outputVoltage) > 1e-6;
+    }
+
+    public double getPosition() {
+        return leftIntakeRotater.getPosition().getValueAsDouble();
+    }
+
+    public double getLeftVelocity() {
+        return leftIntakeRotater.getVelocity().getValueAsDouble();
+    }
+
+    public double getRightVelocity() {
+        return rightIntakeRotater.getVelocity().getValueAsDouble();
+    }
+
+    private double getVelocityDifference() {
+        return Math.abs(getLeftVelocity()) - Math.abs(getRightVelocity());
+    }
+
+    private void setVoltage(double volts) {
+        outputVoltage = MathUtil.clamp(volts, -12.0, 12.0);
+        leftIntakeRotater.setControl(voltageRequest.withOutput(outputVoltage));
+        followLeft();
+    }
+
+    private void followLeft() {
+        rightIntakeRotater.setControl(rightFollower);
+    }
+
+    @Override
+    public void periodic() {
+        table.getEntry("position").setDouble(getPosition());
+        table.getEntry("isRunning").setBoolean(isRunning());
+        table.getEntry("outputVoltage").setDouble(outputVoltage);
+        table.getEntry("leftVelocity").setDouble(getLeftVelocity());
+        table.getEntry("rightVelocity").setDouble(getRightVelocity());
+        table.getEntry("velocityDifference").setDouble(getVelocityDifference());
+    }
+}

src/main/java/frc/robot/subsystems/IntakerSubsystem.java

@@ -1,67 +0,0 @@
-package frc.robot.subsystems;
-
-import com.ctre.phoenix6.controls.PositionVoltage;
-import com.ctre.phoenix6.controls.VelocityVoltage;
-import com.ctre.phoenix6.hardware.TalonFX;
-
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.Constants;
-
-public class IntakerSubsystem extends SubsystemBase{
-    //定义intakerotater&intakegetter
-    private TalonFX intakerotater;
-    private TalonFX intakegetter;
-
-    //PID 
-    private final VelocityVoltage velocityRequest = new VelocityVoltage(0);
-    private final PositionVoltage positionRequest = new PositionVoltage(0);
- 
-    private double intakeRotaterTargetPosition = 0.0;
-
-    public IntakerSubsystem() {
-        //初始化rotater&getter
-        intakegetter = new TalonFX(47);
-        intakerotater = new TalonFX(48);
-        intakegetter.getConfigurator().apply(Constants.Intaker.intakeGetterSlot0Configs);
-        intakerotater.getConfigurator().apply(Constants.Intaker.intakeRotaterSlot0Configs);        
-    }
-
-    public void setIntakerRotaterPosition(double targetPosition) {
-        //目标position
-        if (targetPosition > Constants.Intaker.intakeRotaterUpLimit) {
-            targetPosition = Constants.Intaker.intakeRotaterUpLimit;  // 限制最大上限
-        } else if (targetPosition < Constants.Intaker.intakeRotaterDownLimit) {
-            targetPosition = Constants.Intaker.intakeRotaterDownLimit;  // 限制最小下限
-        }
-
-        intakeRotaterTargetPosition = targetPosition;
-
-        intakerotater.setControl(positionRequest.withPosition(targetPosition)); // 设置rotater位置（TalonFX 内部闭环 PID）
-    }
-
-    // 获取intakerotater当前角度
-    public double getIntakerotaterPosition() {
-        return intakerotater.getPosition().getValueAsDouble();
-    }
-
-    public boolean isIntakerotaterAtTarget() {
-        // 到位判断容差
-        double error = Math.abs(intakeRotaterTargetPosition - getIntakerotaterPosition());
-        return error <= Constants.Intaker.positionToleranceRotations;
-    }
-
-     public void stopintakerotater() {
-        intakerotater.setControl(velocityRequest.withVelocity(0));
-        // 如果你希望停止后保持当前位置，建议改用中性模式/或维持 positionRequest（这句fromAI）
-    }
-
-    //intakegetter的on/off
-    public void setIntakeGetterOn(boolean on) {
-        if (on) {
-            intakegetter.setControl(velocityRequest.withVelocity(Constants.Intaker.IntakeGetterSpeed));
-        }
-        else {
-            intakegetter.setControl(velocityRequest.withVelocity(0));
-        }
-    }
-}

src/main/java/frc/robot/subsystems/LEDSubsystem.java

@@ -18,6 +18,7 @@ public class LEDSubsystem extends SubsystemBase {
   private final AddressableLED led;
   private final AddressableLEDBuffer buffer;
   private final Timer timer = new Timer();
+  private final boolean enabled;
 
   // 左右灯带分段（左 0~71，右 72~143）
   private final int leftStart = 0;
@@ -50,11 +51,15 @@ public class LEDSubsystem extends SubsystemBase {
 
     led = new AddressableLED(pwmPort);
     buffer = new AddressableLEDBuffer(leftLen + rightLen);
+    enabled = Constants.LEDUsing;
 
-    led.setLength(buffer.getLength());
+    if (enabled) {
-    led.start();
+      led.setLength(buffer.getLength());
-
+      led.start();
-    timer.start();
+      timer.start();
+    } else {
+      led.close();
+    }
   }
 
   /* ====================== */
@@ -93,7 +98,9 @@ public class LEDSubsystem extends SubsystemBase {
 
   @Override
   public void periodic() {
-    if(!Constants.LEDUsing) {led.close(); return;}
+    if (!enabled) {
+      return;
+    }
     // 左段
     switch (leftMode) {
       case OFF -> setRange(leftStart, leftLen, 0, 0, 0);

src/main/java/frc/robot/subsystems/LimelightSubsystem.java

@@ -0,0 +1,49 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+/**
+ * Thin read-only wrapper around the existing LimelightHelpers.
+ *
+ * This subsystem does not fuse vision or command mechanisms; it only gives
+ * Superstructure stable target and distance read APIs.
+ */
+public class LimelightSubsystem extends SubsystemBase {
+    private final String limelightName;
+
+    public LimelightSubsystem() {
+        this(Constants.Limelight.LIMELIGHT_NAME_Shooter);
+    }
+
+    public LimelightSubsystem(String limelightName) {
+        this.limelightName = limelightName;
+    }
+
+    public boolean hasTarget() {
+        return LimelightHelpers.getTV(limelightName);
+    }
+
+    public double getTargetYaw() {
+        return LimelightHelpers.getTX(limelightName);
+    }
+
+    public double getTargetPitch() {
+        return LimelightHelpers.getTY(limelightName);
+    }
+
+    public double getDistanceToTarget() {
+        // DriveControls publishes the field-pose distance under AutoControl.
+        // Reuse that value to preserve the existing target calculation path.
+        return NetworkTableInstance.getDefault()
+            .getTable("AutoControl")
+            .getEntry("distanceToHub")
+            .getDouble(0.0);
+    }
+
+    public Pose2d getEstimatedPose() {
+        return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName).pose;
+    }
+}

src/main/java/frc/robot/subsystems/ShooterSubsystem.java

@@ -3,165 +3,524 @@ package frc.robot.subsystems;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.controls.Follower;
-import com.ctre.phoenix6.controls.VelocityDutyCycle;
+import com.ctre.phoenix6.controls.NeutralOut;
+import com.ctre.phoenix6.controls.VelocityVoltage;
+import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.hardware.TalonFXS;
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.MotorAlignmentValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 
+/**
+ * Shooter 总成的硬件封装。
+ *
+ * 飞轮和 shooter 内部 conveyor 使用 VoltageOut 开环电压输出；
+ * shooter 内部 conveyor 仍由右侧电机反向 follower 跟随左侧电机。
+ * 背板控制沿用旧代码中的电机、编码器和 PID 调参流程。
+ * 何时 spin up、何时喂球由 Superstructure 决定，本类不做比赛动作判断。
+ */
 public class ShooterSubsystem extends SubsystemBase {
-    // 定义飞轮、传动和背板电机
+    public final TalonFX leftFlywheel;
-    public TalonFX flywheelMotorLeft;
+    public final TalonFX rightFlywheel;
-    private TalonFX flywheelMotorRight;
+    public final TalonFX leftConveyor;
-    public TalonFX conveyorMotor;
+    public final TalonFX rightConveyor;
-    public TalonFXS backboardMotor;
-    public Encoder backboardEncoder;
 
-    // PID 控制器，控制背板角度
+    public final TalonFXS backboardMotor;
-    public ProfiledPIDController backboardPID;
+    public final Encoder backboardEncoder;
-    private Follower flyWheelFollower;
+    public final ProfiledPIDController backboardPID;
 
-    private NetworkTable shooterNetworkTable;
+    private final VelocityVoltage leftFlywheelVelocityRequest = new VelocityVoltage(0).withSlot(0);
+    private final VelocityVoltage conveyorVelocityRequest = new VelocityVoltage(0).withSlot(0);
+    private final VoltageOut backboardVoltageRequest = new VoltageOut(0);
+    private final NeutralOut flywheelNeutralRequest = new NeutralOut();
+    private final NeutralOut conveyorNeutralRequest = new NeutralOut();
 
-    private final VelocityDutyCycle velocityRequest = new VelocityDutyCycle(0);
+    private final Follower flywheelFollower;
+    private final Follower conveyorFollower;
 
+    private final NetworkTable shooterNetworkTable;
+    private final NetworkTable flywheelTuningTable;
+    private final NetworkTable backboardTuningTable;
+    private final NetworkTable shooterConveyorTuningTable;
+
+    private double flywheelOutputVolts = 0.0;
+    private double flywheelTargetVelocity = 0.0;
+    private double flywheelVelocityTolerance = Constants.Shooter.flywheelReadyVelocityTolerance;
+    private boolean flywheelVelocityTuningActive = false;
+    private double shooterConveyorTargetVelocity = 0.0;
+    private double shooterConveyorCommandedTargetVelocity = Constants.Shooter.shooterConveyorFeedVelocity;
+    private double shooterConveyorClampedTargetVelocity = Constants.Shooter.shooterConveyorFeedVelocity;
+    private boolean shooterConveyorVelocityTuningActive = false;
+    private double shooterConveyorOutputVolts = 0.0;
+    private double backboardOutputVolts = 0.0;
+    private double backboardRawPidOutputVolts = 0.0;
+    private double backboardGravityFeedforwardVolts = 0.0;
+    private double backboardClampedPidOutputVolts = 0.0;
+    private boolean backboardOutputSaturated = false;
+    private boolean backboardMinimumOutputActive = false;
 
-    // 初始化各个部件
     public ShooterSubsystem() {
-        // 初始化飞轮电机
+        leftFlywheel = new TalonFX(Constants.Shooter.leftFlywheelID);
-        flywheelMotorLeft = new TalonFX(41);  // 41是飞轮left的CAN ID
+        rightFlywheel = new TalonFX(Constants.Shooter.rightFlywheelID);
-        flywheelMotorRight = new TalonFX(42);  // 42是飞轮right的CAN ID
+        leftConveyor = new TalonFX(Constants.Shooter.leftConveyorID);
-        flywheelMotorLeft.getConfigurator().apply(Constants.Shooter.flyWheelSlot0Configs);
+        rightConveyor = new TalonFX(Constants.Shooter.rightConveyorID);
-        flywheelMotorRight.getConfigurator().apply(Constants.Shooter.flyWheelSlot0Configs);
-        // 初始化传动电机
-        conveyorMotor = new TalonFX(43);
-        conveyorMotor.getConfigurator().apply(Constants.Shooter.conveyorSlot0Configs);
 
-        var configs1 = new MotorOutputConfigs();
+        leftFlywheel.getConfigurator().apply(Constants.Shooter.flyWheelSlot0Configs);
-        configs1.Inverted = InvertedValue.Clockwise_Positive;
+        rightFlywheel.getConfigurator().apply(Constants.Shooter.flyWheelSlot0Configs);
-        flywheelMotorLeft.getConfigurator().apply(configs1);
+        leftConveyor.getConfigurator().apply(Constants.Shooter.conveyorSlot0Configs);
-        flyWheelFollower = new Follower(flywheelMotorLeft.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50);
+        rightConveyor.getConfigurator().apply(Constants.Shooter.conveyorSlot0Configs);
-        
+
-        var configs2 = new MotorOutputConfigs();
+        var leftFlywheelMotorOutputConfigs = new MotorOutputConfigs();
-        configs2.Inverted = InvertedValue.Clockwise_Positive;
+        leftFlywheelMotorOutputConfigs.Inverted = InvertedValue.Clockwise_Positive;
-        conveyorMotor.getConfigurator().apply(configs2);
+        leftFlywheelMotorOutputConfigs.NeutralMode = NeutralModeValue.Coast;
-        
+        leftFlywheel.getConfigurator().apply(leftFlywheelMotorOutputConfigs);
-        // 初始化背板电机和编码器
+
-        backboardMotor = new TalonFXS(44);  
+        var rightFlywheelMotorOutputConfigs = new MotorOutputConfigs();
+        rightFlywheelMotorOutputConfigs.NeutralMode = NeutralModeValue.Coast;
+        rightFlywheel.getConfigurator().apply(rightFlywheelMotorOutputConfigs);
+
+        var conveyorMotorOutputConfigs = new MotorOutputConfigs();
+        conveyorMotorOutputConfigs.Inverted = InvertedValue.Clockwise_Positive;
+        leftConveyor.getConfigurator().apply(conveyorMotorOutputConfigs);
+
+        flywheelFollower = new Follower(leftFlywheel.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50);
+        conveyorFollower = new Follower(leftConveyor.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50);
+        setRightFlywheelFollowLeft();
+        setRightConveyorFollowLeft();
+
+        backboardMotor = new TalonFXS(Constants.Shooter.backboardMotorID);
         backboardMotor.getConfigurator().apply(Constants.Shooter.backboardSlot0Configs);
+        setBackboardBrakeMode();
+
         backboardEncoder = new Encoder(0, 1, false, Encoder.EncodingType.k4X);
         backboardEncoder.setSamplesToAverage(5);
         backboardEncoder.setMinRate(1.0);
         backboardEncoder.setDistancePerPulse(1);
-        
+
-        // 初始化背板的PID控制器
+        backboardPID = new ProfiledPIDController(
-        backboardPID = new ProfiledPIDController(Constants.Shooter.backboardPositionPID.kP, Constants.Shooter.backboardPositionPID.kI, Constants.Shooter.backboardPositionPID.kD,
+            Constants.Shooter.backboardPositionPID.kP,
-                                                    new Constraints(10000,100000));
+            Constants.Shooter.backboardPositionPID.kI,
-        
+            Constants.Shooter.backboardPositionPID.kD,
-        backboardPID.setTolerance(10.0); 
+            new Constraints(10000, 100000)
-        
+        );
+        backboardPID.setTolerance(10.0);
+
         shooterNetworkTable = NetworkTableInstance.getDefault().getTable("Shooter");
-
+        flywheelTuningTable = NetworkTableInstance.getDefault().getTable("Tuning/flywheel");
+        flywheelTuningTable.getEntry("flywheelVelocityEnable").setDefaultBoolean(false);
+        flywheelTuningTable.getEntry("flywheelTargetVelocity").setDefaultDouble(Constants.Shooter.defaultFlywheelVelocity);
+        flywheelTuningTable.getEntry("flywheelVelocityTolerance").setDefaultDouble(Constants.Shooter.flywheelReadyVelocityTolerance);
+        backboardTuningTable = NetworkTableInstance.getDefault().getTable("Tuning/backboard");
+        backboardTuningTable.getEntry("backboardPositionEnable").setDefaultBoolean(false);
+        backboardTuningTable.getEntry("backboardTargetPosition").setDefaultDouble(0.0);
+        shooterConveyorTuningTable = NetworkTableInstance.getDefault().getTable("Tuning/shooterConveyor");
+        shooterConveyorTuningTable.getEntry("shooterConveyorVelocityEnable").setDefaultBoolean(false);
+        shooterConveyorTuningTable.getEntry("shooterConveyorTargetVelocity")
+            .setDefaultDouble(Constants.Shooter.shooterConveyorFeedVelocity);
     }
 
-    public void setRightFollowLeft(){
+    /* ====================== */
-        flywheelMotorRight.setControl(flyWheelFollower);
+    /*         飞轮            */
+    /* ====================== */
+
+    public void setRightConveyorFollowLeft() {
+        // 成对机构机械方向相反，因此右侧电机以反向 follower 跟随左侧电机。
+        rightConveyor.setControl(conveyorFollower);
     }
 
-    public void applyLeftConfigurationToRight(){
+    public void setRightFlywheelFollowLeft() {
+        rightFlywheel.setControl(flywheelFollower);
+    }
+
+    public void applyLeftConfigurationToRight() {
         Slot0Configs configs = new Slot0Configs();
-        flywheelMotorLeft.getConfigurator().refresh(configs);
+        leftFlywheel.getConfigurator().refresh(configs);
-        flywheelMotorRight.getConfigurator().apply(configs);
+        rightFlywheel.getConfigurator().apply(configs);
     }
 
-
+    public void setFlywheelVelocity(double targetVelocityRps) {
-    // 设置飞轮的速度
+        flywheelTargetVelocity = targetVelocityRps;
-    public void setFlywheelSpeedByRPS(double targetSpeed) {
+        leftFlywheel.setControl(leftFlywheelVelocityRequest.withVelocity(flywheelTargetVelocity));
-        flywheelMotorLeft.setControl(velocityRequest.withVelocity(targetSpeed)); //RPS 转每秒
+        setRightFlywheelFollowLeft();
-        setRightFollowLeft();
-        // System.out.println("set flywheel "+targetSpeed);
     }
 
-    // 设置传动的速度
+    public void stopFlywheel() {
-    public void setConveyorSpeedByRPS(double speed) {
+        flywheelTargetVelocity = 0.0;
-        conveyorMotor.setControl(velocityRequest.withVelocity(speed));  // 设置传动电机速度
+        leftFlywheel.setControl(flywheelNeutralRequest);
+        rightFlywheel.setControl(flywheelNeutralRequest);
     }
 
-    //调试用
+    public boolean atVelocity() {
-    public void setBackboardSpeedByRPS(double speed){
+        if (flywheelTargetVelocity <= 10.0) {
-        backboardMotor.setControl(velocityRequest.withVelocity(speed));
+            return false;
+        }
+        double currentvelocity = getAverageFlywheelVelocity();
+        if(currentvelocity>flywheelTargetVelocity*0.95){
+            return true;
+        } else {
+            return false;
+        }   
+    }
+    
+    public double getLeftFlywheelVelocity() {
+        return leftFlywheel.getVelocity().getValueAsDouble();
+    }
+
+    public double getRightFlywheelVelocity() {
+        return rightFlywheel.getVelocity().getValueAsDouble();
+    }
+
+    public double getAverageFlywheelVelocity() {
+        return (Math.abs(getLeftFlywheelVelocity()) + Math.abs(getRightFlywheelVelocity())) / 2.0;
+    }
+
+    public double getFlywheelVelocityDifference() {
+        return Math.abs(getLeftFlywheelVelocity()) - Math.abs(getRightFlywheelVelocity());
+    }
+
+    public double getTargetFlywheelVelocity() {
+        return flywheelTargetVelocity;
+    }
+
+    public double getFlywheelVelocityError() {
+        return Math.abs(flywheelTargetVelocity) - getAverageFlywheelVelocity();
+    }
+
+    /* ====================== */
+    /*    shooter conveyor     */
+    /* ====================== */
+
+    public void runShooterConveyorVelocity(double targetVelocityRps) {
+        shooterConveyorTargetVelocity = targetVelocityRps;
+        leftConveyor.setControl(conveyorVelocityRequest.withVelocity(shooterConveyorTargetVelocity));
+        setRightConveyorFollowLeft();
+    }
+
+    public void stopShooterConveyor() {
+        shooterConveyorTargetVelocity = 0.0;
+        leftConveyor.setControl(conveyorNeutralRequest);
+        rightConveyor.setControl(conveyorNeutralRequest);
+    }
+
+    public double getShooterConveyorVelocity() {
+        return leftConveyor.getVelocity().getValueAsDouble();
+    }
+
+    public double getShooterConveyorTargetVelocity() {
+        return shooterConveyorTargetVelocity;
+    }
+
+    public double getShooterConveyorVelocityError() {
+        return shooterConveyorTargetVelocity - getShooterConveyorVelocity();
+    }
+
+    public double getConveyorStatorCurrent() {
+        return leftConveyor.getStatorCurrent().getValueAsDouble();
+    }
+
+    /* ====================== */
+    /*         背板            */
+    /* ====================== */
+
+    public void setBackboardVoltage(double outputVolts) {
+        backboardOutputVolts = MathUtil.clamp(
+            outputVolts,
+            -Constants.Shooter.backboardSpeedMax,
+            Constants.Shooter.backboardSpeedMax
+        );
+        backboardMotor.setControl(backboardVoltageRequest.withOutput(backboardOutputVolts));
     }
 
-    // 设置背板的目标位置，通过PID控制器来调整  范围0-3000
     public void setBackboardPosition(double targetPosition) {
-        // 限制目标位置在上下限之间
+        // 保留旧背板机构的软件限位。
         if (targetPosition > Constants.Shooter.backboardUpLimit) {
-            targetPosition = Constants.Shooter.backboardUpLimit;  // 限制最大上限
+            targetPosition = Constants.Shooter.backboardUpLimit;
         } else if (targetPosition < Constants.Shooter.backboardDownLimit) {
-            targetPosition = Constants.Shooter.backboardDownLimit;  // 限制最小下限
+            targetPosition = Constants.Shooter.backboardDownLimit;
         }
         backboardPID.setGoal(targetPosition);
     }
-    public void outputBackboard(){
 
-        // 使用PID控制器来计算背板的运动速度
+    public void outputBackboard() {
-        double pidOutput = backboardPID.calculate(getBackboardPosition());
+        // 背板仍使用旧外部编码器和 ProfiledPID；Superstructure 只选择目标位置。
-        pidOutput = Math.max(-Constants.Shooter.backboardSpeedMax, Math.min(Constants.Shooter.backboardSpeedMax, pidOutput)); // 限制输出范围
+        double backboardPosition = getBackboardPosition();
-        
+        double rawPidOutput = backboardPID.calculate(backboardPosition);
-        if (Math.abs(pidOutput) < 0.8) {
+        double pidOutput = MathUtil.clamp(
-        pidOutput = 0.0;
+            rawPidOutput,
-        }//防止抖动
+            -Constants.Shooter.backboardSpeedMax,
-        // // 设置背板电机的速度，根据PID输出进行调整
+            Constants.Shooter.backboardSpeedMax
-        shooterNetworkTable.getEntry("backboardPIDOutput").setDouble(pidOutput);
+        );
-        setBackboardSpeedByRPS(pidOutput);
+        backboardRawPidOutputVolts = rawPidOutput;
+        backboardClampedPidOutputVolts = pidOutput;
+        backboardOutputSaturated = Math.abs(rawPidOutput - pidOutput) > 1e-9;
+
+        if (pidOutput != 0.0 && Math.abs(pidOutput) < Constants.Shooter.backboardSpeedMin) {
+            pidOutput = Math.copySign(Constants.Shooter.backboardSpeedMin, pidOutput);
+            backboardMinimumOutputActive = true;
+        } else {
+            backboardMinimumOutputActive = false;
+        }
+
+        double gravityOutput = calculateBackboardGravityFeedforward(backboardPosition);
+        double outputVolts = MathUtil.clamp(
+            pidOutput + gravityOutput,
+            -Constants.Shooter.backboardSpeedMax,
+            Constants.Shooter.backboardSpeedMax
+        );
+        backboardGravityFeedforwardVolts = gravityOutput;
+        backboardOutputSaturated = backboardOutputSaturated
+            || Math.abs((pidOutput + gravityOutput) - outputVolts) > 1e-9;
+
+        shooterNetworkTable.getEntry("backboardPIDOutput").setDouble(outputVolts);
+        setBackboardVoltage(outputVolts);
     }
-    // 停止所有电机
+
-    public void stopMotors() {
+    private double calculateBackboardGravityFeedforward(double backboardPosition) {
-        flywheelMotorLeft.set(0);
+        double clampedPosition = MathUtil.clamp(
-        flywheelMotorRight.set(0);
+            backboardPosition,
-        conveyorMotor.set(0);
+            Constants.Shooter.backboardGravityFeedforwardMinPosition,
-        backboardMotor.set(0);
+            Constants.Shooter.backboardGravityFeedforwardMaxPosition
+        );
+        double positionRange = Constants.Shooter.backboardGravityFeedforwardMaxPosition
+            - Constants.Shooter.backboardGravityFeedforwardMinPosition;
+        if (positionRange <= 0.0) {
+            return 0.0;
+        }
+
+        double positionRatio = (clampedPosition - Constants.Shooter.backboardGravityFeedforwardMinPosition)
+            / positionRange;
+        double feedforwardVolts = Constants.Shooter.backboardGravityFeedforwardMinVolts
+            + positionRatio
+                * (Constants.Shooter.backboardGravityFeedforwardMaxVolts
+                    - Constants.Shooter.backboardGravityFeedforwardMinVolts);
+        return Constants.Shooter.backboardGravityFeedforwardDirection * feedforwardVolts;
+    }
+
+    public void holdBackboardAt(double targetPosition) {
+        // 上层只给目标位置；到位后仍继续输出重力补偿。
+        setBackboardPosition(targetPosition);
+        outputBackboard();
+    }
+
+    public void stopBackboard() {
+        backboardRawPidOutputVolts = 0.0;
+        backboardGravityFeedforwardVolts = 0.0;
+        backboardClampedPidOutputVolts = 0.0;
+        backboardOutputSaturated = false;
+        backboardMinimumOutputActive = false;
+        setBackboardVoltage(0.0);
+    }
+
+    public void setBackboardBrakeMode() {
+        var backboardMotorOutputConfigs = new MotorOutputConfigs();
+        backboardMotorOutputConfigs.NeutralMode = NeutralModeValue.Brake;
+        backboardMotor.getConfigurator().apply(backboardMotorOutputConfigs);
     }
 
-    // 获取背板当前角度
     public double getBackboardPosition() {
-        return backboardEncoder.getDistance();  // 返回背板电机编码器的当前角度
+        return backboardEncoder.getDistance();
     }
 
-    // 检查背板是否到达目标角度
     public boolean isBackboardAtTarget() {
-        return backboardPID.atGoal();  // 判断背板是否到达目标角度
+        return backboardPID.atGoal();
     }
-    public void conveyorWaitForAcceleration(){
+
-        conveyorMotor.set(0);
+    public double getBackboardGoalPosition() {
+        return backboardPID.getGoal().position;
     }
-    public void conveyorRun(){
+
-        setConveyorSpeedByRPS(Constants.Shooter.conveyorSpeed);
+    public double getBackboardPositionError() {
+        return getBackboardGoalPosition() - getBackboardPosition();
+    }
+
+    public boolean isBackboardNearTarget(double tolerance) {
+        return Math.abs(getBackboardPositionError()) < tolerance;
+    }
+
+    public boolean isBackboardPositionTuningActive() {
+        return DriverStation.isTest()
+            && backboardTuningTable.getEntry("backboardPositionEnable").getBoolean(false);
+    }
+
+    public boolean isFlywheelTuningControlActive() {
+        return DriverStation.isTest()
+            && flywheelTuningTable.getEntry("flywheelVelocityEnable").getBoolean(false);
+    }
+
+    public boolean isShooterConveyorTuningControlActive() {
+        return DriverStation.isTestEnabled()
+            && shooterConveyorTuningTable.getEntry("shooterConveyorVelocityEnable").getBoolean(false);
+    }
+
+    public void resetbackboardencoder() {
+        backboardEncoder.reset();
+        backboardPID.reset(0);
+    }
+
+    /**
+     * 背板回零命令：闭环回到 0 位后停止输出。
+     * 命令留在 subsystem 内，controls 层只负责绑定按键。
+     */
+    public Command homeBackboardCommand() {
+        return Commands.sequence(
+            Commands.runOnce(() -> setBackboardPosition(0.0), this),
+            Commands.run(() -> holdBackboardAt(0.0), this).until(this::isBackboardAtTarget),
+            Commands.runOnce(this::stopBackboard, this)
+        );
+    }
+
+    /* ====================== */
+    /*         整体            */
+    /* ====================== */
+
+    public void stopAll() {
+        stopFlywheel();
+        stopShooterConveyor();
+        stopBackboard();
+    }
+
+    public void debugBackboardTargetPeriodic(boolean allowMotorOutput) {
+        boolean positionEnable =
+            allowMotorOutput && backboardTuningTable.getEntry("backboardPositionEnable").getBoolean(false);
+        double requestedTarget = backboardTuningTable.getEntry("backboardTargetPosition").getDouble(0.0);
+        double clampedTarget = MathUtil.clamp(
+            requestedTarget,
+            Constants.Shooter.backboardDownLimit,
+            Constants.Shooter.backboardUpLimit
+        );
+
+        backboardTuningTable.getEntry("backboardPositionEnableActive").setBoolean(positionEnable);
+        backboardTuningTable.getEntry("backboardCommandedTargetPosition").setDouble(requestedTarget);
+        backboardTuningTable.getEntry("backboardClampedTargetPosition").setDouble(clampedTarget);
+
+        if (positionEnable) {
+            holdBackboardAt(clampedTarget);
+        }
+    }
+
+    public void debugFlywheelVelocityPeriodic(boolean allowMotorOutput) {
+        flywheelVelocityTuningActive =
+            allowMotorOutput && flywheelTuningTable.getEntry("flywheelVelocityEnable").getBoolean(false);
+        double requestedTargetVelocity =
+            flywheelTuningTable.getEntry("flywheelTargetVelocity").getDouble(Constants.Shooter.defaultFlywheelVelocity);
+        flywheelVelocityTolerance = Math.max(
+            0.0,
+            flywheelTuningTable.getEntry("flywheelVelocityTolerance")
+                .getDouble(Constants.Shooter.flywheelReadyVelocityTolerance)
+        );
+
+        double clampedTargetVelocity = MathUtil.clamp(
+            requestedTargetVelocity,
+            -Constants.Shooter.flywheelMaxVelocity,
+            Constants.Shooter.flywheelMaxVelocity
+        );
+        flywheelTuningTable.getEntry("flywheelVelocityEnableActive").setBoolean(flywheelVelocityTuningActive);
+        flywheelTuningTable.getEntry("flywheelCommandedTargetVelocity").setDouble(requestedTargetVelocity);
+        flywheelTuningTable.getEntry("flywheelClampedTargetVelocity").setDouble(clampedTargetVelocity);
+        flywheelTuningTable.getEntry("flywheelVelocityToleranceActive").setDouble(flywheelVelocityTolerance);
+
+        if (flywheelVelocityTuningActive) {
+            setFlywheelVelocity(clampedTargetVelocity);
+        }
+    }
+
+    public void debugShooterConveyorVelocityPeriodic(boolean allowMotorOutput) {
+        shooterConveyorVelocityTuningActive =
+            allowMotorOutput
+                && DriverStation.isTestEnabled()
+                && shooterConveyorTuningTable.getEntry("shooterConveyorVelocityEnable").getBoolean(false);
+        shooterConveyorCommandedTargetVelocity = shooterConveyorTuningTable
+            .getEntry("shooterConveyorTargetVelocity")
+            .getDouble(Constants.Shooter.shooterConveyorFeedVelocity);
+        shooterConveyorClampedTargetVelocity = MathUtil.clamp(
+            shooterConveyorCommandedTargetVelocity,
+            -Constants.Shooter.shooterConveyorMaxVelocity,
+            Constants.Shooter.shooterConveyorMaxVelocity
+        );
+
+        shooterConveyorTuningTable.getEntry("shooterConveyorVelocityEnableActive")
+            .setBoolean(shooterConveyorVelocityTuningActive);
+        shooterConveyorTuningTable.getEntry("shooterConveyorCommandedTargetVelocity")
+            .setDouble(shooterConveyorCommandedTargetVelocity);
+        shooterConveyorTuningTable.getEntry("shooterConveyorClampedTargetVelocity")
+            .setDouble(shooterConveyorClampedTargetVelocity);
+        shooterConveyorTuningTable.getEntry("shooterConveyorMaxVelocity")
+            .setDouble(Constants.Shooter.shooterConveyorMaxVelocity);
+
+        if (shooterConveyorVelocityTuningActive) {
+            runShooterConveyorVelocity(shooterConveyorClampedTargetVelocity);
+        }
     }
 
     @Override
     public void periodic() {
-        backboardPID.calculate(getBackboardPosition());
+        double backboardPosition = getBackboardPosition();
-        shooterNetworkTable.getEntry("flywheelSpeed").setDouble(flywheelMotorLeft.getVelocity().getValueAsDouble());
+        double backboardGoalPosition = backboardPID.getGoal().position;
-        shooterNetworkTable.getEntry("flywheelPIDReference").setDouble(flywheelMotorLeft.getClosedLoopReference().getValueAsDouble());
+        double backboardSetpointPosition = backboardPID.getSetpoint().position;
-        shooterNetworkTable.getEntry("conveyorSpeed").setDouble(conveyorMotor.getVelocity().getValueAsDouble());
+        flywheelOutputVolts = leftFlywheel.getMotorVoltage().getValueAsDouble();
-        shooterNetworkTable.getEntry("conveyorPIDReference").setDouble(conveyorMotor.getClosedLoopReference().getValueAsDouble());
+        shooterNetworkTable.getEntry("flywheelVelocity").setDouble(getAverageFlywheelVelocity());
-        shooterNetworkTable.getEntry("backboardCurrentRate").setDouble(getBackboardPosition());
+        shooterNetworkTable.getEntry("leftFlywheelVelocity").setDouble(getLeftFlywheelVelocity());
-        shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardPID.getSetpoint().position);
+        shooterNetworkTable.getEntry("rightFlywheelVelocity").setDouble(getRightFlywheelVelocity());
+        shooterNetworkTable.getEntry("flywheelVelocityDifference").setDouble(getFlywheelVelocityDifference());
+        shooterNetworkTable.getEntry("flywheelTargetVelocity").setDouble(getTargetFlywheelVelocity());
+        shooterNetworkTable.getEntry("flywheelVelocityError").setDouble(getFlywheelVelocityError());
+        shooterNetworkTable.getEntry("flywheelVelocityTolerance").setDouble(flywheelVelocityTolerance);
+        shooterNetworkTable.getEntry("flywheelAtVelocity").setBoolean(atVelocity());
+        shooterNetworkTable.getEntry("flywheelVelocityTuningActive").setBoolean(flywheelVelocityTuningActive);
+        shooterNetworkTable.getEntry("flywheelFollowerOpposed").setBoolean(true);
+        shooterNetworkTable.getEntry("flywheelOutputVolts").setDouble(flywheelOutputVolts);
+        shooterNetworkTable.getEntry("leftFlywheelOutputVolts").setDouble(leftFlywheel.getMotorVoltage().getValueAsDouble());
+        shooterNetworkTable.getEntry("rightFlywheelOutputVolts").setDouble(rightFlywheel.getMotorVoltage().getValueAsDouble());
+        shooterNetworkTable.getEntry("leftFlywheelStatorCurrent").setDouble(leftFlywheel.getStatorCurrent().getValueAsDouble());
+        shooterNetworkTable.getEntry("rightFlywheelStatorCurrent").setDouble(rightFlywheel.getStatorCurrent().getValueAsDouble());
+        shooterConveyorOutputVolts = leftConveyor.getMotorVoltage().getValueAsDouble();
+        shooterNetworkTable.getEntry("shooterConveyorOutputVolts").setDouble(shooterConveyorOutputVolts);
+        shooterNetworkTable.getEntry("conveyorTargetVelocity").setDouble(getShooterConveyorTargetVelocity());
+        shooterNetworkTable.getEntry("conveyorVelocity").setDouble(getShooterConveyorVelocity());
+        shooterNetworkTable.getEntry("conveyorVelocityError").setDouble(getShooterConveyorVelocityError());
+        shooterNetworkTable.getEntry("conveyorVelocityTuningActive").setBoolean(shooterConveyorVelocityTuningActive);
+        shooterNetworkTable.getEntry("conveyorCommandedTargetVelocity").setDouble(shooterConveyorCommandedTargetVelocity);
+        shooterNetworkTable.getEntry("conveyorClampedTargetVelocity").setDouble(shooterConveyorClampedTargetVelocity);
+        shooterNetworkTable.getEntry("conveyorLeftStatorCurrent").setDouble(leftConveyor.getStatorCurrent().getValueAsDouble());
+        shooterNetworkTable.getEntry("conveyorRightStatorCurrent").setDouble(rightConveyor.getStatorCurrent().getValueAsDouble());
+        shooterNetworkTable.getEntry("backboardCurrentRate").setDouble(backboardPosition);
+        shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardSetpointPosition);
+        shooterNetworkTable.getEntry("backboardOutputVolts").setDouble(backboardOutputVolts);
         shooterNetworkTable.getEntry("isBackboardAtTarget").setBoolean(isBackboardAtTarget());
-        shooterNetworkTable.getEntry("distanceGetted").setDouble(getDistance());
+        shooterNetworkTable.getEntry("backboardGoalPosition").setDouble(backboardGoalPosition);
-    }
+        shooterNetworkTable.getEntry("backboardSetpointPosition").setDouble(backboardSetpointPosition);
-
+        shooterNetworkTable.getEntry("backboardSetpointVelocity").setDouble(backboardPID.getSetpoint().velocity);
-    public double getDistance(){
+        shooterNetworkTable.getEntry("backboardGoalError").setDouble(backboardGoalPosition - backboardPosition);
-        //获得车子距离Hub的距离
+        shooterNetworkTable.getEntry("backboardSetpointError").setDouble(backboardSetpointPosition - backboardPosition);
-        return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToHub").getDouble(0.0);
+        shooterNetworkTable.getEntry("backboardRawPIDOutput").setDouble(backboardRawPidOutputVolts);
+        shooterNetworkTable.getEntry("backboardGravityFeedforward").setDouble(backboardGravityFeedforwardVolts);
+        shooterNetworkTable.getEntry("backboardClampedPIDOutput").setDouble(backboardClampedPidOutputVolts);
+        shooterNetworkTable.getEntry("backboardOutputSaturated").setBoolean(backboardOutputSaturated);
+        shooterNetworkTable.getEntry("backboardMinimumOutputActive").setBoolean(backboardMinimumOutputActive);
+        shooterNetworkTable.getEntry("backboardMaxOutputVolts").setDouble(Constants.Shooter.backboardSpeedMax);
+        shooterNetworkTable.getEntry("backboardMinOutputVolts").setDouble(Constants.Shooter.backboardSpeedMin);
+        shooterNetworkTable.getEntry("backboardGravityFeedforwardMinVolts").setDouble(Constants.Shooter.backboardGravityFeedforwardMinVolts);
+        shooterNetworkTable.getEntry("backboardGravityFeedforwardMaxVolts").setDouble(Constants.Shooter.backboardGravityFeedforwardMaxVolts);
+        shooterNetworkTable.getEntry("backboardPositionTolerance").setDouble(10.0);
+        shooterNetworkTable.getEntry("backboardShootPosition").setDouble(Constants.Superstructure.backboardShootPosition);
+        shooterNetworkTable.getEntry("backboardPassPosition").setDouble(Constants.Superstructure.passBallBackboardPosition);
+        shooterNetworkTable.getEntry("backboardMotorPosition").setDouble(backboardMotor.getPosition().getValueAsDouble());
+        shooterNetworkTable.getEntry("backboardMotorVelocity").setDouble(backboardMotor.getVelocity().getValueAsDouble());
+        shooterNetworkTable.getEntry("backboardMotorVoltage").setDouble(backboardMotor.getMotorVoltage().getValueAsDouble());
+        shooterNetworkTable.getEntry("backboardMotorStatorCurrent").setDouble(backboardMotor.getStatorCurrent().getValueAsDouble());
+        shooterNetworkTable.getEntry("statorCurrent").setDouble(getConveyorStatorCurrent());
     }
 }

src/main/java/frc/robot/subsystems/Superstructure.java

@@ -0,0 +1,476 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+/**
+ * 统一协调所有得分相关机构。
+ *
+ * subsystem 只暴露硬件级动作，OperatorControls 只表达操作员意图；
+ * shooter、intake、conveyor、底盘瞄准和 Limelight 距离如何配合，
+ * 统一由这里决定。底盘瞄准本身仍由 DriveControls 的 AutoAimming
+ * 模式完成，这里只读取 drive.isAimed() 等状态，不直接控制底盘电机。
+ */
+public class Superstructure extends SubsystemBase {
+    // 操作员或自动程序请求的目标状态。
+    public enum WantedState {
+        INIT,
+        IDLE,
+        INTAKE,
+        SHOOT_HUB,
+        AIM_HUB,
+        PASS_BALL,
+        EJECT,
+        MANUAL
+    }
+
+    // 机器人内部实际执行状态，用于动作排序和仪表盘诊断。
+    public enum SystemState {
+        DISABLED,
+        INITIALIZING,
+        STOWED,
+        INTAKING,
+        PREP_SHOOT,
+        SHOOTING,
+        EJECTING,
+        MANUAL_OVERRIDE
+    }
+
+    private final ShooterSubsystem shooter;
+    private final IntakeRollerSubsystem intakeRoller;
+    private final IntakeRotaterSubsystem intakeRotater;
+    private final ConveyorSubsystem conveyor;
+    private final CommandSwerveDrivetrain drive;
+    private final LimelightSubsystem limelight;
+    private final NetworkTable table = NetworkTableInstance.getDefault().getTable("Superstructure");
+
+    private WantedState wantedState = WantedState.IDLE;
+    private SystemState systemState = SystemState.STOWED;
+    private double stateStartTimestamp = Timer.getFPGATimestamp();
+    private double flywheelVelocityOffset = 0.0;
+    private double backboardPositionOffset = 0.0;
+    private boolean shotCompletedThisRequest = false;
+    private double postShootIdleAssistStartTimestamp = Double.NaN;
+
+    public Superstructure(
+        ShooterSubsystem shooter,
+        IntakeRollerSubsystem intakeRoller,
+        IntakeRotaterSubsystem intakeRotater,
+        ConveyorSubsystem conveyor,
+        CommandSwerveDrivetrain drive,
+        LimelightSubsystem limelight
+    ) {
+        this.shooter = shooter;
+        this.intakeRoller = intakeRoller;
+        this.intakeRotater = intakeRotater;
+        this.conveyor = conveyor;
+        this.drive = drive;
+        this.limelight = limelight;
+    }
+
+    /* ====================== */
+    /*     操作员意图入口       */
+    /* ====================== */
+
+    public void requestIdle() {
+        if (systemState == SystemState.SHOOTING) {
+            postShootIdleAssistStartTimestamp = Timer.getFPGATimestamp();
+        }
+        wantedState = WantedState.IDLE;
+    }
+
+    public void requestInit() {
+        wantedState = WantedState.INIT;
+    }
+
+    public void requestIntake() {
+        wantedState = WantedState.INTAKE;
+    }
+
+    public void requestShootHub() {
+        if (wantedState != WantedState.SHOOT_HUB) {
+            shotCompletedThisRequest = false;
+        }
+        wantedState = WantedState.SHOOT_HUB;
+    }
+
+    public void requestAimHub() {
+        wantedState = WantedState.AIM_HUB;
+    }
+
+    public void requestPassBall() {
+        if (wantedState != WantedState.PASS_BALL) {
+            shotCompletedThisRequest = false;
+        }
+        wantedState = WantedState.PASS_BALL;
+    }
+
+    public void requestShoot() {
+        requestShootHub();
+    }
+
+    public void requestEject() {
+        wantedState = WantedState.EJECT;
+    }
+
+    public void requestManual() {
+        wantedState = WantedState.MANUAL;
+    }
+
+    public Command requestIdleCommand() {
+        return Commands.runOnce(this::requestIdle, this);
+    }
+
+    public Command requestInitCommand() {
+        return Commands.sequence(
+            Commands.runOnce(this::requestInit),
+            intakeRotater.lowerForMatchCommand(),
+            Commands.runOnce(this::requestIntake)
+        );
+    }
+
+    public Command requestIntakeCommand() {
+        return Commands.runOnce(this::requestIntake, this);
+    }
+
+    public Command requestShootHubCommand() {
+        return Commands.runOnce(this::requestShootHub, this);
+    }
+
+    public Command requestAimHubCommand() {
+        return Commands.runOnce(this::requestAimHub, this);
+    }
+
+    public Command requestPassBallCommand() {
+        return Commands.runOnce(this::requestPassBall, this);
+    }
+
+    public Command requestShootCommand() {
+        return requestShootHubCommand();
+    }
+
+    public Command requestEjectCommand() {
+        return Commands.runOnce(this::requestEject, this);
+    }
+
+    public Command requestManualCommand() {
+        return Commands.runOnce(this::requestManual, this);
+    }
+
+    public WantedState getWantedState() {
+        return wantedState;
+    }
+
+    public SystemState getSystemState() {
+        return systemState;
+    }
+
+    public boolean isShootCycleComplete() {
+        return shotCompletedThisRequest;
+    }
+
+    /* ====================== */
+    /*        射击门控          */
+    /* ====================== */
+
+    public boolean canShoot() {
+        return shooter.atVelocity()
+            && drive.isAimed()
+            && isBackboardReady();
+    }
+
+    private boolean isDriveStationary() {
+        ChassisSpeeds speeds = drive.getChassisSpeeds();
+        double translationSpeed = Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond);
+        return translationSpeed <= Constants.Superstructure.stationarySpeedToleranceMetersPerSecond;
+    }
+
+    private boolean isBackboardReady() {
+        return shooter.isBackboardNearTarget(Constants.Superstructure.backboardReadyPositionTolerance);
+    }
+
+    /* ====================== */
+    /*      现场微调 offset     */
+    /* ====================== */
+
+    public void adjustFlywheelVelocityOffset(double offsetVelocity) {
+        flywheelVelocityOffset += offsetVelocity;
+    }
+
+    public void adjustFlywheelVoltageOffset(double offsetVolts) {
+        adjustFlywheelVelocityOffset(offsetVolts / Constants.Shooter.flywheelNominalKv);
+    }
+
+    public void adjustBackboardPositionOffset(double offset) {
+        backboardPositionOffset += offset;
+    }
+
+    public void resetOffsets() {
+        flywheelVelocityOffset = 0.0;
+        backboardPositionOffset = 0.0;
+    }
+
+    public void resetBackboardEncoder() {
+        shooter.resetbackboardencoder();
+    }
+
+    /* ====================== */
+    /*        状态机主体        */
+    /* ====================== */
+
+    @Override
+    public void periodic() {
+        // 禁用状态优先级最高，防止旧按钮输入或自动请求在禁用后继续驱动机构。
+        if (DriverStation.isDisabled()) {
+            setSystemState(SystemState.DISABLED);
+            stopAllMechanisms();
+            publishTelemetry();
+            return;
+        }
+
+        switch (wantedState) {
+            case INIT -> handleInit();
+            case IDLE -> handleIdle();
+            case INTAKE -> handleIntake();
+            case SHOOT_HUB -> handleShootHub();
+            case AIM_HUB -> handleAimHub();
+            case PASS_BALL -> handlePassBall();
+            case EJECT -> handleEject();
+            case MANUAL -> handleManual();
+        }
+
+        publishTelemetry();
+    }
+
+    private void handleInit() {
+        if (systemState != SystemState.INITIALIZING) {
+            setSystemState(SystemState.INITIALIZING);
+        }
+
+        stopMainConveyorUnlessTuning();
+        intakeRoller.stop();
+        stopFlywheelUnlessTuning();
+        stopShooterConveyorUnlessTuning();
+        holdBackboardStowedUnlessTuning();
+    }
+
+    private void handleIdle() {
+        // 安全默认状态：球路不动，intaker 不主动动作，背板保持受控。
+        stopMainConveyorUnlessTuning();
+        intakeRoller.stop();
+        if (!runPostShootIdleAssist()) {
+            intakeRotater.stop();
+        }
+        stopFlywheelUnlessTuning();
+        stopShooterConveyorUnlessTuning();
+        holdBackboardStowedUnlessTuning();
+        setSystemState(SystemState.STOWED);
+    }
+
+    private boolean runPostShootIdleAssist() {
+        if (Double.isNaN(postShootIdleAssistStartTimestamp)) {
+            return false;
+        }
+
+        double elapsed = Timer.getFPGATimestamp() - postShootIdleAssistStartTimestamp;
+        if (elapsed >= 0.6) {
+            postShootIdleAssistStartTimestamp = Double.NaN;
+            return false;
+        }
+
+        intakeRotater.postShootIdleAssist(elapsed);
+        return true;
+    }
+
+    private void handleIntake() {
+        stopFlywheelUnlessTuning();
+        stopShooterConveyorUnlessTuning();
+        holdBackboardStowedUnlessTuning();
+        // Intaker 比赛开始后保持下放，收球何时结束由操作员松开按键决定。
+        intakeRoller.intake();
+        intakeFeedMainConveyorUnlessTuning();
+        setSystemState(SystemState.INTAKING);
+    }
+
+    private void handleAimHub() {
+        intakeRoller.stop();
+        stopMainConveyorUnlessTuning();
+        stopFlywheelUnlessTuning();
+        stopShooterConveyorUnlessTuning();
+        holdBackboardStowedUnlessTuning();
+        setSystemState(SystemState.STOWED);
+    }
+
+    private void handleShootHub() {
+        double x = NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToHub").getDouble(0.0);
+        x = MathUtil.clamp(x, 1.2, 4.5);
+        double rawtargetVelocity = -0.9702766369910669*x*x*x*x*x*x+16.17984114722666*x*x*x*x*x-107.89020547785285*x*x*x*x+365.6895126610877*x*x*x-659.1553030522557*x*x+598.8266122952737*x-188.96203796840197;
+        double targetVelocity = rawtargetVelocity + flywheelVelocityOffset;
+        double rawtargetbackboardPosition = +72.63723930800967*x*x*x*x*x*x-1226.7072434356646*x*x*x*x*x+8380.732539436454*x*x*x*x-29585.06108203164*x*x*x+56753.91414329895*x*x-55469.56099616276*x+21418.08191862802;
+        rawtargetbackboardPosition = MathUtil.clamp(rawtargetbackboardPosition, 0, 2200);
+        double backboardPosition = rawtargetbackboardPosition + backboardPositionOffset;
+        handlePreparedFeed(targetVelocity, backboardPosition);
+    }
+
+    private void handlePassBall() {
+        double targetVelocity = 30;
+        double backboardPosition = 2000;
+        handlePreparedFeed(targetVelocity, backboardPosition);
+    }
+
+    private void handlePreparedFeed(double targetVelocity, double backboardPosition) {
+        if (systemState != SystemState.PREP_SHOOT
+            && systemState != SystemState.SHOOTING) {
+            setSystemState(SystemState.PREP_SHOOT);
+        }
+        // 距离到射速的映射由 LimelightSubsystem 提供；操作员 offset 用于现场微调，
+        // 但不会绕过 Superstructure 状态机。
+        shooter.setFlywheelVelocity(targetVelocity);
+        shooter.holdBackboardAt(backboardPosition);
+
+        if (systemState == SystemState.SHOOTING) {
+            // 只有 canShoot() 通过后才会进入本状态；喂球期间不再复查门控，
+            // 避免球接触飞轮导致的掉速中断喂球。
+            intakeRotater.shootAssist();
+            intakeRoller.intakeforshoot();
+            feedMainConveyorUnlessTuning();
+            runShooterConveyorVelocityUnlessTuning(Constants.Superstructure.shooterFeedVelocity);
+            return;
+        }
+
+        // 未开始喂球前，球路保持静止。
+        stopMainConveyorUnlessTuning();
+        stopShooterConveyorUnlessTuning();
+        intakeRoller.stop();
+
+        // Keep normal gating, but force the shot if prep takes too long.
+        if (canShoot() || prepShootFallbackElapsed()) {
+            setSystemState(SystemState.SHOOTING);
+        }
+    }
+
+    private boolean prepShootFallbackElapsed() {
+        return systemState == SystemState.PREP_SHOOT
+            && timeInState() >= Constants.Superstructure.prepShootFallbackDelaySeconds;
+    }
+
+    private void handleEject() {
+        // Eject 反转整条球路，飞轮保持停止。
+        setSystemState(SystemState.EJECTING);
+        intakeRoller.outtake();
+        reverseMainConveyorUnlessTuning();
+        runShooterConveyorVelocityUnlessTuning(Constants.Superstructure.shooterReverseVelocity);
+        stopFlywheelUnlessTuning();
+        holdBackboardStowedUnlessTuning();
+    }
+
+    private void holdBackboardStowedUnlessTuning() {
+        if (!shooter.isBackboardPositionTuningActive()) {
+            shooter.holdBackboardAt(0.0);
+        }
+    }
+
+    private void stopFlywheelUnlessTuning() {
+        if (!shooter.isFlywheelTuningControlActive()) {
+            shooter.stopFlywheel();
+        }
+    }
+
+    private void runShooterConveyorVelocityUnlessTuning(double targetVelocity) {
+        if (!shooter.isShooterConveyorTuningControlActive()) {
+            shooter.runShooterConveyorVelocity(targetVelocity);
+        }
+    }
+
+    private void stopShooterConveyorUnlessTuning() {
+        if (!shooter.isShooterConveyorTuningControlActive()) {
+            shooter.stopShooterConveyor();
+        }
+    }
+
+    private void feedMainConveyorUnlessTuning() {
+        if (!conveyor.isMainConveyorTuningControlActive()) {
+            conveyor.feedToShooter();
+        }
+    }
+
+    private void intakeFeedMainConveyorUnlessTuning() {
+        if (!conveyor.isMainConveyorTuningControlActive()) {
+            conveyor.feedForIntake();
+        }
+    }
+
+    private void reverseMainConveyorUnlessTuning() {
+        if (!conveyor.isMainConveyorTuningControlActive()) {
+            conveyor.reverse();
+        }
+    }
+
+    private void stopMainConveyorUnlessTuning() {
+        if (!conveyor.isMainConveyorTuningControlActive()) {
+            conveyor.stop();
+        }
+    }
+
+    private void handleManual() {
+        // 手动模式：状态机不再驱动机构，交给现场手动/调参工具；
+        // 只保持飞轮与喂球停止，避免意外射出。
+        setSystemState(SystemState.MANUAL_OVERRIDE);
+        stopFlywheelUnlessTuning();
+        stopShooterConveyorUnlessTuning();
+        stopMainConveyorUnlessTuning();
+        intakeRoller.stop();
+    }
+
+    private void stopAllMechanisms() {
+        shooter.stopAll();
+        intakeRoller.stop();
+        intakeRotater.stop();
+        conveyor.stop();
+    }
+
+    private void setSystemState(SystemState nextState) {
+        if (systemState != nextState) {
+            systemState = nextState;
+            stateStartTimestamp = Timer.getFPGATimestamp();
+            if (nextState == SystemState.SHOOTING) {
+                shooter.setBackboardBrakeMode();
+            }
+        }
+    }
+
+    private double timeInState() {
+        return Timer.getFPGATimestamp() - stateStartTimestamp;
+    }
+
+    private void publishTelemetry() {
+        // 发布状态和各项门控信号，方便判断机器人为什么还没进入射击。
+        table.getEntry("wantedState").setString(wantedState.name());
+        table.getEntry("systemState").setString(systemState.name());
+        table.getEntry("timeInState").setDouble(timeInState());
+        table.getEntry("canShoot").setBoolean(canShoot());
+        table.getEntry("shooterAtVelocity").setBoolean(shooter.atVelocity());
+        table.getEntry("driveAimed").setBoolean(drive.isAimed());
+        table.getEntry("driveHeadingError").setDouble(drive.getHeadingError());
+        table.getEntry("driveStationary").setBoolean(isDriveStationary());
+        table.getEntry("backboardReady").setBoolean(isBackboardReady());
+        table.getEntry("backboardPositionError").setDouble(shooter.getBackboardPositionError());
+        table.getEntry("backboardReadyTolerance").setDouble(Constants.Superstructure.backboardReadyPositionTolerance);
+        table.getEntry("flywheelVelocityOffset").setDouble(flywheelVelocityOffset);
+        table.getEntry("flywheelTuningControlActive").setBoolean(shooter.isFlywheelTuningControlActive());
+        table.getEntry("shooterConveyorTuningControlActive")
+            .setBoolean(shooter.isShooterConveyorTuningControlActive());
+        table.getEntry("mainConveyorTuningControlActive")
+            .setBoolean(conveyor.isMainConveyorTuningControlActive());
+        table.getEntry("flywheelVoltageOffset").setDouble(flywheelVelocityOffset * Constants.Shooter.flywheelNominalKv);
+        table.getEntry("backboardPositionOffset").setDouble(backboardPositionOffset);
+    }
+}

src/main/java/frc/robot/tuning/ConfigTalonFXMotorTuner.java

@@ -1,9 +1,8 @@
 package frc.robot.tuning;
-import java.util.function.Function;
-
 import com.ctre.phoenix6.configs.Slot0Configs;
-import com.ctre.phoenix6.controls.VelocityDutyCycle;
+import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.TalonFX;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.networktables.*;
 
 public class ConfigTalonFXMotorTuner {
@@ -11,6 +10,7 @@ public class ConfigTalonFXMotorTuner {
   private final String motorName;
   private final Slot0Configs motorSlot0Configs;
   private final NetworkTable table;
+  private final boolean voltageRunEnabled;
 
   // ---- Subscribers：读取 Elastic 改过的新值 ----
   private final BooleanSubscriber enableSub;
@@ -22,14 +22,25 @@ public class ConfigTalonFXMotorTuner {
   private final DoubleSubscriber KsSub;
 
   private final BooleanSubscriber enableRuningSub;
-  private final DoubleSubscriber runningSpeedRPS;
+  private final DoubleSubscriber runningVolts;
   private double lastKp = 0, lastKi = 0, lastKd = 0, lastKv = 0, lastKa = 0, lastKs = 0;
+  private boolean wasRunning = false;
   
-  private final VelocityDutyCycle velocityRequest = new VelocityDutyCycle(0);
+  private final VoltageOut voltageRequest = new VoltageOut(0);
   public ConfigTalonFXMotorTuner(TalonFX motor_, String motorName_, Slot0Configs motorSlot0Configs_) {
+    this(motor_, motorName_, motorSlot0Configs_, true);
+  }
+
+  public ConfigTalonFXMotorTuner(
+      TalonFX motor_,
+      String motorName_,
+      Slot0Configs motorSlot0Configs_,
+      boolean voltageRunEnabled_
+  ) {
     this.motor = motor_;
     this.motorName = motorName_;
     this.motorSlot0Configs = motorSlot0Configs_;
+    this.voltageRunEnabled = voltageRunEnabled_;
 
     this.table = NetworkTableInstance.getDefault().getTable("Tuning/" + motorName);
 
@@ -41,8 +52,10 @@ public class ConfigTalonFXMotorTuner {
     table.getDoubleTopic(motorName+"kA").publish().set(motorSlot0Configs.kA);
     table.getDoubleTopic(motorName+"kS").publish().set(motorSlot0Configs.kS);
 
-    table.getBooleanTopic(motorName+"EnableRunning").publish().set(false);;
+    if (voltageRunEnabled) {
-    table.getDoubleTopic(motorName+"RunningSpeedRPS").publish().set(0);;
+      table.getBooleanTopic(motorName+"EnableRunning").publish().set(false);;
+      table.getDoubleTopic(motorName+"RunningVolts").publish().set(0);;
+    }
     
     this.enableSub = table.getBooleanTopic(motorName+"Enable").subscribe(false);
     this.KpSub = table.getDoubleTopic(motorName+"kP").subscribe(motorSlot0Configs.kP);
@@ -51,8 +64,12 @@ public class ConfigTalonFXMotorTuner {
     this.KvSub = table.getDoubleTopic(motorName+"kV").subscribe(motorSlot0Configs.kV);
     this.KaSub = table.getDoubleTopic(motorName+"kA").subscribe(motorSlot0Configs.kA);
     this.KsSub = table.getDoubleTopic(motorName+"kS").subscribe(motorSlot0Configs.kS);
-    this.enableRuningSub = table.getBooleanTopic(motorName+"EnableRunning").subscribe(false);
+    this.enableRuningSub = voltageRunEnabled
-    this.runningSpeedRPS = table.getDoubleTopic(motorName+"RunningSpeedRPS").subscribe(0);
+        ? table.getBooleanTopic(motorName+"EnableRunning").subscribe(false)
+        : null;
+    this.runningVolts = voltageRunEnabled
+        ? table.getDoubleTopic(motorName+"RunningVolts").subscribe(0)
+        : null;
   }
 
   public void periodic(Runnable func) {
@@ -79,12 +96,14 @@ public class ConfigTalonFXMotorTuner {
       }
     }
     
-    if (enableRuningSub.get()) {;
+    if (voltageRunEnabled && enableRuningSub.get()) {;
-      double speed = runningSpeedRPS.get();
+      double volts = MathUtil.clamp(runningVolts.get(), -12.0, 12.0);
-      motor.setControl(velocityRequest.withVelocity(speed));
+      motor.setControl(voltageRequest.withOutput(volts));
-    // System.out.println("running motor with speed: "+speed+" on motor: "+motorName);
+      wasRunning = true;
-    }else{
+    // System.out.println("running motor with volts: "+volts+" on motor: "+motorName);
-      motor.setControl(velocityRequest.withVelocity(0));
+    }else if (wasRunning){
+      motor.setControl(voltageRequest.withOutput(0));
+      wasRunning = false;
     }
   }
 

src/main/java/frc/robot/tuning/ConfigTalonFXSMotorTuner.java

@@ -1,7 +1,5 @@
 package frc.robot.tuning;
 import com.ctre.phoenix6.configs.Slot0Configs;
-import com.ctre.phoenix6.controls.VelocityDutyCycle;
-import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.TalonFXS;
 
 import edu.wpi.first.networktables.*;
@@ -21,11 +19,8 @@ public class ConfigTalonFXSMotorTuner {
   private final DoubleSubscriber KaSub;
   private final DoubleSubscriber KsSub;
 
-  private final BooleanSubscriber enableRuningSub;
-  private final DoubleSubscriber runningSpeedRPS;
   private double lastKp = 0, lastKi = 0, lastKd = 0, lastKv = 0, lastKa = 0, lastKs = 0;
-  
+
-  private final VelocityDutyCycle velocityRequest = new VelocityDutyCycle(0);
   public ConfigTalonFXSMotorTuner(TalonFXS motor_, String motorName_, Slot0Configs motorSlot0Configs_) {
     this.motor = motor_;
     this.motorName = motorName_;
@@ -40,9 +35,6 @@ public class ConfigTalonFXSMotorTuner {
     table.getDoubleTopic(motorName+"kA").publish().set(motorSlot0Configs.kA);
     table.getDoubleTopic(motorName+"kS").publish().set(motorSlot0Configs.kS);
 
-    table.getBooleanTopic(motorName+"EnableRunning").publish().set(false);
-    table.getDoubleTopic(motorName+"RunningSpeedRPS").publish().set(0);
-    
     this.enableSub = table.getBooleanTopic(motorName+"Enable").subscribe(false);
     this.KpSub = table.getDoubleTopic(motorName+"kP").subscribe(motorSlot0Configs.kP);
     this.KiSub = table.getDoubleTopic(motorName+"kI").subscribe(motorSlot0Configs.kI);
@@ -50,8 +42,6 @@ public class ConfigTalonFXSMotorTuner {
     this.KvSub = table.getDoubleTopic(motorName+"kV").subscribe(motorSlot0Configs.kV);
     this.KaSub = table.getDoubleTopic(motorName+"kA").subscribe(motorSlot0Configs.kA);
     this.KsSub = table.getDoubleTopic(motorName+"kS").subscribe(motorSlot0Configs.kS);
-    this.enableRuningSub = table.getBooleanTopic(motorName+"EnableRunning").subscribe(false);
-    this.runningSpeedRPS = table.getDoubleTopic(motorName+"RunningSpeedRPS").subscribe(0);
   }
 
   public void periodic() {
@@ -74,13 +64,5 @@ public class ConfigTalonFXSMotorTuner {
 
       lastKp = kp; lastKi = ki; lastKd = kd; lastKv = kv; lastKa = ka; lastKs = ks;
     }
-    
-    if (enableRuningSub.get()) {;
-      double speed = runningSpeedRPS.get();
-      motor.setControl(velocityRequest.withVelocity(speed));
-    // System.out.println("running motor with speed: "+speed+" on motor: "+motorName);
-    }else{
-      motor.setControl(velocityRequest.withVelocity(0));
-    }
   }
 }

src/main/java/frc/robot/utils/GameData2026.java

@@ -0,0 +1,117 @@
+package frc.robot.utils;
+
+import java.util.Optional;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+
+public class GameData2026 {
+    private static final double SHIFT_1_END_TIME = 105.0;
+    private static final double SHIFT_2_END_TIME = 80.0;
+    private static final double SHIFT_3_END_TIME = 55.0;
+    private static final double SHIFT_4_END_TIME = 30.0;
+    private static final double TRANSITION_END_TIME = 130.0;
+
+    private final NetworkTable table = NetworkTableInstance.getDefault().getTable("GameData2026");
+
+    public void periodic() {
+        Optional<Alliance> alliance = DriverStation.getAlliance();
+        double matchTime = DriverStation.getMatchTime();
+        String gameData = DriverStation.getGameSpecificMessage();
+
+        HubWindow window = calculateHubWindow(alliance, matchTime, gameData);
+
+        table.getEntry("HubActive").setBoolean(window.hubActive);
+        table.getEntry("HubStatus").setString(window.status);
+        table.getEntry("SecondsUntilHubOpens").setDouble(window.secondsUntilHubOpens);
+        table.getEntry("SecondsUntilHubChanges").setDouble(window.secondsUntilHubChanges);
+        table.getEntry("Shift").setString(window.shift);
+        table.getEntry("MatchTime").setDouble(matchTime);
+        table.getEntry("GameData").setString(gameData);
+        table.getEntry("Alliance").setString(alliance.map(Enum::name).orElse("Unknown"));
+        table.getEntry("FirstInactiveAlliance").setString(getFirstInactiveAlliance(gameData));
+    }
+
+    private HubWindow calculateHubWindow(Optional<Alliance> alliance, double matchTime, String gameData) {
+        if (alliance.isEmpty()) {
+            return new HubWindow(false, "NO_ALLIANCE", "Disabled", Double.NaN, Double.NaN);
+        }
+
+        if (DriverStation.isAutonomousEnabled()) {
+            return new HubWindow(true, "OPEN", "Auto", 0.0, Double.NaN);
+        }
+
+        if (!DriverStation.isTeleopEnabled()) {
+            return new HubWindow(false, "DISABLED", "Disabled", Double.NaN, Double.NaN);
+        }
+
+        if (matchTime < 0.0) {
+            return new HubWindow(true, "OPEN_TIME_UNKNOWN", "Unknown", 0.0, Double.NaN);
+        }
+
+        Boolean redInactiveFirst = getRedInactiveFirst(gameData);
+        if (redInactiveFirst == null) {
+            return new HubWindow(true, "WAITING_DATA", "WaitingData", 0.0, Double.NaN);
+        }
+
+        boolean shift1Active = switch (alliance.get()) {
+            case Red -> !redInactiveFirst;
+            case Blue -> redInactiveFirst;
+        };
+
+        if (matchTime > TRANSITION_END_TIME) {
+            return new HubWindow(true, "OPEN", "Transition", 0.0, matchTime - TRANSITION_END_TIME);
+        } else if (matchTime > SHIFT_1_END_TIME) {
+            return fromShift("Shift1", shift1Active, matchTime, SHIFT_1_END_TIME);
+        } else if (matchTime > SHIFT_2_END_TIME) {
+            return fromShift("Shift2", !shift1Active, matchTime, SHIFT_2_END_TIME);
+        } else if (matchTime > SHIFT_3_END_TIME) {
+            return fromShift("Shift3", shift1Active, matchTime, SHIFT_3_END_TIME);
+        } else if (matchTime > SHIFT_4_END_TIME) {
+            return fromShift("Shift4", !shift1Active, matchTime, SHIFT_4_END_TIME);
+        } else {
+            return new HubWindow(true, "OPEN", "Endgame", 0.0, Math.max(matchTime, 0.0));
+        }
+    }
+
+    private HubWindow fromShift(String shift, boolean hubActive, double matchTime, double nextBoundary) {
+        double secondsUntilChange = Math.max(matchTime - nextBoundary, 0.0);
+        double secondsUntilOpen = hubActive ? 0.0 : secondsUntilChange;
+        String status = hubActive ? "OPEN" : "CLOSED";
+        return new HubWindow(hubActive, status, shift, secondsUntilOpen, secondsUntilChange);
+    }
+
+    private Boolean getRedInactiveFirst(String gameData) {
+        if (gameData.isEmpty()) {
+            return null;
+        }
+
+        return switch (gameData.charAt(0)) {
+            case 'R' -> true;
+            case 'B' -> false;
+            default -> null;
+        };
+    }
+
+    private String getFirstInactiveAlliance(String gameData) {
+        if (gameData.isEmpty()) {
+            return "None";
+        }
+
+        return switch (gameData.charAt(0)) {
+            case 'R' -> "Red";
+            case 'B' -> "Blue";
+            default -> "Invalid";
+        };
+    }
+
+    private record HubWindow(
+        boolean hubActive,
+        String status,
+        String shift,
+        double secondsUntilHubOpens,
+        double secondsUntilHubChanges
+    ) {}
+}