Compare commits
22
Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 OnDYCLaptop
|
c98ef164c5 | ||
|
OnDYCLaptop
|
40c5b5d642 | ||
|
OnDYCLaptop
|
a6595a79d7 | ||
|
OnDYCLaptop
|
5c1d9c3230 | ||
|
OnDYCLaptop
|
632da8d654 | ||
|
OnDYCLaptop
|
c5919af3ef | ||
|
OnDYCLaptop
|
d2f5adc813 | ||
|
OnDYCLaptop
|
6abdf34cf0 | ||
|
OnDYCLaptop
|
915b1b1af0 | ||
|
OnDYCLaptop
|
914584b71c | ||
|
OnDYCLaptop
|
1c39a3bc42 | ||
|
OnDYCLaptop
|
2b14e3f99e | ||
|
OnDYCLaptop
|
0053c29893 | ||
|
OnDYCLaptop
|
179913194c | ||
|
OnDYCLaptop
|
5947fa27d3 | ||
|
OnDYCLaptop
|
a1fc79659e | ||
|
OnDYCLaptop
|
0c97452a13 | ||
|
OnDYCLaptop
|
a78a71e4ed | ||
|
dingdoorfiah-ui
|
fad52c40c0 | ||
|
Claude
|
9da20a108c | ||
|
Claude
|
6264ef0762 | ||
|
OnDYCLaptop
|
3c5d17b527 |
@@ -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`。第三方库及外部代码应遵循各自原有的许可说明。
|
||||
File diff suppressed because it is too large
Load Diff
+2517
-1108
File diff suppressed because it is too large
Load Diff
@@ -1,31 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Back To Shoot Blue"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "shoot"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "shoot"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": "Blue Middle",
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -1,25 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Back To Shoot Red"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "shoot"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": "Red Middle",
|
||||
"choreoAuto": false
|
||||
}
|
||||
+54
-44
@@ -8,12 +8,6 @@
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Back To Shoot Red"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
@@ -23,18 +17,18 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "climber_init"
|
||||
"name": "start_intake_and_conveyor"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "blueLeft1"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "shoot"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
@@ -42,42 +36,21 @@
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"type": "path",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Shoot to Depot Red"
|
||||
}
|
||||
}
|
||||
]
|
||||
"pathName": "blueLeft2"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"type": "path",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Depot Intake Red"
|
||||
}
|
||||
}
|
||||
]
|
||||
"pathName": "blueLeft3"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"type": "path",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AutoAim"
|
||||
}
|
||||
}
|
||||
]
|
||||
"pathName": "blueLeftTurnToHub2"
|
||||
}
|
||||
},
|
||||
{
|
||||
@@ -93,19 +66,56 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AutoShoot"
|
||||
"name": "shoot_hub"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Shake"
|
||||
"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": "intaker_up"
|
||||
"name": "AutoAim"
|
||||
}
|
||||
}
|
||||
]
|
||||
@@ -115,6 +125,6 @@
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": "Red Middle",
|
||||
"folder": "Blue",
|
||||
"choreoAuto": false
|
||||
}
|
||||
+43
-64
@@ -8,12 +8,6 @@
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Back To Shoot Blue"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
@@ -23,18 +17,18 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "climber_init"
|
||||
"name": "start_intake_and_conveyor"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "blueRight1"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "shoot"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
@@ -42,42 +36,21 @@
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"type": "path",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Shoot to Depot Blue"
|
||||
}
|
||||
}
|
||||
]
|
||||
"pathName": "blueRight2"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"type": "path",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Depot Intake Blue"
|
||||
}
|
||||
}
|
||||
]
|
||||
"pathName": "blueRight3"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"type": "path",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AutoAim"
|
||||
}
|
||||
}
|
||||
]
|
||||
"pathName": "blueRightTurnToHub2"
|
||||
}
|
||||
},
|
||||
{
|
||||
@@ -93,59 +66,65 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AutoShoot"
|
||||
"name": "shoot_hub"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Shake"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "intaker_up"
|
||||
"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": "path",
|
||||
"type": "named",
|
||||
"data": {
|
||||
"pathName": "Depot To Climb Blue"
|
||||
"name": "shoot_hub"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "climb_up"
|
||||
"name": "AutoAim"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Climb Step2 Blue"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Climb Step3 Blue"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": "Blue Middle",
|
||||
"folder": "Blue",
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -1,152 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Back To Shoot Red"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "intaker_down"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "shoot"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "intaker_down"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "start_intake_and_conveyor"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Shoot to Depot Red"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Depot Intake Red"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AutoAim"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "stop_intake_and_conveyor"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AutoShoot"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Shake"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "intaker_up"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AutoShoot"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Shake"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "intaker_up"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": "Red Middle",
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -4,6 +4,12 @@
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "wait",
|
||||
"data": {
|
||||
"waitTime": 0.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
@@ -13,19 +19,13 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AutoAim"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AutoShoot"
|
||||
"name": "intaker_down"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": "Blue Middle",
|
||||
"folder": null,
|
||||
"choreoAuto": false
|
||||
}
|
||||
+60
-37
@@ -4,20 +4,26 @@
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "intaker_down"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "intaker_down"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "start_intake_and_conveyor"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "To Depot Blue"
|
||||
"pathName": "red1"
|
||||
}
|
||||
}
|
||||
]
|
||||
@@ -26,7 +32,31 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "start_intake_timelimit"
|
||||
"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"
|
||||
}
|
||||
},
|
||||
{
|
||||
@@ -36,38 +66,13 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "start_intake_timelimit"
|
||||
"name": "shoot_hub"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Depot Intake Blue"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "wait",
|
||||
"data": {
|
||||
"waitTime": 2.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "intaker_up"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Depot to Shoot Blue"
|
||||
"name": "AutoAim"
|
||||
}
|
||||
}
|
||||
]
|
||||
@@ -76,13 +81,31 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "shoot"
|
||||
"name": "start_intake_and_conveyor"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "turntohub1"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "red4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "red5"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": "Blue Middle",
|
||||
"folder": "Red",
|
||||
"choreoAuto": false
|
||||
}
|
||||
+35
-42
@@ -4,31 +4,31 @@
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "intaker_down"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "intaker_down"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "start_intake_and_conveyor"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "To Depot Red"
|
||||
"pathName": "RedGotodepot"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "start_intake_timelimit"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
@@ -36,53 +36,46 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "start_intake_timelimit"
|
||||
"name": "shoot_hub"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Depot Intake Red"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "wait",
|
||||
"data": {
|
||||
"waitTime": 2.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "intaker_up"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Depot to Shoot Red"
|
||||
"name": "AutoAim"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "redDepotToGoudong"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "shoot"
|
||||
"name": "start_intake_and_conveyor"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "red4"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "red5"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": "Red Middle",
|
||||
"folder": "Red",
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
@@ -1,67 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Back To Shoot Blue"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "shoot"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "wait",
|
||||
"data": {
|
||||
"waitTime": 0.25
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Shoot to Climb Step1 Blue"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "climb_up"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Climb Step2 Blue"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Climb Step3 Blue"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "wait",
|
||||
"data": {
|
||||
"waitTime": 0.25
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "climb_down"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": "Blue Middle",
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -1,80 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Back To Shoot Red"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "climber_init"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "shoot"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "wait",
|
||||
"data": {
|
||||
"waitTime": 0.25
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Shoot to Climb Step1 Red"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "climb_up"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Climb Step2 Red"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Climb Step3 Red"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "wait",
|
||||
"data": {
|
||||
"waitTime": 0.25
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "climb_down"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": "Red Middle",
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -1,158 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Back To Shoot Blue"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "intaker_down"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "shoot"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "intaker_down"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "start_intake_and_conveyor"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Shoot to Depot Blue"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Depot Intake Blue"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AutoAim"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "stop_intake_and_conveyor"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AutoShoot"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Shake"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "intaker_up"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AutoShoot"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Shake"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "intaker_up"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "blue depot go 1"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": "Blue Middle",
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -1,151 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Back To Shoot Red"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "intaker_down"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "shoot"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Shoot to Depot Red"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "start_intake_timelimit"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "start_intake_timelimit"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Depot Intake Red"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "intaker_up"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Depot to Shoot Red"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AutoAim"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AutoShoot"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Shake"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "intaker_up"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AutoShoot"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Shake"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "intaker_up"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": "Red Middle",
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -1,157 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Back To Shoot Red"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "intaker_down"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "climber_init"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "shoot"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "start_intake_and_conveyor"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Shoot to Depot Red"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Depot Intake Red"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AutoAim"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "stop_intake_and_conveyor"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AutoShoot"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "Shake"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "intaker_up"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "parallel",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Depot To Climb Red"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "climb_up"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Climb Step2 Red"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "path",
|
||||
"data": {
|
||||
"pathName": "Climb Step3 Red"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "climb_down"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": true,
|
||||
"folder": "Red Middle",
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -1,31 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "start_intake"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "wait",
|
||||
"data": {
|
||||
"waitTime": 2.0
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "stop_intake"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": false,
|
||||
"folder": "Blue Middle",
|
||||
"choreoAuto": false
|
||||
}
|
||||
@@ -1,25 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"command": {
|
||||
"type": "sequential",
|
||||
"data": {
|
||||
"commands": [
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "start_intake_and_conveyor"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "wait",
|
||||
"data": {
|
||||
"waitTime": 1.0
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
},
|
||||
"resetOdom": false,
|
||||
"folder": "Red Middle",
|
||||
"choreoAuto": false
|
||||
}
|
||||
File diff suppressed because one or more lines are too long
@@ -1,54 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 0.64,
|
||||
"y": 4.704
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 0.9248292506025121,
|
||||
"y": 4.704
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.073,
|
||||
"y": 4.704
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 0.823,
|
||||
"y": 4.704
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 0.5,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAngularVelocity": 360.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Blue Middle",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
@@ -1,54 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 15.9,
|
||||
"y": 3.465
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 15.522355156858092,
|
||||
"y": 3.465
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 15.517,
|
||||
"y": 3.465
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 15.77785583456356,
|
||||
"y": 3.465
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 0.5,
|
||||
"maxAcceleration": 1.0,
|
||||
"maxAngularVelocity": 360.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Red Middle",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
@@ -1,54 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.073,
|
||||
"y": 4.704
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 1.323,
|
||||
"y": 4.704
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.073,
|
||||
"y": 4.604
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 0.7925211774538647,
|
||||
"y": 4.604
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 0.25,
|
||||
"maxAcceleration": 0.5,
|
||||
"maxAngularVelocity": 360.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Blue Middle",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
@@ -1,54 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 15.517,
|
||||
"y": 3.465
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 15.767,
|
||||
"y": 3.465
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 15.517,
|
||||
"y": 3.565
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 15.239842716853756,
|
||||
"y": 3.565
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 0.25,
|
||||
"maxAcceleration": 0.5,
|
||||
"maxAngularVelocity": 360.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Red Middle",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
@@ -1,63 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 0.64,
|
||||
"y": 5.969
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 0.9200056277857709,
|
||||
"y": 5.902393157337589
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 7.485927152317881,
|
||||
"y": 5.276448675496688
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 5.220827814571248,
|
||||
"y": 5.6757450331143255
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [
|
||||
{
|
||||
"waypointRelativePos": 0.4,
|
||||
"rotationDegrees": 135.0
|
||||
},
|
||||
{
|
||||
"waypointRelativePos": 0.68,
|
||||
"rotationDegrees": 135.0
|
||||
}
|
||||
],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 1.5,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 360.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Blue Middle",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -1,54 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.14,
|
||||
"y": 5.969
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 1.39,
|
||||
"y": 5.969
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 0.64,
|
||||
"y": 5.969
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 0.39,
|
||||
"y": 5.969
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 0.25,
|
||||
"maxAcceleration": 0.5,
|
||||
"maxAngularVelocity": 360.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Blue Middle",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
@@ -1,54 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 15.9,
|
||||
"y": 2.1
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 15.9,
|
||||
"y": 3.0999999999999983
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 15.9,
|
||||
"y": 3.465
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 15.9,
|
||||
"y": 2.465000000000005
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 1.0,
|
||||
"maxAcceleration": 2.0,
|
||||
"maxAngularVelocity": 360.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Red Middle",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
@@ -3,25 +3,25 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.5,
|
||||
"y": 2.5
|
||||
"x": 3.5467141162514837,
|
||||
"y": 4.051138790035587
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 3.8524186062054477,
|
||||
"y": 2.382931901404091
|
||||
"x": 4.255399988299603,
|
||||
"y": 4.013414379701552
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 2.0,
|
||||
"y": 3.0
|
||||
"x": 2.944199288256229,
|
||||
"y": 4.051138790035587
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 1.7628130762195064,
|
||||
"y": 3.0790086272982027
|
||||
"x": 2.694910022767602,
|
||||
"y": 4.069976574197767
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -33,8 +33,8 @@
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 1.5,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxVelocity": 2.0,
|
||||
"maxAcceleration": 4.0,
|
||||
"maxAngularVelocity": 360.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
@@ -45,7 +45,7 @@
|
||||
"rotation": 0.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Blue Middle",
|
||||
"folder": null,
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
|
||||
@@ -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
|
||||
}
|
||||
@@ -1,70 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.04,
|
||||
"y": 4.0
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 3.3282933300270625,
|
||||
"y": 3.975244391494451
|
||||
},
|
||||
"isLocked": true,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.97314,
|
||||
"y": 4.704
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 1.4585920284163918,
|
||||
"y": 4.684160090340553
|
||||
},
|
||||
"nextControl": {
|
||||
"x": 2.2475848605490505,
|
||||
"y": 4.714582028383155
|
||||
},
|
||||
"isLocked": true,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 0.64,
|
||||
"y": 4.704
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 0.3610340465991982,
|
||||
"y": 4.704101661030112
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 1.5,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 360.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Blue Middle",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -1,70 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 13.5,
|
||||
"y": 4.0
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 14.25,
|
||||
"y": 4.0
|
||||
},
|
||||
"isLocked": true,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 14.567,
|
||||
"y": 3.4
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 14.088847622018482,
|
||||
"y": 3.5461858523613685
|
||||
},
|
||||
"nextControl": {
|
||||
"x": 14.806076188990758,
|
||||
"y": 3.326907073819316
|
||||
},
|
||||
"isLocked": true,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 15.9,
|
||||
"y": 3.365
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 15.572533463420765,
|
||||
"y": 3.399470424413628
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 1.5,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 360.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Red Middle",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -1,54 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.54,
|
||||
"y": 4.0
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 2.539999999999999,
|
||||
"y": 4.0
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.14,
|
||||
"y": 5.97
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 2.1399999999999992,
|
||||
"y": 5.97
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 1.5,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 360.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Blue Middle",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -1,95 +0,0 @@
|
||||
{
|
||||
"version": "2025.0",
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 0.64,
|
||||
"y": 5.969
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 1.678433584781481,
|
||||
"y": 5.9229373186851895
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 2.6943708609271524,
|
||||
"y": 7.381829470198674
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 2.178364289572292,
|
||||
"y": 7.226083943133918
|
||||
},
|
||||
"nextControl": {
|
||||
"x": 3.0531960414062747,
|
||||
"y": 7.490133162361142
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 6.0,
|
||||
"y": 7.2
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 5.804119228777829,
|
||||
"y": 7.355340669064503
|
||||
},
|
||||
"nextControl": {
|
||||
"x": 6.195880771222171,
|
||||
"y": 7.044659330935497
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 7.195529801324503,
|
||||
"y": 5.969
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 7.354483593458411,
|
||||
"y": 5.7760396621938845
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
}
|
||||
],
|
||||
"rotationTargets": [
|
||||
{
|
||||
"waypointRelativePos": 0.9,
|
||||
"rotationDegrees": 180.0
|
||||
},
|
||||
{
|
||||
"waypointRelativePos": 1.7,
|
||||
"rotationDegrees": 180.0
|
||||
}
|
||||
],
|
||||
"constraintZones": [],
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 1.5,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 360.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 135.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Blue Middle",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
+12
-12
@@ -3,25 +3,25 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.04,
|
||||
"y": 4.0
|
||||
"x": 3.0028433734939757,
|
||||
"y": 6.471879518072289
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 3.4702773700848932,
|
||||
"y": 3.562911556367572
|
||||
"x": 3.1230481927710834,
|
||||
"y": 6.941771084337349
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 1.14,
|
||||
"y": 5.969
|
||||
"x": 3.418096385542169,
|
||||
"y": 7.3788795180722895
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 0.8995187790061673,
|
||||
"y": 6.223926270411972
|
||||
"x": 3.260671343049193,
|
||||
"y": 7.152135161355801
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -41,14 +41,14 @@
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"velocity": 1.2,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Blue Middle",
|
||||
"folder": "Blue Left from pwz",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
"rotation": -38.99099404250592
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
+13
-13
@@ -3,25 +3,25 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 0.84,
|
||||
"y": 5.969
|
||||
"x": 3.418096385542169,
|
||||
"y": 7.3788795180722895
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 1.2669518600756762,
|
||||
"y": 5.612104897874479
|
||||
"x": 3.175553518148792,
|
||||
"y": 7.3182745885978715
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.04,
|
||||
"y": 4.0
|
||||
"x": 3.0028433734939757,
|
||||
"y": 6.471879518072289
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 2.8248463530252574,
|
||||
"y": 4.180015650217846
|
||||
"x": 3.540329100641909,
|
||||
"y": 7.025981025402732
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -33,8 +33,8 @@
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 1.5,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxVelocity": 2.0,
|
||||
"maxAcceleration": 4.0,
|
||||
"maxAngularVelocity": 360.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
@@ -42,12 +42,12 @@
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
"rotation": -38.99099404250592
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Blue Middle",
|
||||
"folder": "Blue Left from pwz",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"velocity": 1.2,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
@@ -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
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
+13
-13
@@ -3,25 +3,25 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.53,
|
||||
"y": 4.0
|
||||
"x": 3.0028433734939757,
|
||||
"y": 1.5981204819277117
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 3.9033070768298836,
|
||||
"y": 3.9994852751840226
|
||||
"x": 3.1230481927710834,
|
||||
"y": 1.128228915662652
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 3.04,
|
||||
"y": 4.0
|
||||
"x": 3.418096385542169,
|
||||
"y": 0.691120481927711
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 2.783639997726092,
|
||||
"y": 4.003292975326709
|
||||
"x": 3.260671343049193,
|
||||
"y": 0.9178648386441994
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -33,22 +33,22 @@
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 1.5,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxVelocity": 2.0,
|
||||
"maxAcceleration": 4.0,
|
||||
"maxAngularVelocity": 360.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"velocity": 1.2,
|
||||
"rotation": 0.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Blue Middle",
|
||||
"folder": "Blue Right from pwz",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
"rotation": 38.99099404250589
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
+14
-14
@@ -3,29 +3,29 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 15.4,
|
||||
"y": 2.1
|
||||
"x": 15.613421686746985,
|
||||
"y": 2.2319277108433733
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 15.65,
|
||||
"y": 2.1
|
||||
"x": 15.36208433734939,
|
||||
"y": 2.2319277108433733
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
"linkedName": "depot"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 15.9,
|
||||
"y": 2.1
|
||||
"x": 13.12190361445783,
|
||||
"y": 0.691120481927711
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 15.65,
|
||||
"y": 2.1
|
||||
"x": 13.37190361445783,
|
||||
"y": 0.691120481927711
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
"linkedName": "3"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
@@ -33,7 +33,7 @@
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 0.5,
|
||||
"maxVelocity": 1.5,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAngularVelocity": 360.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
@@ -41,14 +41,14 @@
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"velocity": 0.0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Red Middle",
|
||||
"folder": "Red Right from pwz",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
"rotation": 158.4590240814616
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
+14
-14
@@ -3,25 +3,25 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 0.64,
|
||||
"y": 5.97
|
||||
"x": 8.774607703281026,
|
||||
"y": 6.784465049928674
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 0.6399999999999998,
|
||||
"y": 4.9700000000000015
|
||||
"x": 8.78930219561021,
|
||||
"y": 5.401087218387813
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 0.64,
|
||||
"y": 4.705
|
||||
"x": 8.774607703281026,
|
||||
"y": 4.597831669044222
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 0.6400000000000002,
|
||||
"y": 5.705
|
||||
"x": 8.796102898053132,
|
||||
"y": 5.99227817461089
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -34,21 +34,21 @@
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 1.5,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxAcceleration": 4.0,
|
||||
"maxAngularVelocity": 360.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
"velocity": 0.0,
|
||||
"rotation": 90.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Blue Middle",
|
||||
"folder": "Red Right from pwz",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 0.0
|
||||
"velocity": 1.5,
|
||||
"rotation": 90.0
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
@@ -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
|
||||
}
|
||||
+12
-12
@@ -3,25 +3,25 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 15.7,
|
||||
"y": 2.1
|
||||
"x": 13.537156626506023,
|
||||
"y": 6.471879518072289
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 16.18921636103803,
|
||||
"y": 1.647541846848796
|
||||
"x": 13.416951807228916,
|
||||
"y": 6.941771084337349
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 13.5,
|
||||
"y": 4.0
|
||||
"x": 13.12190361445783,
|
||||
"y": 7.3788795180722895
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 13.247149348869588,
|
||||
"y": 4.238251681778358
|
||||
"x": 13.279328656950806,
|
||||
"y": 7.152135161355801
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -41,14 +41,14 @@
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"velocity": 0.0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Red Middle",
|
||||
"folder": "Red Right from pwz",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
"rotation": -141.0090059574941
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
+12
-12
@@ -3,25 +3,25 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 13.5,
|
||||
"y": 4.0
|
||||
"x": 13.12190361445783,
|
||||
"y": 7.3788795180722895
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 13.699377937127288,
|
||||
"y": 3.7771427571164287
|
||||
"x": 13.364446481851207,
|
||||
"y": 7.3182745885978715
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 15.400452528841061,
|
||||
"y": 2.1
|
||||
"x": 13.537156626506023,
|
||||
"y": 6.471879518072289
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 15.073650526430217,
|
||||
"y": 2.4474296685616617
|
||||
"x": 12.99967089935809,
|
||||
"y": 7.025981025402732
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
@@ -42,13 +42,13 @@
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
"rotation": -141.0090059574941
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Red Middle",
|
||||
"folder": "Red Right from pwz",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"velocity": 1.2,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": false
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
+15
-15
@@ -3,29 +3,29 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 13.01022182786158,
|
||||
"y": 4.0
|
||||
"x": 13.537156626506023,
|
||||
"y": 1.5981204819277117
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 13.865906250551847,
|
||||
"y": 4.0
|
||||
"x": 13.416951807228916,
|
||||
"y": 1.128228915662652
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
"linkedName": "sssss"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 13.5,
|
||||
"y": 4.0
|
||||
"x": 13.12190361445783,
|
||||
"y": 0.691120481927711
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 13.231071353115471,
|
||||
"y": 4.0
|
||||
"x": 13.279328656950806,
|
||||
"y": 0.9178648386441994
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
"linkedName": "3"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
@@ -33,22 +33,22 @@
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 1.5,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxVelocity": 2.0,
|
||||
"maxAcceleration": 4.0,
|
||||
"maxAngularVelocity": 360.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
"unlimited": false
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"velocity": 0.0,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Red Middle",
|
||||
"folder": "Red Right from pwz",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
"rotation": 141.0090059574941
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
}
|
||||
+15
-15
@@ -3,29 +3,29 @@
|
||||
"waypoints": [
|
||||
{
|
||||
"anchor": {
|
||||
"x": 13.0,
|
||||
"y": 4.0
|
||||
"x": 13.12190361445783,
|
||||
"y": 0.691120481927711
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 14.000000000000002,
|
||||
"y": 4.0
|
||||
"x": 13.364446481851207,
|
||||
"y": 0.7517254114021283
|
||||
},
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
"linkedName": "3"
|
||||
},
|
||||
{
|
||||
"anchor": {
|
||||
"x": 15.400452528841061,
|
||||
"y": 2.1
|
||||
"x": 13.537156626506023,
|
||||
"y": 1.5981204819277117
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 14.400452528841061,
|
||||
"y": 2.1
|
||||
"x": 12.99967089935809,
|
||||
"y": 1.0440189745972686
|
||||
},
|
||||
"nextControl": null,
|
||||
"isLocked": false,
|
||||
"linkedName": null
|
||||
"linkedName": "sssss"
|
||||
}
|
||||
],
|
||||
"rotationTargets": [],
|
||||
@@ -33,8 +33,8 @@
|
||||
"pointTowardsZones": [],
|
||||
"eventMarkers": [],
|
||||
"globalConstraints": {
|
||||
"maxVelocity": 1.5,
|
||||
"maxAcceleration": 3.0,
|
||||
"maxVelocity": 2.0,
|
||||
"maxAcceleration": 4.0,
|
||||
"maxAngularVelocity": 360.0,
|
||||
"maxAngularAcceleration": 720.0,
|
||||
"nominalVoltage": 12.0,
|
||||
@@ -42,12 +42,12 @@
|
||||
},
|
||||
"goalEndState": {
|
||||
"velocity": 0,
|
||||
"rotation": 180.0
|
||||
"rotation": 141.0090059574941
|
||||
},
|
||||
"reversed": false,
|
||||
"folder": "Red Middle",
|
||||
"folder": "Red Right from pwz",
|
||||
"idealStartingState": {
|
||||
"velocity": 0,
|
||||
"velocity": 1.2,
|
||||
"rotation": 180.0
|
||||
},
|
||||
"useDefaultConstraints": true
|
||||
@@ -3,19 +3,21 @@
|
||||
"robotLength": 0.85,
|
||||
"holonomicMode": true,
|
||||
"pathFolders": [
|
||||
"Blue Middle",
|
||||
"Red Middle"
|
||||
"Blue Left from pwz",
|
||||
"Red Right from pwz",
|
||||
"Blue Right from pwz"
|
||||
],
|
||||
"autoFolders": [
|
||||
"Red Middle",
|
||||
"Blue Middle"
|
||||
"Blue",
|
||||
"New Folder",
|
||||
"Red"
|
||||
],
|
||||
"defaultMaxVel": 1.5,
|
||||
"defaultMaxAccel": 3.0,
|
||||
"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,
|
||||
|
||||
@@ -1,70 +1,92 @@
|
||||
package frc.robot;
|
||||
|
||||
import javax.net.ssl.TrustManager;
|
||||
|
||||
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,
|
||||
AllTelop,
|
||||
PassingBall,
|
||||
AutoAimming,
|
||||
Climbing,
|
||||
CrossingBump,
|
||||
CrossingTrench
|
||||
}
|
||||
public class Shooter {
|
||||
public static final double conveyorSpeed = 150;
|
||||
public static final double conveyorSpeedForAuto = 10;
|
||||
|
||||
public static class Shooter {
|
||||
// Placeholder mechanism CAN IDs; update here after final wiring.
|
||||
public static final int leftFlywheelID = 38;
|
||||
public static final int rightFlywheelID = 39;
|
||||
public static final int leftConveyorID = 40;
|
||||
public static final int rightConveyorID = 37;
|
||||
public static final int backboardMotorID = 44;
|
||||
|
||||
public static final double flywheelNominalKv = 0.12;
|
||||
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)
|
||||
.withKV(0.099).withKA(0.0).withKS(0.0);
|
||||
.withKP(0.21)
|
||||
.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)
|
||||
.withKV(0.16).withKA(0.15).withKS(0);
|
||||
.withKP(0.19).withKI(0).withKD(0.001)
|
||||
.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.03).withKA(0.0).withKS(0.010);
|
||||
|
||||
public class backboardPositionPID {
|
||||
public static final double kP = 0.3;
|
||||
public static class backboardPositionPID {
|
||||
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 = 60; //也就是output
|
||||
// 背板的上下限位角度
|
||||
public static final double backboardUpLimit = 3000.0; // 背板上限角度（根据实际需求设置）
|
||||
public static final double backboardDownLimit = 0.0; // 背板下限角度（根据实际需求设置）
|
||||
|
||||
public static final double backboardSpeedMax = 3.0;
|
||||
public static final double backboardSpeedMin = 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 AutoPositioning {
|
||||
public static final double autoRotationkP = 0.08;
|
||||
|
||||
public static class AutoPositioning {
|
||||
public static final double autoRotationkP = 0.07;
|
||||
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 autoPositioningAngleError = 1.0;
|
||||
|
||||
public static final double[] bumpY = {2.51,5.556};
|
||||
public static final double[] trenchY = {0.639,7.43};
|
||||
public static final double[] bumpY = {2.51, 5.556};
|
||||
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 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 Point2 = new Pose2d(14.314, 4.15, edu.wpi.first.math.geometry.Rotation2d.fromDegrees(-180));
|
||||
public static class StartingPoints {
|
||||
public static class Red {
|
||||
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, Rotation2d.fromDegrees(-180));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public enum DriveMode {
|
||||
@@ -73,41 +95,19 @@ public final class Constants {
|
||||
SYSID,
|
||||
OTHER
|
||||
}
|
||||
public class Pigeon {
|
||||
|
||||
public static class Pigeon {
|
||||
public static final int CANID = 0;
|
||||
}
|
||||
|
||||
public class Limelight {
|
||||
public static final String LIMELIGHT_NAME_Shooter = "limelight-shooter";
|
||||
public static final String LIMELIGHT_NAME_Intaker = "limelight-intaker";
|
||||
public static final Boolean UsingMetaTag2 = true; // 是否使用 MetaTag2（如果你们的 pipeline 配置了这个功能的话）
|
||||
public static final int climbPipelineIndex = 8;
|
||||
public static class Limelight {
|
||||
public static final String LIMELIGHT_NAME_Shooter = "limelight-left";
|
||||
public static final String LIMELIGHT_NAME_Intaker = "limelight-front";
|
||||
public static final Boolean UsingMetaTag2 = true;
|
||||
public static final int locatePipelineIndex = 0;
|
||||
|
||||
// 误差死区 / 对齐判定
|
||||
public static final double AutoClimbToleranceTX = 1.5; // deg
|
||||
public static final double AutoClimbToleranceTZ = 0.10; // m
|
||||
public static final double AutoClimbToleranceRY = 2.0; // deg
|
||||
|
||||
// P 控制系数
|
||||
public static final double AutoClimbKpTX = 0.020; // deg -> rad/s
|
||||
public static final double AutoClimbKpTZ = 0.90; // m -> m/s
|
||||
public static final double AutoClimbKpRY = 0.015; // deg -> rad/s
|
||||
|
||||
// 最小输出（防止快到目标时推不动）
|
||||
public static final double AutoClimbMinVX = 0.18; // m/s
|
||||
public static final double AutoClimbMinOmega = 0.10; // rad/s
|
||||
|
||||
// 最大输出（防止冲太猛）
|
||||
public static final double AutoClimbMaxVX = 1.20; // m/s
|
||||
public static final double AutoClimbMaxOmega = 0.60; // rad/s
|
||||
|
||||
// 远距离 / 大角度时额外限速，可后续微调
|
||||
public static final double AutoClimbFastTurnThresholdTX = 15.0; // deg
|
||||
public static final double AutoClimbFastTurnOmega = 0.45; // rad/s
|
||||
}
|
||||
|
||||
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 BlueHubPositionX = 4.625;
|
||||
@@ -118,55 +118,58 @@ public final class Constants {
|
||||
public static final double BluePassingBallPosX = 3.184;
|
||||
}
|
||||
|
||||
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 class Intaker {
|
||||
public static final double intakeRollerIntakeVoltage = -6.0;
|
||||
public static final double intakeRollerOuttakeVoltage = 3.0;
|
||||
|
||||
public static final double intakeRotaterUpLimit = 0.0; // rotator上限角度
|
||||
public static final double intakeRotaterDownLimit = -7.6; // rotator下限角度
|
||||
public static final double intakeRotaterVelocity = 5; // rotater的速度（如果你用速度控制的话）
|
||||
public static final double IntakeRotaterGravityFF = 0.0; // 重力前馈增益（需要实测调整）
|
||||
public static final double intakeRotaterRaiseVoltage = 0.8;
|
||||
public static final double intakeRotaterLowerVoltage = -1.5;
|
||||
|
||||
public static final Slot0Configs intakeGetterSlot0Configs = new Slot0Configs()
|
||||
.withKP(0.00).withKI(0.00).withKD(0.00)
|
||||
.withKV(0.00).withKA(0.00).withKS(0.00);
|
||||
// getter 如果你只用固定速度开关，也可以先不启闭环；若用闭环速度，这里 kP/kV/KS 等需要实测调参(这句fromAI)
|
||||
public static final Slot0Configs intakeRollerSlot0Configs = new Slot0Configs()
|
||||
.withKP(0.00).withKI(0.00).withKD(0.00)
|
||||
.withKV(0.00).withKA(0.00).withKS(0.00);
|
||||
public static final Slot0Configs intakeGetterSlot0Configs = intakeRollerSlot0Configs;
|
||||
|
||||
public static final Slot0Configs intakeRotaterSlot0Configs = new Slot0Configs()
|
||||
.withKP(0.00).withKI(0.00).withKD(0.00)
|
||||
.withKV(0.00).withKA(0.00).withKS(0.00);
|
||||
//如果 rotater 受重力影响明显，可能需要用 withKG/withGravityType/withGravityArmPositionOffset（看机构形式）(这句fromAI)
|
||||
|
||||
public static final double IntakeGetterSpeedforAuto = 0.5;
|
||||
// 单位RPS
|
||||
public static final double positionToleranceRotations = 0.00;
|
||||
//rotater容差
|
||||
public static final Slot0Configs intakeRotaterSlot0Configs = new Slot0Configs()
|
||||
.withKP(0.00).withKI(0.00).withKD(0.00)
|
||||
.withKV(0.00).withKA(0.00).withKS(0.00);
|
||||
|
||||
public static final double IntakeGetterSpeedforAuto = 0.5;
|
||||
// Placeholder values; tighten after the real mechanism is tuned.
|
||||
}
|
||||
public static class Climber {
|
||||
|
||||
public static final int motorID = 49;
|
||||
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 boolean inverted = false;
|
||||
public static final Slot0Configs mainConveyorSlot0Configs = new Slot0Configs()
|
||||
.withKP(0.00).withKI(0.00).withKD(0.00)
|
||||
.withKV(0.00).withKA(0.00).withKS(0.00);
|
||||
}
|
||||
|
||||
public static double climbUpTicks = 94; // TODO: 实测后调整
|
||||
public static double climbDownTicks = 0; // TODO: 实测后调整
|
||||
public static double climbDownAutoTicks = 0; // TODO: 实测后调整，自动爬时可能不需要完全降到底
|
||||
public static final double climbVelocity = -200;
|
||||
public static final double holdToleranceRot = 0.5;
|
||||
public static final double deltaDownTicks = 1;
|
||||
public static class Superstructure {
|
||||
// State-machine timing and gate tolerances; tune on the real robot.
|
||||
|
||||
public static final Slot0Configs slot0Configs = new Slot0Configs();
|
||||
public static final double shooterFeedVelocity = Shooter.shooterConveyorFeedVelocity;
|
||||
public static final double shooterReverseVelocity = Shooter.shooterConveyorReverseVelocity;
|
||||
public static final double shootTimeoutSeconds = 4.0;
|
||||
public static final double prepShootFallbackDelaySeconds = 0.5;
|
||||
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;
|
||||
|
||||
static {
|
||||
// TODO: PID/F 值你们实测后填
|
||||
slot0Configs.kP = 0.2;
|
||||
slot0Configs.kI = 0.0;
|
||||
slot0Configs.kD = 0.0;
|
||||
|
||||
// TODO: 如果要前馈（速度闭环一般建议有 kV）
|
||||
slot0Configs.kS = 0.0;
|
||||
slot0Configs.kV = 0.1;
|
||||
slot0Configs.kA = 0.0;
|
||||
// Shoot backboard target position in external encoder counts.
|
||||
public static final double backboardShootPosition = 175.0;
|
||||
public static final double backboardReadyPositionTolerance = 100.0;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,6 +1,6 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
// 版权所有 (c) FIRST 和其他 WPILib 贡献者。
|
||||
// 本项目为开源软件；你可以依据项目根目录中的 WPILib BSD license 文件
|
||||
// 修改和/或分享本代码。
|
||||
|
||||
package frc.robot;
|
||||
|
||||
|
||||
@@ -1,14 +1,11 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
// 版权所有 (c) FIRST 和其他 WPILib 贡献者。
|
||||
// 本项目为开源软件；你可以依据项目根目录中的 WPILib BSD license 文件
|
||||
// 修改和/或分享本代码。
|
||||
|
||||
package frc.robot;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
import frc.robot.subsystems.ShooterSubsystem;
|
||||
|
||||
public class Robot extends TimedRobot {
|
||||
private Command m_autonomousCommand;
|
||||
@@ -30,9 +27,13 @@ public class Robot extends TimedRobot {
|
||||
@Override
|
||||
public void disabledInit() {
|
||||
System.out.println("Robot disabledInit called");
|
||||
// 进入 disabled 时强制所有重构后的机构回到安全状态，
|
||||
// 即使上一周期还有手动或自动请求也不能继续动作。
|
||||
m_robotContainer.superstructure.requestIdle();
|
||||
m_robotContainer.shooterSubsystem.stopAll();
|
||||
// m_robotContainer.climberSubsystem.stopAll();
|
||||
m_robotContainer.IntakerSubsystem.stopAll();
|
||||
m_robotContainer.intakeRollerSubsystem.stop();
|
||||
m_robotContainer.intakeRotaterSubsystem.stop();
|
||||
m_robotContainer.conveyorSubsystem.stop();
|
||||
}
|
||||
|
||||
@Override
|
||||
@@ -44,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);
|
||||
|
||||
@@ -9,96 +9,92 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
||||
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.RunCommand;
|
||||
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.IntakerControls;
|
||||
import frc.robot.controls.ShooterControls;
|
||||
import frc.robot.controls.OperatorControls;
|
||||
import frc.robot.generated.TunerConstants;
|
||||
import frc.robot.subsystems.CommandSwerveDrivetrain;
|
||||
import frc.robot.subsystems.IntakerSubsystem;
|
||||
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.LedBindings;
|
||||
import frc.robot.controls.ClimberControls;
|
||||
import frc.robot.controls.Autocommand;
|
||||
import frc.robot.subsystems.ClimberSubsystem;
|
||||
import frc.robot.utils.GameData2026;
|
||||
import frc.robot.utils.LedBindings;
|
||||
|
||||
public class RobotContainer {
|
||||
|
||||
/* ====================== */
|
||||
/* 手柄 */
|
||||
/* ====================== */
|
||||
|
||||
// 使用2个 Xbox 手柄（端口 0,1）
|
||||
private final CommandXboxController controllerlower = new CommandXboxController(0);
|
||||
private final CommandXboxController controllerupper = new CommandXboxController(1);
|
||||
// 使用一个 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 IntakerSubsystem IntakerSubsystem = new IntakerSubsystem();
|
||||
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 子系统
|
||||
private final LEDSubsystem leds = new LEDSubsystem(0, 72 ,0); // 总长度=72+72
|
||||
// Superstructure 是所有得分机构的唯一调度器。
|
||||
// 控制层和自动命令只能向它请求状态，不能直接命令电机。
|
||||
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();
|
||||
/* ====================== */
|
||||
/* 控制封装/日志 */
|
||||
/* 控制封装/调参 */
|
||||
/* ====================== */
|
||||
|
||||
public final DriveControls driveControls = new DriveControls(drivetrain, controllerlower, robotStatusManager );
|
||||
private final ShooterControls shooterControls = new ShooterControls(shooterSubsystem, controllerupper,robotStatusManager);
|
||||
//private final ClimberControls climberControls = new ClimberControls(climberSubsystem, controllerupper);
|
||||
private final IntakerControls intakerControls = new IntakerControls(IntakerSubsystem, shooterSubsystem, controllerupper);
|
||||
public final DriveControls driveControls = new DriveControls(drivetrain, controller, superstructure);
|
||||
|
||||
// 操作员意图层替代旧的射击/进球分离控制类，让复杂动作判断集中留在 Superstructure。
|
||||
private final OperatorControls operatorControls = new OperatorControls(superstructure, shooterSubsystem);
|
||||
private final DriveGainsTuner driveGainsTuner = new DriveGainsTuner(drivetrain);
|
||||
private final GameData2026 gameData2026 = new GameData2026();
|
||||
private ConfigTalonFXMotorTuner configFlywheelTuner = new ConfigTalonFXMotorTuner(shooterSubsystem.flywheelMotorLeft, "flywheel", Constants.Shooter.flyWheelSlot0Configs);
|
||||
private ConfigTalonFXMotorTuner configConveyorTuner = new ConfigTalonFXMotorTuner(shooterSubsystem.conveyorMotor, "conveyor", Constants.Shooter.conveyorSlot0Configs);
|
||||
private ConfigTalonFXSMotorTuner configBackboardTuner = new ConfigTalonFXSMotorTuner(shooterSubsystem.backboardMotor, "backboard", Constants.Shooter.backboardSlot0Configs);
|
||||
private ConfigTalonFXMotorTuner configFlywheelTuner =
|
||||
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();
|
||||
// configueClimber();
|
||||
bingdingLED();
|
||||
configueIntaker();
|
||||
// Autocommand.preNameCommands(
|
||||
// climberSubsystem,
|
||||
// IntakerSubsystem,
|
||||
// drivetrain,
|
||||
// shooterSubsystem,
|
||||
// driveControls,
|
||||
// shooterControls
|
||||
// );
|
||||
|
||||
Autocommand.preNameCommands(
|
||||
IntakerSubsystem,
|
||||
drivetrain,
|
||||
shooterSubsystem,
|
||||
superstructure,
|
||||
intakeRotaterSubsystem,
|
||||
driveControls,
|
||||
shooterControls
|
||||
operatorControls
|
||||
);
|
||||
SignalLogger.enableAutoLogging(false);
|
||||
autoChooser = AutoBuilder.buildAutoChooser();
|
||||
@@ -114,77 +110,44 @@ public class RobotContainer {
|
||||
Pose2d startingPose = Constants.StartingPoints.Red.Point2;
|
||||
drivetrain.resetPose(startingPose);
|
||||
|
||||
// 默认驾驶命令
|
||||
/*
|
||||
* LeftX 底盘左右
|
||||
* LeftY 底盘前后 Bump/Trench偏移未完成
|
||||
* RightX Aiming偏移 AllTelop模式下 底盘旋转
|
||||
*
|
||||
*/
|
||||
// 默认驾驶命令说明：
|
||||
// 左摇杆 X 控制底盘左右平移。
|
||||
// 左摇杆 Y 控制底盘前后平移，越障/壕沟偏移逻辑目前仍沿用旧状态。
|
||||
// 右摇杆 X 在自动瞄准模式下作为偏移量，在全手动模式下控制底盘旋转。
|
||||
drivetrain.setDefaultCommand(driveControls.defaultDriveCommand());
|
||||
|
||||
// Disabled 时进入 idle（防止模块乱动）
|
||||
// 禁用状态下进入空闲命令，防止模块继续响应上一周期的运动请求。
|
||||
RobotModeTriggers.disabled().whileTrue(driveControls.idleCommand());
|
||||
|
||||
// Back 键：重置场地坐标系角度。
|
||||
controller.back().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric));
|
||||
|
||||
//左保险：按住刹车
|
||||
controllerlower.leftBumper()
|
||||
.onTrue(Commands.runOnce(driveControls::emergencyStop))
|
||||
.whileTrue(driveControls.brakeWhileHeld());
|
||||
// 预留的左摇杆自动瞄准切换，当前保持注释，避免改变驾驶员已习惯的按键。
|
||||
// controller.leftStick().onTrue(operatorControls.requestAimHubCommand());
|
||||
// controller.leftStick().onFalse(operatorControls.requestIdleCommand());
|
||||
|
||||
// 右保险：按住加速（Boost）
|
||||
controllerlower.rightBumper()
|
||||
.onTrue(Commands.runOnce(() -> driveControls.setBoostEnabled(true)))
|
||||
.onFalse(Commands.runOnce(() -> driveControls.setBoostEnabled(false)));
|
||||
|
||||
// Back：重置场地坐标系角度
|
||||
controllerlower.back().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric));
|
||||
|
||||
// Start：强行使用metaTag2全场定位
|
||||
controllerlower.start().whileTrue(drivetrain.run(drivetrain::forceUsingLimelightmt2));
|
||||
|
||||
controllerlower.a().whileTrue(drivetrain.run(()->{drivetrain.forceUsingLimelightmt2WithllName(Constants.Limelight.LIMELIGHT_NAME_Shooter);
|
||||
System.out.println("Try using shooter for metatag2");
|
||||
}));
|
||||
controllerlower.b().whileTrue(drivetrain.run(()->{drivetrain.forceUsingLimelightmt2WithllName(Constants.Limelight.LIMELIGHT_NAME_Intaker);
|
||||
System.out.println("Try using intaker for metatag2");
|
||||
}));
|
||||
//LM 按下时autoaimming，松开时alltelop
|
||||
// controllerlower.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming));
|
||||
// controllerlower.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
|
||||
|
||||
//双操均可触发autoaimming模式，松开时alltelop
|
||||
Trigger autoAim = controllerupper.rightBumper();//.or(controllerlower.rightStick());
|
||||
autoAim.onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming));
|
||||
autoAim.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
|
||||
|
||||
//双操均可触发passingball模式，松开时alltelop
|
||||
Trigger autoAim2 = controllerupper.leftBumper();//.or(controllerlower.leftStick());
|
||||
autoAim2.onTrue(robotStatusManager.setStatusCommand(RobotStatus.PassingBall));
|
||||
autoAim2.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
|
||||
|
||||
controllerlower.leftTrigger()
|
||||
.onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingTrench))
|
||||
.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
|
||||
|
||||
controllerlower.rightTrigger()
|
||||
.onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingBump))
|
||||
.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
|
||||
|
||||
controllerlower.a()
|
||||
.onTrue(robotStatusManager.setStatusCommand(RobotStatus.Climbing))
|
||||
.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
|
||||
}
|
||||
|
||||
private void configueShooter(){
|
||||
shooterSubsystem.setDefaultCommand(shooterControls.defaultShooterCommand());
|
||||
controllerupper.povLeft().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(-1)));
|
||||
controllerupper.povRight().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(1)));
|
||||
controllerupper.povUp().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(100)));
|
||||
controllerupper.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-100)));
|
||||
controllerupper.back().onTrue(Commands.runOnce(() -> shooterControls.resetOffsets()));
|
||||
controllerupper.start().onTrue(Commands.runOnce(() -> shooterControls.resetBackboardCommand()));
|
||||
|
||||
private void configueShooter() {
|
||||
// 射击相关按钮只请求 Superstructure 状态；是否喂球由 Superstructure
|
||||
// 根据飞轮速度和底盘瞄准统一判断。
|
||||
// 这些命令 require Superstructure，与自动程序共用需求；
|
||||
// 用 teleop 门控避免自动赛阶段误触直接取消整个 PathPlanner 自动。
|
||||
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());
|
||||
}
|
||||
|
||||
private void bingdingLED() {
|
||||
@@ -192,33 +155,48 @@ public class RobotContainer {
|
||||
}
|
||||
|
||||
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();
|
||||
}
|
||||
|
||||
public void dashboardPeriodic() {
|
||||
gameData2026.periodic();
|
||||
}
|
||||
|
||||
// private void configueClimber() {
|
||||
// climberSubsystem.setDefaultCommand(climberControls.defaultClimberCommand());
|
||||
// }
|
||||
|
||||
private void configueIntaker() {
|
||||
IntakerSubsystem.setDefaultCommand(intakerControls.defaultIntakerCommand());
|
||||
//controllerupper.a().onTrue(intakerControls.resetIntakerRotaterEncoderCommand(IntakerSubsystem));
|
||||
// 进球相关按钮也只请求 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());
|
||||
}
|
||||
|
||||
// public ClimberSubsystem getClimberSubsystem() {
|
||||
// return climberSubsystem;
|
||||
// }
|
||||
|
||||
}
|
||||
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
@@ -2,188 +2,66 @@ package frc.robot.controls;
|
||||
|
||||
import com.pathplanner.lib.auto.NamedCommands;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
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.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.Subsystem;
|
||||
import frc.robot.subsystems.ClimberSubsystem;
|
||||
import frc.robot.RobotStatusManager;
|
||||
import frc.robot.subsystems.IntakerSubsystem;
|
||||
import frc.robot.subsystems.ShooterSubsystem;
|
||||
import frc.robot.subsystems.CommandSwerveDrivetrain;
|
||||
import frc.robot.Constants;
|
||||
import frc.robot.Constants.RobotStatus;
|
||||
import frc.robot.subsystems.IntakeRotaterSubsystem;
|
||||
import frc.robot.subsystems.Superstructure;
|
||||
|
||||
public class Autocommand extends SequentialCommandGroup{
|
||||
|
||||
public static void preNameCommands(
|
||||
// ClimberSubsystem climber,
|
||||
IntakerSubsystem intaker,
|
||||
CommandSwerveDrivetrain drivetrain,
|
||||
ShooterSubsystem shooter,
|
||||
DriveControls driveControls,
|
||||
ShooterControls shooterControls
|
||||
/**
|
||||
* 注册 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("climb_up", climb_up(climber));
|
||||
// NamedCommands.registerCommand("climb_down", climb_down(climber));
|
||||
NamedCommands.registerCommand("start_intake", start_intake(intaker));
|
||||
NamedCommands.registerCommand("start_intake_and_conveyor", start_intake_and_conveyor(intaker,shooter));
|
||||
NamedCommands.registerCommand("stop_intake_and_conveyor", stop_intake_and_conveyor(intaker,shooter));
|
||||
NamedCommands.registerCommand("start_intake_timelimit", start_intake_timelimit(intaker));
|
||||
NamedCommands.registerCommand("stop_intake", stop_intake(intaker));
|
||||
NamedCommands.registerCommand("intaker_down", intaker_down(intaker));
|
||||
NamedCommands.registerCommand("intaker_up", intaker_up(intaker));
|
||||
//NamedCommands.registerCommand("auto_align_to_climb", autoAlignToClimb(drivetrain));
|
||||
NamedCommands.registerCommand("shoot", shoot(shooter));
|
||||
NamedCommands.registerCommand("Shake", shake(driveControls));
|
||||
NamedCommands.registerCommand("AutoAim", autoAim(driveControls));
|
||||
NamedCommands.registerCommand("AutoShoot", autoShoot(shooterControls));
|
||||
// NamedCommands.registerCommand("climber_init", climber_Zeroencoder(climber));
|
||||
NamedCommands.registerCommand("reset_backboard0", resetBackboard0Command(shooterControls));
|
||||
|
||||
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 climb_up(ClimberSubsystem climber) {
|
||||
return new RunCommand(() -> {
|
||||
climber.climbUp(); // 控制爬升器上升
|
||||
}, climber).withTimeout(2).andThen(new RunCommand(() -> {
|
||||
climber.climbUpNonStopforAuto();
|
||||
}, climber).withTimeout(0.5)).andThen(new InstantCommand(() -> {
|
||||
climber.captureHoldPosition(); // 捕获当前爬升器位置以保持
|
||||
climber.holdPosition(); // 停止并锁定当前爬升器位置
|
||||
}, climber));
|
||||
}
|
||||
public static Command climb_down(ClimberSubsystem climber) {
|
||||
return new RunCommand(() -> {
|
||||
climber.climbDown(); // 控制爬升器下降
|
||||
}, climber).withTimeout(2.0)
|
||||
.andThen(new InstantCommand(() -> {
|
||||
climber.captureHoldPosition(); // 捕获当前爬升器位置以保持
|
||||
climber.holdPosition(); // 停止并锁定当前爬升器位置
|
||||
}, climber));
|
||||
}
|
||||
|
||||
public static Command start_intake(IntakerSubsystem intaker) {
|
||||
return new InstantCommand(() -> {
|
||||
intaker.setIntakerGetterSpeed(Constants.Intaker.IntakeGetterSpeedforAuto); // 启动吸球
|
||||
}, intaker);
|
||||
}
|
||||
public static Command start_intake_and_conveyor(IntakerSubsystem intaker, ShooterSubsystem shooter) {
|
||||
return new InstantCommand(() -> {
|
||||
shooter.setConveyorSpeedByRPS(Constants.Shooter.conveyorSpeedForAuto);
|
||||
intaker.setIntakerGetterSpeed(Constants.Intaker.IntakeGetterSpeedforAuto); // 启动吸球
|
||||
|
||||
}, intaker, shooter);
|
||||
}
|
||||
public static Command stop_intake_and_conveyor(IntakerSubsystem intaker, ShooterSubsystem shooter) {
|
||||
return new InstantCommand(() -> {
|
||||
intaker.setIntakerGetterSpeed(0); // 启动吸球
|
||||
shooter.setConveyorSpeedByRPS(0);
|
||||
}, intaker, shooter);
|
||||
}
|
||||
public static Command start_intake_timelimit(IntakerSubsystem intaker) {
|
||||
return new RunCommand(() -> {
|
||||
intaker.setIntakerGetterSpeed(Constants.Intaker.IntakeGetterSpeedforAuto); // 吸球机构下降
|
||||
}, intaker).withTimeout(1.5).andThen(new InstantCommand(() -> {
|
||||
intaker.setIntakerGetterSpeed(0); // 停止旋转
|
||||
}, intaker));
|
||||
}
|
||||
public static Command stop_intake(IntakerSubsystem intaker) {
|
||||
return new RunCommand(() -> {
|
||||
intaker.setIntakerGetterSpeed(0); // 停止吸球
|
||||
}, intaker);
|
||||
}
|
||||
public static Command intaker_down(IntakerSubsystem intaker) {
|
||||
return new RunCommand(() -> {
|
||||
intaker.IntakerDownNonStop(); // 吸球机构下降
|
||||
}, intaker).withTimeout(0.6)
|
||||
|
||||
.andThen(new RunCommand(() -> {
|
||||
intaker.IntakerUpNonStop(); // 吸球机构上升
|
||||
}, intaker).withTimeout(0.1))
|
||||
|
||||
.andThen(new InstantCommand(() -> {
|
||||
intaker.stopIntakerotater(); // 停止
|
||||
}, intaker));
|
||||
}
|
||||
public static Command intaker_up(IntakerSubsystem intaker) {
|
||||
return new RunCommand(() -> {
|
||||
intaker.IntakerUpNonStop(); // 吸球机构上升
|
||||
}, intaker).withTimeout(0.4).andThen(new InstantCommand(() -> {
|
||||
intaker.stopIntakerotater(); // 停止旋转
|
||||
}, intaker));
|
||||
}
|
||||
// public static Command autoAlignToClimb(CommandSwerveDrivetrain drivetrain) {
|
||||
// return Commands.sequence(
|
||||
// new InstantCommand(() -> {
|
||||
// LimelightSupplier.setPipeline(Constants.Limelight.climbPipelineIndex);
|
||||
// }),
|
||||
// Commands.runEnd(
|
||||
// () -> {
|
||||
// drivetrain.driveToAprilTag();
|
||||
// },
|
||||
// () -> {
|
||||
// LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
|
||||
// },
|
||||
// drivetrain
|
||||
// ).until(() -> drivetrain.isAlignedToAprilTag()));
|
||||
// }
|
||||
|
||||
public static Command shoot(ShooterSubsystem shooter) {
|
||||
return new RunCommand(() -> {
|
||||
shooter.setFlywheelSpeedByRPS(64);
|
||||
shooter.setBackboardPosition(175); // 设置自动射击转速
|
||||
if(shooter.isBackboardAtTarget()){
|
||||
shooter.backboardMotor.set(0);
|
||||
}else{
|
||||
shooter.outputBackboard();
|
||||
}
|
||||
}, shooter).withTimeout(0.25).andThen(new RunCommand(() -> {
|
||||
shooter.setFlywheelSpeedByRPS(64);
|
||||
shooter.setBackboardPosition(175);
|
||||
if(shooter.isBackboardAtTarget()){
|
||||
shooter.backboardMotor.set(0);
|
||||
}else{
|
||||
shooter.outputBackboard();
|
||||
}
|
||||
shooter.setConveyorSpeedByRPS(25);
|
||||
}, shooter).withTimeout(1.5).andThen(new InstantCommand(() -> {
|
||||
shooter.setFlywheelSpeedByRPS(0); // 停止射击
|
||||
//shooter.setBackboardPosition(0); // 恢复背板位置
|
||||
shooter.setConveyorSpeedByRPS(0); // 停止输送
|
||||
}, shooter)));
|
||||
}
|
||||
public static Command autoAim(DriveControls driveControls) {
|
||||
return driveControls.autoAimCommand();
|
||||
}
|
||||
|
||||
public static Command autoShoot(ShooterControls shooterControl) {
|
||||
return shooterControl.autoShootToHubCommand();
|
||||
}
|
||||
public static Command resetBackboard0Command(ShooterControls shooterControl) {
|
||||
return shooterControl.resetBackboard0Command();
|
||||
}
|
||||
|
||||
public static Command shake(DriveControls driveControls) {
|
||||
return driveControls.shakeCommand();
|
||||
}
|
||||
|
||||
public static Command climber_Zeroencoder(ClimberSubsystem climber) {
|
||||
return climber.initCommand();
|
||||
}
|
||||
|
||||
|
||||
|
||||
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();
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
@@ -1,57 +0,0 @@
|
||||
package frc.robot.controls;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
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.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||
import frc.robot.Constants;
|
||||
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.getRightY() < -0.5){
|
||||
climber.climbUpNonStop();
|
||||
} else if(controller.getRightY() > 0.5){
|
||||
climber.climbDownNonStop();
|
||||
} else if (controller.y().getAsBoolean()) {
|
||||
climber.climbUp();
|
||||
} else if (controller.x().getAsBoolean()) {
|
||||
climber.climbDown();
|
||||
} else {
|
||||
// 松开：记录当前位置 + 锁死
|
||||
climber.captureHoldPosition();
|
||||
climber.holdPosition();
|
||||
}
|
||||
|
||||
|
||||
// 调试用
|
||||
table.getEntry("xHeld_up").setBoolean(controller.x().getAsBoolean());
|
||||
table.getEntry("yHeld_down").setBoolean(controller.y().getAsBoolean());
|
||||
}, climber);
|
||||
|
||||
}
|
||||
|
||||
public Command resetClimberEncoderCommand(ClimberSubsystem climber) {
|
||||
return Commands.runOnce(climber::zeroEncoder, climber);
|
||||
}
|
||||
|
||||
}
|
||||
@@ -15,11 +15,11 @@ 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;
|
||||
@@ -36,35 +36,35 @@ public class DriveControls {
|
||||
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 normalTurnScale = 0.7;
|
||||
// 默认速度比例，普通驾驶使用。
|
||||
private double normalDriveScale = 0.6;
|
||||
private double normalTurnScale = 0.8;
|
||||
|
||||
// 加速模式比例（按住 RB）
|
||||
// 加速模式比例，按住 RB 时使用。
|
||||
private double boostDriveScale = 1.0;
|
||||
private double boostTurnScale = 1.0;
|
||||
//autoaim模式
|
||||
private double slowDriveScale =0.1;
|
||||
// 自动瞄准时降低平移速度。
|
||||
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 =
|
||||
@@ -86,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");
|
||||
}
|
||||
|
||||
@@ -101,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);
|
||||
@@ -123,104 +123,40 @@ public class DriveControls {
|
||||
vy = limitedVy;
|
||||
vomega = limitedOmega;
|
||||
|
||||
double[] distanceAndRotation = calculateDistanceAndRotationToHub();
|
||||
// 将距离值写入 NetworkTable
|
||||
autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceAndRotation[0]);
|
||||
autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(distanceAndRotation[1]);
|
||||
autoControlNetworkTable.getEntry("distanceToPassball").setDouble(calculateDistanceAndRotationToPassBall()[0]);
|
||||
autoControlNetworkTable.getEntry("angleDifferenceToPassball").setDouble(calculateDistanceAndRotationToPassBall()[1]);
|
||||
double[] hubAim = calculateDistanceAndRotationToHub();
|
||||
double[] passAim = calculateDistanceAndRotationToPassBall();
|
||||
autoControlNetworkTable.getEntry("distanceToHub").setDouble(hubAim[0]);
|
||||
autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(hubAim[1]);
|
||||
autoControlNetworkTable.getEntry("distanceToPassball").setDouble(passAim[0]);
|
||||
autoControlNetworkTable.getEntry("angleDifferenceToPassball").setDouble(passAim[1]);
|
||||
|
||||
switch (robotStatusManager.getStatus()) {
|
||||
case Stopped:
|
||||
vx = 0;
|
||||
vy = 0;
|
||||
vomega = 0;
|
||||
break;
|
||||
case PassingBall:
|
||||
//LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
|
||||
vomega = calculateRotationSpeedFromRotationAngle(calculateDistanceAndRotationToPassBall()[1]);
|
||||
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
|
||||
break;
|
||||
case AutoAimming:
|
||||
//LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
|
||||
vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]);
|
||||
double[] activeAim = wantedState == WantedState.PASS_BALL ? passAim : hubAim;
|
||||
drivetrain.setTargetHeading(
|
||||
drivetrain.getRotation().plus(Rotation2d.fromDegrees(activeAim[1])));
|
||||
|
||||
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 AllTelop:
|
||||
case PASS_BALL:
|
||||
vomega = calculateRotationSpeedFromRotationAngle(passAim[1]);
|
||||
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
|
||||
break;
|
||||
default:
|
||||
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(Double.NaN);
|
||||
break;
|
||||
case Climbing:
|
||||
// alignToClimb();
|
||||
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
|
||||
);
|
||||
vy = isRedAlliance ? -vy : vy;
|
||||
}
|
||||
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;
|
||||
vomega = calculatevomegaFromRotationAngle(chosenError);
|
||||
autoControlNetworkTable.getEntry("BumpAutoRotationRate").setDouble(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
|
||||
);
|
||||
vy = isRedAlliance ? -vy : vy;
|
||||
}
|
||||
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;
|
||||
vomega = calculatevomegaFromRotationAngle(TrenchAngleerror);
|
||||
autoControlNetworkTable.getEntry("TrenchAutoRotationRate").setDouble(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) {
|
||||
|
||||
// 自动状态用 BlueAlliance（不翻转），手柄 field-centric 仍用原 fieldCentric（OperatorPerspective）
|
||||
//var chosen = isAutoLike ? fieldCentricAuto : fieldCentric;
|
||||
var chosen = fieldCentric;
|
||||
return chosen
|
||||
if (fieldCentricEnabled) {
|
||||
return fieldCentric
|
||||
.withVelocityX(vx)
|
||||
.withVelocityY(vy)
|
||||
.withRotationalRate(vomega);
|
||||
@@ -230,33 +166,26 @@ public class DriveControls {
|
||||
.withVelocityY(vy)
|
||||
.withRotationalRate(vomega);
|
||||
}
|
||||
}, () -> fieldCentricEnabled ||
|
||||
robotStatusManager.getStatus() == RobotStatus.CrossingBump ||
|
||||
robotStatusManager.getStatus() == RobotStatus.CrossingTrench
|
||||
? DriveMode.FIELD_CENTRIC : DriveMode.ROBOT_CENTRIC);
|
||||
}, () -> fieldCentricEnabled ? 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[] calculateDistanceAndRotationToHub() {
|
||||
Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
|
||||
Pose2d currentPose = drivetrain.getState().Pose; // 当前机器人位姿
|
||||
double targetX = isRedAlliance ? Constants.Field.RedHubPositionX : Constants.Field.BlueHubPositionX;
|
||||
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]
|
||||
@@ -266,26 +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; // 获取机器人当前的位置和角度
|
||||
double targetX = isRedAlliance ? Constants.Field.RedPassingBallPosX : Constants.Field.BluePassingBallPosX; // 目标 X 坐标
|
||||
double targetY1 = Constants.Field.PassingBallPosY1; // 目标 Y 坐标
|
||||
double targetY2 = Constants.Field.PassingBallPosY2; // 目标 Y 坐标
|
||||
Pose2d currentPose = drivetrain.getState().Pose; // 当前机器人位姿
|
||||
double targetX = isRedAlliance ? Constants.Field.RedPassingBallPosX : Constants.Field.BluePassingBallPosX; // 传球目标 X 坐标
|
||||
double targetY1 = Constants.Field.PassingBallPosY1; // 传球目标 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]
|
||||
@@ -295,12 +224,12 @@ public class DriveControls {
|
||||
angleDifference += 360;
|
||||
}
|
||||
|
||||
// 计算距离
|
||||
// 计算距离。
|
||||
double distance = Math.sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
// 计算距离和旋转角度
|
||||
// 返回距离和旋转角度误差。
|
||||
double[] distanceAndRotation = new double[]{distance, angleDifference};
|
||||
|
||||
// 返回距离和角度差
|
||||
// 距离单位为米，角度单位为度。
|
||||
return distanceAndRotation;
|
||||
}
|
||||
|
||||
@@ -315,7 +244,11 @@ 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;
|
||||
}
|
||||
@@ -328,26 +261,26 @@ public class DriveControls {
|
||||
} 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 内部状态，防止下一次突变
|
||||
return target;
|
||||
limiter.reset(target); // 同步 limiter 内部状态，防止下一次突跳。
|
||||
return target;
|
||||
}
|
||||
// 否则是加速，使用 limiter
|
||||
// 否则是加速，使用 limiter。
|
||||
return limiter.calculate(target);
|
||||
}
|
||||
|
||||
/** 紧急停止：清零限速器，让速度输出立刻归零 */
|
||||
/** 紧急停止：清零限速器，让速度输出立刻归零。 */
|
||||
public void emergencyStop() {
|
||||
vxLimiter.reset(0.0);
|
||||
vyLimiter.reset(0.0);
|
||||
@@ -385,9 +318,9 @@ public class DriveControls {
|
||||
return maxAngularRate;
|
||||
}
|
||||
public void setTeamColors() {
|
||||
Optional<Alliance> ally = DriverStation.getAlliance(); // 获取队伍颜色
|
||||
Optional<Alliance> ally = DriverStation.getAlliance(); // 获取联盟颜色
|
||||
if (ally.isPresent()) {
|
||||
// 根据队伍颜色设置布尔值
|
||||
// 根据联盟颜色更新自动瞄准目标。
|
||||
if (ally.get() == Alliance.Red) {
|
||||
isRedAlliance = true; // 红队
|
||||
} else if (ally.get() == Alliance.Blue) {
|
||||
@@ -405,31 +338,37 @@ public class DriveControls {
|
||||
public Command getAllianceColorCommand() {
|
||||
return Commands.runOnce(this::setTeamColors);
|
||||
}
|
||||
// public void alignToClimb(){
|
||||
// LimelightSupplier.setPipeline(Constants.Limelight.climbPipelineIndex);
|
||||
// if(!drivetrain.isAlignedToAprilTag()){
|
||||
// drivetrain.driveToAprilTag();
|
||||
// }
|
||||
// else{
|
||||
// LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
|
||||
// drivetrain.stop();
|
||||
// }
|
||||
// }
|
||||
public Command autoAimCommand() {
|
||||
|
||||
// DriveControls.java
|
||||
public Command autoAimForShootCommand() {
|
||||
return drivetrain.applyRequest(() -> {
|
||||
double[] distanceAndRotation = calculateDistanceAndRotationToHub();
|
||||
double vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]);
|
||||
return fieldCentric
|
||||
.withVelocityX(0.0)
|
||||
.withVelocityY(0.0)
|
||||
.withRotationalRate(vomega);
|
||||
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)
|
||||
.until(() -> Math.abs(calculateDistanceAndRotationToHub()[1]) < 1.5)
|
||||
.withTimeout(1.0)
|
||||
.andThen(new InstantCommand(() -> {
|
||||
drivetrain.stop();
|
||||
}, drivetrain));
|
||||
.withTimeout(Constants.Superstructure.shootTimeoutSeconds)
|
||||
.andThen(new InstantCommand(() -> drivetrain.stop(), drivetrain));
|
||||
}
|
||||
|
||||
public Command shakeCommand(double amplitude, double switchPeriod, double totalTime) {
|
||||
Timer timer = new Timer();
|
||||
|
||||
@@ -452,7 +391,7 @@ public class DriveControls {
|
||||
break;
|
||||
case 1:
|
||||
vx = 0.0;
|
||||
vy = amplitude; // 右
|
||||
vy = amplitude; // 左
|
||||
break;
|
||||
case 2:
|
||||
vx = -amplitude; // 后
|
||||
@@ -460,7 +399,7 @@ public class DriveControls {
|
||||
break;
|
||||
case 3:
|
||||
vx = 0.0;
|
||||
vy = -amplitude; // 左
|
||||
vy = -amplitude; // 右
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -482,9 +421,13 @@ public class DriveControls {
|
||||
|
||||
public void pushDistanceData(){
|
||||
double[] distanceAndRotation = calculateDistanceAndRotationToHub();
|
||||
// 将距离值写入 NetworkTable
|
||||
// 将距离数据写入 NetworkTable。
|
||||
autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceAndRotation[0]);
|
||||
autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(distanceAndRotation[1]);
|
||||
// 自动阶段默认驾驶命令不运行，这里同样保持目标朝向最新，
|
||||
// 让自动程序里的射击门控 isAimed() 有效。
|
||||
drivetrain.setTargetHeading(
|
||||
drivetrain.getRotation().plus(Rotation2d.fromDegrees(distanceAndRotation[1])));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -1,96 +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.IntakerSubsystem;
|
||||
import frc.robot.subsystems.ShooterSubsystem;
|
||||
import frc.robot.Constants;
|
||||
|
||||
|
||||
public class IntakerControls {
|
||||
private final IntakerSubsystem IntakerSubsystem;
|
||||
private final CommandXboxController controller;
|
||||
private final ShooterSubsystem ShooterSubsystem;
|
||||
|
||||
//private static final double ROTATER_STEP_PER_CYCLE = 0.00;
|
||||
|
||||
public IntakerControls(IntakerSubsystem Intaker, ShooterSubsystem Shooter, CommandXboxController controller) {
|
||||
this.ShooterSubsystem = Shooter;
|
||||
this.IntakerSubsystem = Intaker;
|
||||
this.controller = controller;
|
||||
}
|
||||
|
||||
|
||||
|
||||
//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.run(() -> {
|
||||
double getterspeed = 0;
|
||||
double statorCurrent = ShooterSubsystem.getConveyorStatorCurrent();
|
||||
|
||||
if (controller.getLeftTriggerAxis() > 0.1) {
|
||||
getterspeed = controller.getLeftTriggerAxis() * 0.5 + 0.3;
|
||||
}
|
||||
|
||||
IntakerSubsystem.setIntakerGetterSpeed(getterspeed);
|
||||
|
||||
if(controller.getLeftY() < -0.5){
|
||||
IntakerSubsystem.IntakerUpNonStop();
|
||||
} else if(controller.getLeftY() > 0.5){
|
||||
IntakerSubsystem.IntakerDownNonStop();
|
||||
} else {
|
||||
IntakerSubsystem.stopIntakerotater();
|
||||
}
|
||||
}, IntakerSubsystem);
|
||||
}
|
||||
public Command resetIntakerRotaterEncoderCommand(IntakerSubsystem intakerSubsystem) {
|
||||
return Commands.runOnce(intakerSubsystem::zeroEncoder, intakerSubsystem);
|
||||
}
|
||||
}
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
@@ -1,225 +0,0 @@
|
||||
package frc.robot.controls;
|
||||
|
||||
import java.util.Set;
|
||||
|
||||
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.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||
import frc.robot.Constants;
|
||||
import frc.robot.RobotStatusManager;
|
||||
import frc.robot.Constants.Intaker;
|
||||
import frc.robot.Constants.RobotStatus;
|
||||
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:
|
||||
shooterSubsystem.setBackboardPosition(0);
|
||||
if(shooterSubsystem.isBackboardAtTarget()){
|
||||
shooterSubsystem.backboardMotor.set(0);
|
||||
}else{
|
||||
shooterSubsystem.outputBackboard();
|
||||
}
|
||||
shooterSubsystem.setFlywheelSpeedByRPS(0);
|
||||
if(controller.a().getAsBoolean()) { shooterSubsystem.setConveyorSpeedByRPS(Constants.Shooter.conveyorSpeed);}
|
||||
else{shooterSubsystem.setConveyorSpeedByRPS(0);}
|
||||
break;
|
||||
case PassingBall:
|
||||
var xP = shooterSubsystem.getDistanceToPassball();
|
||||
xP = Math.max(2.5, Math.min(10.0, xP));
|
||||
flywheelSpeed = +0.004960317456971805*xP*xP*xP*xP*xP*xP*xP-0.22361111097424535*xP*xP*xP*xP*xP*xP+4.234722219890569*xP*xP*xP*xP*xP-43.63194442298942*xP*xP*xP*xP+264.05555544032126*xP*xP*xP-938.6444440829168*xP*xP+1818.2047612908214*xP-1415.9999995643104;
|
||||
flywheelSpeed += flywheelSpeedOffset;
|
||||
backboardPosition = +0.10912698402591796*xP*xP*xP*xP*xP*xP*xP-5.277777773586415*xP*xP*xP*xP*xP*xP+105.55555548303796*xP*xP*xP*xP*xP-1125.6944437656143*xP*xP*xP*xP+6865.763885174154*xP*xP*xP-23769.027765888906*xP*xP+43328.57140795847*xP-31599.99998506075;
|
||||
conveyorSpeed = Constants.Shooter.conveyorSpeed;
|
||||
if (controller.getRightTriggerAxis() < 0.1) {
|
||||
flywheelSpeed = 0;
|
||||
conveyorSpeed = 0;
|
||||
}
|
||||
if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.9){
|
||||
conveyorSpeed = 0;
|
||||
}
|
||||
//if(controller.a().getAsBoolean()) { conveyorSpeed = Constants.Shooter.conveyorSpeed;}
|
||||
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.getDistanceToHub();
|
||||
x = Math.max(1.2, Math.min(4.5, x));
|
||||
flywheelSpeed = -1.4964026859575532*x*x*x*x*x*x*x+29.82445640241506*x*x*x*x*x*x-247.71603153159896*x*x*x*x*x+1108.8727302184625*x*x*x*x-2881.517500617481*x*x*x+4332.05363145275*x*x-3467.2508337871454*x+1192.1508062663759;
|
||||
flywheelSpeed += flywheelSpeedOffset;
|
||||
backboardPosition = +34.5286998307899*x*x*x*x*x*x*x*x-788.5788623683884*x*x*x*x*x*x*x+7658.073303693148*x*x*x*x*x*x-41191.69338972519*x*x*x*x*x+133870.54588688645*x*x*x*x-268559.6987062158*x*x*x+324254.6530324262*x*x-215015.48893131423*x+59977.46545382827;
|
||||
backboardPosition += backboardPositionOffset;
|
||||
conveyorSpeed = Constants.Shooter.conveyorSpeed;
|
||||
if (controller.getRightTriggerAxis() < 0.1) {
|
||||
flywheelSpeed = 0;
|
||||
conveyorSpeed = 0;
|
||||
}
|
||||
if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.9){
|
||||
conveyorSpeed = 0;
|
||||
}
|
||||
//if(controller.a().getAsBoolean()) { conveyorSpeed = Constants.Shooter.conveyorSpeed;}
|
||||
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);
|
||||
}
|
||||
|
||||
public void adjustFlywheelSpeedOffset(double offset) {
|
||||
flywheelSpeedOffset += offset;
|
||||
}
|
||||
|
||||
public void adjustBackboardRateOffset(double offset) {
|
||||
backboardPositionOffset += offset;
|
||||
}
|
||||
public void resetOffsets(){
|
||||
flywheelSpeedOffset = 0.0;
|
||||
backboardPositionOffset = 0.0;
|
||||
}
|
||||
|
||||
public Command autoShootToHubCommand() {
|
||||
return Commands.defer(() -> {
|
||||
double x = shooterSubsystem.getDistanceToHub();
|
||||
x = Math.max(1.2, Math.min(4.5, x));
|
||||
|
||||
double flywheelTargetSpeed = -1.4964026859575532*x*x*x*x*x*x*x+29.82445640241506*x*x*x*x*x*x-247.71603153159896*x*x*x*x*x+1108.8727302184625*x*x*x*x-2881.517500617481*x*x*x+4332.05363145275*x*x-3467.2508337871454*x+1192.1508062663759;
|
||||
double backboardTargetPosition = +34.5286998307899*x*x*x*x*x*x*x*x-788.5788623683884*x*x*x*x*x*x*x+7658.073303693148*x*x*x*x*x*x-41191.69338972519*x*x*x*x*x+133870.54588688645*x*x*x*x-268559.6987062158*x*x*x+324254.6530324262*x*x-215015.48893131423*x+59977.46545382827;
|
||||
double conveyorTargetSpeed = Constants.Shooter.conveyorSpeed;
|
||||
|
||||
return new RunCommand(() -> {
|
||||
shooterSubsystem.setFlywheelSpeedByRPS(flywheelTargetSpeed);
|
||||
shooterSubsystem.setConveyorSpeedByRPS(0.0);
|
||||
shooterSubsystem.setBackboardPosition(backboardTargetPosition);
|
||||
if (shooterSubsystem.isBackboardAtTarget()) {
|
||||
shooterSubsystem.backboardMotor.set(0);
|
||||
} else {
|
||||
shooterSubsystem.outputBackboard();
|
||||
}
|
||||
}, shooterSubsystem)
|
||||
.until(() ->
|
||||
shooterSubsystem.isBackboardAtTarget()
|
||||
&& shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble() >= flywheelTargetSpeed * 0.9
|
||||
)
|
||||
.withTimeout(0.3)
|
||||
.andThen(new RunCommand(() -> {
|
||||
shooterSubsystem.setFlywheelSpeedByRPS(flywheelTargetSpeed);
|
||||
shooterSubsystem.setBackboardPosition(backboardTargetPosition);
|
||||
if (shooterSubsystem.isBackboardAtTarget()) {
|
||||
shooterSubsystem.backboardMotor.set(0);
|
||||
} else {
|
||||
shooterSubsystem.outputBackboard();
|
||||
}
|
||||
shooterSubsystem.setConveyorSpeedByRPS(25);
|
||||
}, shooterSubsystem).withTimeout(5.0))
|
||||
.andThen(new InstantCommand(() -> {
|
||||
shooterSubsystem.setFlywheelSpeedByRPS(0.0);
|
||||
shooterSubsystem.setConveyorSpeedByRPS(0.0);
|
||||
shooterSubsystem.backboardMotor.set(0.0);
|
||||
}, shooterSubsystem));
|
||||
}, Set.of(shooterSubsystem));
|
||||
}
|
||||
|
||||
public Command resetBackboard0Command() {
|
||||
return new RunCommand(() -> {
|
||||
shooterSubsystem.setBackboardPosition(0);
|
||||
|
||||
if (shooterSubsystem.isBackboardAtTarget()) {
|
||||
shooterSubsystem.backboardMotor.set(0);
|
||||
} else {
|
||||
shooterSubsystem.outputBackboard();
|
||||
}
|
||||
|
||||
}, shooterSubsystem)
|
||||
.until(() -> shooterSubsystem.isBackboardAtTarget())
|
||||
.andThen(new InstantCommand(() -> {
|
||||
shooterSubsystem.backboardMotor.set(0);
|
||||
}));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -24,14 +24,14 @@ public class TunerConstants {
|
||||
// The steer motor uses any SwerveModule.SteerRequestType control request with the
|
||||
// output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
|
||||
private static final Slot0Configs steerGains = new Slot0Configs()
|
||||
.withKP(100).withKI(0).withKD(0.5)
|
||||
.withKP(70).withKI(0).withKD(0.5)
|
||||
.withKS(0.1).withKV(1.59).withKA(0)
|
||||
.withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
|
||||
// When using closed-loop control, the drive motor uses the control
|
||||
// output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
|
||||
private static final Slot0Configs driveGains = new Slot0Configs()
|
||||
.withKP(0.1).withKI(0).withKD(0)
|
||||
.withKS(0).withKV(0.124);
|
||||
.withKP(0.01).withKI(0).withKD(0)
|
||||
.withKS(0.05).withKV(0.117);
|
||||
|
||||
// The closed-loop output type to use for the steer motors;
|
||||
// This affects the PID/FF gains for the steer motors
|
||||
@@ -46,8 +46,8 @@ public class TunerConstants {
|
||||
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*
|
||||
private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder;
|
||||
// Use RemoteCANcoder so the steer TalonFXs do not require Phoenix Pro licenses.
|
||||
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
|
||||
@@ -73,7 +73,16 @@ 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
|
||||
@@ -99,7 +108,7 @@ public class TunerConstants {
|
||||
|
||||
// These are only used for simulation
|
||||
private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01);
|
||||
private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.035);
|
||||
private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01);
|
||||
// Simulated voltage necessary to overcome friction
|
||||
private static final Voltage kSteerFrictionVoltage = Volts.of(0.2);
|
||||
private static final Voltage kDriveFrictionVoltage = Volts.of(0.2);
|
||||
|
||||
@@ -1,127 +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.Command;
|
||||
import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc.robot.Constants;
|
||||
|
||||
|
||||
public class ClimberSubsystem extends SubsystemBase {
|
||||
|
||||
private boolean autoClimbUsedInAuto = false;
|
||||
|
||||
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 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 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 climbDownAuto() {
|
||||
targetPositionTicks = Constants.Climber.climbDownAutoTicks;
|
||||
climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks));
|
||||
}
|
||||
|
||||
public void climbDownNonStop(){
|
||||
climberMotor.setControl(velocityRequest.withVelocity(Constants.Climber.climbVelocity));
|
||||
}
|
||||
|
||||
public void climbUpNonStop(){
|
||||
climberMotor.setControl(velocityRequest.withVelocity(-Constants.Climber.climbVelocity));
|
||||
}
|
||||
public void climbUpNonStopforAuto(){
|
||||
climberMotor.set(0.07);
|
||||
}
|
||||
|
||||
public void captureHoldPosition() {
|
||||
holdPositionTicks = getPositionTicks();
|
||||
}
|
||||
|
||||
public void holdPosition() {
|
||||
climberMotor.setControl(
|
||||
positionHoldRequest.withPosition(holdPositionTicks)
|
||||
);
|
||||
}
|
||||
|
||||
public void stopMotor() {
|
||||
climberMotor.set(0);
|
||||
}
|
||||
public void setMotor(double speed) {
|
||||
climberMotor.set(speed);
|
||||
}
|
||||
|
||||
public Command initCommand() {
|
||||
return Commands.sequence(
|
||||
Commands.run(() -> climberMotor.set(-0.07), this).withTimeout(0.5),
|
||||
Commands.runOnce(this::zeroEncoder, this)
|
||||
);
|
||||
}
|
||||
|
||||
public void zeroEncoder() {
|
||||
climberMotor.setPosition(0);
|
||||
stopMotor();
|
||||
}
|
||||
|
||||
public void stopAll() {
|
||||
climberMotor.set(0);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
climberTable.getEntry("velocityRps").setDouble(getVelocityRps());
|
||||
climberTable.getEntry("positionTicks").setDouble(getPositionTicks());
|
||||
climberTable.getEntry("holdPositionTicks").setDouble(holdPositionTicks);
|
||||
}
|
||||
}
|
||||
@@ -1,4 +1,5 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import java.util.Optional;
|
||||
import java.util.function.Supplier;
|
||||
|
||||
@@ -15,40 +16,39 @@ import edu.wpi.first.math.Matrix;
|
||||
import edu.wpi.first.math.VecBuilder;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
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.subsystems.LimelightHelpers.RawFiducial;
|
||||
import frc.robot.Constants;
|
||||
|
||||
import edu.wpi.first.networktables.*;
|
||||
|
||||
|
||||
/**
|
||||
* Class that extends the Phoenix 6 SwerveDrivetrain class and implements
|
||||
* Subsystem so it can easily be used in command-based projects.
|
||||
* Phoenix 6 Tuner X 生成底盘的命令式封装。
|
||||
*
|
||||
* Generated by the 2026 Tuner X Swerve Project Generator
|
||||
* https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
|
||||
* 底层 swerve、模块控制和里程计保持原结构；本类只在外层补充
|
||||
* 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 boolean isAlignedToAprilTag = false;
|
||||
|
||||
// 上层状态机射击门控使用的目标朝向；正常底盘控制仍由手动驾驶掌控。
|
||||
private Rotation2d targetHeading = Rotation2d.kZero;
|
||||
|
||||
private DriveMode m_driveMode = DriveMode.OTHER;
|
||||
|
||||
@@ -58,15 +58,14 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
private NetworkTable driveNetworkTable;
|
||||
|
||||
private final StructPublisher<Pose2d> posePub =
|
||||
NetworkTableInstance.getDefault()
|
||||
.getStructTopic("/Odometry/RobotPose", Pose2d.struct)
|
||||
.publish();
|
||||
NetworkTableInstance.getDefault()
|
||||
.getStructTopic("/Odometry/RobotPose", Pose2d.struct)
|
||||
.publish();
|
||||
|
||||
private Field2d m_field;
|
||||
private Field2d shooter_ll_field;
|
||||
private Field2d intaker_ll_field;
|
||||
|
||||
|
||||
public CommandSwerveDrivetrain(
|
||||
SwerveDrivetrainConstants drivetrainConstants,
|
||||
SwerveModuleConstants<?, ?, ?>... modules
|
||||
@@ -82,16 +81,13 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
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_intaker_NT = NetworkTableInstance.getDefault().getTable("Positioning/limelight-intaker");
|
||||
ll_shooter_NT = NetworkTableInstance.getDefault().getTable("Positioning/limelight-left");
|
||||
ll_intaker_NT = NetworkTableInstance.getDefault().getTable("Positioning/limelight-front");
|
||||
|
||||
configueAutoBuilder();
|
||||
|
||||
}
|
||||
|
||||
|
||||
private void configueAutoBuilder(){
|
||||
|
||||
private void configueAutoBuilder() {
|
||||
RobotConfig config;
|
||||
try {
|
||||
config = RobotConfig.fromGUISettings();
|
||||
@@ -100,20 +96,20 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
}
|
||||
|
||||
AutoBuilder.configure(
|
||||
this::getPose, // 当前机器人 Pose
|
||||
this::resetPose, // 重置里程计
|
||||
this::getRobotRelativeSpeeds, // ROBOT RELATIVE speeds
|
||||
(speeds, feedforwards) -> driveRobotRelative(speeds), // 用 speeds 驱动机器人
|
||||
this::getPose, // 当前机器人位姿。
|
||||
this::resetPose, // 重置里程计。
|
||||
this::getRobotRelativeSpeeds, // 机器人坐标系速度。
|
||||
(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) // Theta PID
|
||||
new PIDConstants(5.0, 0.0, 0.0), // X/Y 位置 PID。
|
||||
new PIDConstants(5.0, 0.0, 0.0) // 朝向 PID。
|
||||
),
|
||||
config,
|
||||
() -> false,
|
||||
this
|
||||
);
|
||||
|
||||
}
|
||||
|
||||
public Pose2d getPose() {
|
||||
return getState().Pose;
|
||||
}
|
||||
@@ -122,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.vyMetersPerSecond, // Y 不变
|
||||
speeds.omegaRadiansPerSecond * 1.3 // 只放大旋转
|
||||
speeds.vxMetersPerSecond, // X 方向速度保持原值。
|
||||
speeds.vyMetersPerSecond, // Y 方向速度保持原值。
|
||||
speeds.omegaRadiansPerSecond * 1.3 // 只放大旋转输出，沿用原驾驶手感。
|
||||
);
|
||||
|
||||
setControl(
|
||||
@@ -140,14 +162,26 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
);
|
||||
}
|
||||
|
||||
private static final double kMaxVisionJumpMeters = 1.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 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
|
||||
* @return Command to run
|
||||
* @param request 生成待下发请求的函数。
|
||||
* @return 可调度执行的 Command。
|
||||
*/
|
||||
public Command applyRequest(Supplier<SwerveRequest> request) {
|
||||
return run(() -> this.setControl(request.get()));
|
||||
@@ -163,7 +197,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
|
||||
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;
|
||||
@@ -175,7 +209,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
|
||||
public void forceUsingLimelightmt2WithllName(String llString) {
|
||||
LimelightHelpers.PoseEstimate mt2 =
|
||||
LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(llString);
|
||||
LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(llString);
|
||||
|
||||
if (mt2 == null || mt2.tagCount == 0) {
|
||||
return;
|
||||
@@ -184,14 +218,15 @@ 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() {
|
||||
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);
|
||||
@@ -202,32 +237,31 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
|
||||
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());
|
||||
@@ -258,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 基本不信任就给极大）
|
||||
// 你也可以根据 mt2.avgTagDist / mt2.tagSpan 动态调
|
||||
// 视觉测量噪声：theta 基本不信任，只融合平移。
|
||||
// 后续可以根据 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);
|
||||
}
|
||||
@@ -273,156 +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(); // 若你有这个信号
|
||||
|
||||
// 旋转太快时拒绝该帧，避免高速转动时的视觉跳变。
|
||||
double omegaDegPerSec = getPigeon2().getAngularVelocityZWorld().getValueAsDouble();
|
||||
if (Math.abs(omegaDegPerSec) > kMaxOmegaDegPerSec) {
|
||||
//System.out.println("NoUsingMetaTag2ForTooFastRotationSpeed");
|
||||
return false;
|
||||
}
|
||||
|
||||
// 3) 跳变太大：拒绝（Phoenix6 推荐的稳定策略）
|
||||
//Pose2d current = getState().Pose;
|
||||
// if (currentPose.getTranslation().getDistance(mt2.pose.getTranslation()) > kMaxVisionJumpMeters) {
|
||||
// //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));
|
||||
}
|
||||
|
||||
// public String driveToAprilTag() {
|
||||
// System.out.println("Pipeline = " + LimelightHelpers.getCurrentPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker));
|
||||
// System.out.println("TV = " + LimelightSupplier.isTargetVisible());
|
||||
|
||||
// if (!LimelightSupplier.isTargetVisible()) {
|
||||
// isAlignedToAprilTag = false;
|
||||
// //stop();
|
||||
// return "No Target";
|
||||
// }
|
||||
|
||||
// RawFiducial[] fiducials =
|
||||
// LimelightHelpers.getRawFiducials(Constants.Limelight.LIMELIGHT_NAME_Intaker);
|
||||
|
||||
// boolean foundDesiredTag = false;
|
||||
// for (RawFiducial fiducial : fiducials) {
|
||||
// if (fiducial.id == 16 || fiducial.id == 32) {
|
||||
// foundDesiredTag = true;
|
||||
// break;
|
||||
// }
|
||||
// }
|
||||
|
||||
// if (!foundDesiredTag) {
|
||||
// isAlignedToAprilTag = false;
|
||||
// stop();
|
||||
// return "No Desired Tag";
|
||||
// }
|
||||
|
||||
// // ===== 读取原始误差 =====
|
||||
// double tx = LimelightSupplier.getTX(); // deg
|
||||
// double tz = -LimelightSupplier.getTargetTZ(); // m，方向按你当前经验保留负号
|
||||
// double ry = LimelightSupplier.getTargetRotationY(); // deg
|
||||
|
||||
// // ===== 用原始误差判断是否已对齐 =====
|
||||
// isAlignedToAprilTag =
|
||||
// Math.abs(tx) < Constants.Limelight.AutoClimbToleranceTX &&
|
||||
// Math.abs(tz) < Constants.Limelight.AutoClimbToleranceTZ &&
|
||||
// Math.abs(ry) < Constants.Limelight.AutoClimbToleranceRY;
|
||||
|
||||
// if (isAlignedToAprilTag) {
|
||||
// stop();
|
||||
// return "Aligned";
|
||||
// }
|
||||
|
||||
// // ===== 计算输出 =====
|
||||
// double vx = 0.0;
|
||||
// double vy = 0.0;
|
||||
// double omega = 0.0;
|
||||
|
||||
// // 1) 前后控制：根据 tz 前进/后退
|
||||
// if (Math.abs(tz) > Constants.Limelight.AutoClimbToleranceTZ) {
|
||||
// vx = tz * Constants.Limelight.AutoClimbKpTZ;
|
||||
|
||||
// // 最小输出，避免快到目标时不动
|
||||
// if (Math.abs(vx) < Constants.Limelight.AutoClimbMinVX) {
|
||||
// vx = Math.copySign(Constants.Limelight.AutoClimbMinVX, vx);
|
||||
// }
|
||||
|
||||
// // 最大输出限幅
|
||||
// vx = clamp(vx,
|
||||
// -Constants.Limelight.AutoClimbMaxVX,
|
||||
// Constants.Limelight.AutoClimbMaxVX);
|
||||
// }
|
||||
|
||||
// // 2) 旋转控制：tx 为主，ry 为辅
|
||||
// // tx 负责“镜头中心对准 tag”
|
||||
// // ry 负责“机器人姿态微调”
|
||||
// // double omegaFromTx = 0.0;
|
||||
// // double omegaFromRy = 0.0;
|
||||
|
||||
// // if (Math.abs(tx) > Constants.Limelight.AutoClimbToleranceTX) {
|
||||
// // omegaFromTx = -tx * Constants.Limelight.AutoClimbKpTX;
|
||||
// // }
|
||||
|
||||
// // if (Math.abs(ry) > Constants.Limelight.AutoClimbToleranceRY) {
|
||||
// // omegaFromRy = ry * Constants.Limelight.AutoClimbKpRY;
|
||||
// // }
|
||||
|
||||
// // omega = omegaFromTx + omegaFromRy;
|
||||
|
||||
// // // 大偏角时直接给较明显的转速，先把朝向拉回来
|
||||
// // if (Math.abs(tx) > Constants.Limelight.AutoClimbFastTurnThresholdTX) {
|
||||
// // omega = Math.copySign(Constants.Limelight.AutoClimbFastTurnOmega, omega);
|
||||
// // }
|
||||
|
||||
// // // 最小输出，避免转不动
|
||||
// // if (Math.abs(omega) > 1e-6 && Math.abs(omega) < Constants.Limelight.AutoClimbMinOmega) {
|
||||
// // omega = Math.copySign(Constants.Limelight.AutoClimbMinOmega, omega);
|
||||
// // }
|
||||
|
||||
// // // 最大输出限幅
|
||||
// // omega = clamp(omega,
|
||||
// // -Constants.Limelight.AutoClimbMaxOmega,
|
||||
// // Constants.Limelight.AutoClimbMaxOmega);
|
||||
|
||||
// Rotation2d ClimbcurrentAngle = getState().Pose.getRotation();
|
||||
// double climbCurrentRad = ClimbcurrentAngle.getRadians();
|
||||
// double TargetAngleforClimbing = Math.toRadians(0);
|
||||
// double Climberror = TargetAngleforClimbing - climbCurrentRad;
|
||||
// if (Math.abs(Climberror) < Math.toRadians(Constants.AutoPositioning.autoPositioningAngleError)) {
|
||||
// omega = 0.0;
|
||||
// } else {
|
||||
// omega = Climberror * Constants.AutoPositioning.TurningkP;
|
||||
// }
|
||||
|
||||
|
||||
// // ===== 调试输出 =====
|
||||
// driveNetworkTable.getEntry("ClimbTX").setDouble(tx);
|
||||
// driveNetworkTable.getEntry("ClimbTZ").setDouble(tz);
|
||||
// driveNetworkTable.getEntry("ClimbRY").setDouble(ry);
|
||||
// driveNetworkTable.getEntry("ClimbVXCmd").setDouble(vx);
|
||||
// driveNetworkTable.getEntry("ClimbOmegaCmd").setDouble(omega);
|
||||
// driveNetworkTable.getEntry("ClimbAligned").setBoolean(isAlignedToAprilTag);
|
||||
|
||||
// // ===== 下发到底盘 =====
|
||||
// driveRobotRelative(new ChassisSpeeds(vx, vy, omega));
|
||||
|
||||
// return "Driving to AprilTag";
|
||||
// }
|
||||
|
||||
// public boolean isAlignedToAprilTag(){
|
||||
// return isAlignedToAprilTag;
|
||||
// }
|
||||
@Override
|
||||
public void periodic() {
|
||||
|
||||
Pose2d pose = getState().Pose;
|
||||
m_field.setRobotPose(pose);
|
||||
posePub.set(pose);
|
||||
@@ -431,20 +337,19 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
driveNetworkTable.getEntry("YawFromState").setDouble(getState().Pose.getRotation().getDegrees());
|
||||
driveNetworkTable.getEntry("DriveMode").setString(m_driveMode.name());
|
||||
driveNetworkTable.getEntry("IsFieldCentric").setBoolean(m_driveMode == DriveMode.FIELD_CENTRIC);
|
||||
//positioningNetworkTable.getEntry("isAlignedToAprilTag").setBoolean(isAlignedToAprilTag());
|
||||
// positioningNetworkTable.getEntry("interestPointsClimber TX").setDouble(LimelightSupplier.getTX());
|
||||
// positioningNetworkTable.getEntry("interestPointsClimber RY").setDouble(LimelightSupplier.getTargetRotationY());
|
||||
// positioningNetworkTable.getEntry("interestPointsClimber TZ").setDouble(-LimelightSupplier.getTargetTZ());
|
||||
|
||||
// 保留 Positioning 表中的基础状态输出。
|
||||
positioningNetworkTable.getEntry("interestPointsCl").setBoolean(false);
|
||||
// 1) 每帧喂给 LL：机器人当前 yaw（度），其余先全 0
|
||||
|
||||
// 每帧向 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
|
||||
);
|
||||
|
||||
@@ -462,13 +367,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.
|
||||
/**
|
||||
* 添加一帧视觉位姿测量，用于修正里程计估计。
|
||||
*
|
||||
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
|
||||
* @param timestampSeconds The timestamp of the vision measurement in seconds.
|
||||
* @param visionRobotPoseMeters 视觉测得的机器人位姿。
|
||||
* @param timestampSeconds 视觉测量时间戳，单位秒。
|
||||
*/
|
||||
@Override
|
||||
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
|
||||
@@ -476,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 timestampSeconds The timestamp of the vision measurement in seconds.
|
||||
* @param visionMeasurementStdDevs Standard deviations of the vision pose measurement
|
||||
* in the form [x, y, theta]ᵀ, with units in meters and radians.
|
||||
* @param visionRobotPoseMeters 视觉测得的机器人位姿。
|
||||
* @param timestampSeconds 视觉测量时间戳，单位秒。
|
||||
* @param visionMeasurementStdDevs 视觉测量标准差，格式为 [x, y, theta]。
|
||||
*/
|
||||
@Override
|
||||
public void addVisionMeasurement(
|
||||
@@ -498,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.
|
||||
* @return The pose at the given timestamp (or Optional.empty() if the buffer is empty).
|
||||
* @param timestampSeconds 位姿时间戳，单位秒。
|
||||
* @return 指定时间的位姿；缓存为空时返回 Optional.empty()。
|
||||
*/
|
||||
@Override
|
||||
public Optional<Pose2d> samplePoseAt(double timestampSeconds) {
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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());
|
||||
}
|
||||
}
|
||||
@@ -1,100 +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 double getGravityFFVolts(){
|
||||
double intakerotaterPosition = getIntakerotaterPosition();
|
||||
double gravityFFVolts = Constants.Intaker.IntakeRotaterGravityFF * intakerotaterPosition;
|
||||
return gravityFFVolts;
|
||||
}
|
||||
|
||||
public void setIntakerRotaterVelocityRps(double IntakeRotaterTargetVelocity) {
|
||||
double gravityFeedforward = getGravityFFVolts();
|
||||
intakerotater.setControl(velocityRequest.withVelocity(IntakeRotaterTargetVelocity).withFeedForward(gravityFeedforward));// 重力前馈);
|
||||
}
|
||||
|
||||
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.set(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));
|
||||
// }
|
||||
// }
|
||||
public void setIntakerGetterSpeed(double speed) {
|
||||
intakegetter.set(speed);
|
||||
}
|
||||
public void zeroEncoder(){
|
||||
intakerotater.setPosition(0);
|
||||
stopIntakerotater();
|
||||
}
|
||||
public void IntakerDownNonStop(){
|
||||
intakerotater.set(-0.1);
|
||||
}
|
||||
|
||||
public void IntakerUpNonStop(){
|
||||
intakerotater.set(0.2);
|
||||
}
|
||||
|
||||
public void setIntakerRotater(double rotaterspeed){
|
||||
intakerotater.set(rotaterspeed);
|
||||
}
|
||||
public void stopAll() {
|
||||
stopIntakerotater();
|
||||
setIntakerGetterSpeed(0);
|
||||
}
|
||||
}
|
||||
@@ -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());
|
||||
led.start();
|
||||
|
||||
timer.start();
|
||||
if (enabled) {
|
||||
led.setLength(buffer.getLength());
|
||||
led.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);
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -3,192 +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.geometry.Pose2d;
|
||||
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 TalonFX flywheelMotorLeft;
|
||||
private TalonFX flywheelMotorRight;
|
||||
public TalonFX conveyorMotor;
|
||||
public TalonFXS backboardMotor;
|
||||
public Encoder backboardEncoder;
|
||||
public final TalonFX leftFlywheel;
|
||||
public final TalonFX rightFlywheel;
|
||||
public final TalonFX leftConveyor;
|
||||
public final TalonFX rightConveyor;
|
||||
|
||||
// PID 控制器，控制背板角度
|
||||
public ProfiledPIDController backboardPID;
|
||||
private Follower flyWheelFollower;
|
||||
public final TalonFXS backboardMotor;
|
||||
public final Encoder backboardEncoder;
|
||||
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 VelocityVoltage velocityRequest = new VelocityVoltage(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() {
|
||||
// 初始化飞轮电机
|
||||
flywheelMotorLeft = new TalonFX(41); // 41是飞轮left的CAN ID
|
||||
flywheelMotorRight = new TalonFX(42); // 42是飞轮right的CAN ID
|
||||
flywheelMotorLeft.getConfigurator().apply(Constants.Shooter.flyWheelSlot0Configs);
|
||||
flywheelMotorRight.getConfigurator().apply(Constants.Shooter.flyWheelSlot0Configs);
|
||||
// 初始化传动电机
|
||||
conveyorMotor = new TalonFX(43);
|
||||
conveyorMotor.getConfigurator().apply(Constants.Shooter.conveyorSlot0Configs);
|
||||
leftFlywheel = new TalonFX(Constants.Shooter.leftFlywheelID);
|
||||
rightFlywheel = new TalonFX(Constants.Shooter.rightFlywheelID);
|
||||
leftConveyor = new TalonFX(Constants.Shooter.leftConveyorID);
|
||||
rightConveyor = new TalonFX(Constants.Shooter.rightConveyorID);
|
||||
|
||||
var configs1 = new MotorOutputConfigs();
|
||||
configs1.Inverted = InvertedValue.Clockwise_Positive;
|
||||
flywheelMotorLeft.getConfigurator().apply(configs1);
|
||||
flyWheelFollower = new Follower(flywheelMotorLeft.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50);
|
||||
leftFlywheel.getConfigurator().apply(Constants.Shooter.flyWheelSlot0Configs);
|
||||
rightFlywheel.getConfigurator().apply(Constants.Shooter.flyWheelSlot0Configs);
|
||||
leftConveyor.getConfigurator().apply(Constants.Shooter.conveyorSlot0Configs);
|
||||
rightConveyor.getConfigurator().apply(Constants.Shooter.conveyorSlot0Configs);
|
||||
|
||||
var configs2 = new MotorOutputConfigs();
|
||||
configs2.Inverted = InvertedValue.Clockwise_Positive;
|
||||
conveyorMotor.getConfigurator().apply(configs2);
|
||||
var leftFlywheelMotorOutputConfigs = new MotorOutputConfigs();
|
||||
leftFlywheelMotorOutputConfigs.Inverted = InvertedValue.Clockwise_Positive;
|
||||
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(Constants.Shooter.backboardPositionPID.kP, Constants.Shooter.backboardPositionPID.kI, Constants.Shooter.backboardPositionPID.kD,
|
||||
new Constraints(10000,100000));
|
||||
|
||||
backboardPID = new ProfiledPIDController(
|
||||
Constants.Shooter.backboardPositionPID.kP,
|
||||
Constants.Shooter.backboardPositionPID.kI,
|
||||
Constants.Shooter.backboardPositionPID.kD,
|
||||
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);
|
||||
flywheelMotorRight.getConfigurator().apply(configs);
|
||||
leftFlywheel.getConfigurator().refresh(configs);
|
||||
rightFlywheel.getConfigurator().apply(configs);
|
||||
}
|
||||
|
||||
|
||||
// 设置飞轮的速度
|
||||
public void setFlywheelSpeedByRPS(double targetSpeed) {
|
||||
flywheelMotorLeft.setControl(velocityRequest.withVelocity(targetSpeed)); //RPS 转每秒
|
||||
setRightFollowLeft();
|
||||
// System.out.println("set flywheel "+targetSpeed);
|
||||
public void setFlywheelVelocity(double targetVelocityRps) {
|
||||
flywheelTargetVelocity = targetVelocityRps;
|
||||
leftFlywheel.setControl(leftFlywheelVelocityRequest.withVelocity(flywheelTargetVelocity));
|
||||
setRightFlywheelFollowLeft();
|
||||
}
|
||||
|
||||
// 设置传动的速度
|
||||
public void setConveyorSpeedByRPS(double speed) {
|
||||
conveyorMotor.setControl(velocityRequest.withVelocity(speed)); // 设置传动电机速度
|
||||
}
|
||||
public void setConveyorSpeedOpen(double speed){
|
||||
conveyorMotor.set(speed);
|
||||
public void stopFlywheel() {
|
||||
flywheelTargetVelocity = 0.0;
|
||||
leftFlywheel.setControl(flywheelNeutralRequest);
|
||||
rightFlywheel.setControl(flywheelNeutralRequest);
|
||||
}
|
||||
|
||||
//调试用
|
||||
public void setBackboardSpeedByRPS(double speed){
|
||||
if (speed > 0 && speed < 10) {
|
||||
speed = 10;
|
||||
} else if (speed < 0 && speed > -10) {
|
||||
speed = -10;
|
||||
public boolean atVelocity() {
|
||||
if (flywheelTargetVelocity <= 10.0) {
|
||||
return false;
|
||||
}
|
||||
double currentvelocity = getAverageFlywheelVelocity();
|
||||
if(currentvelocity>flywheelTargetVelocity*0.95){
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
backboardMotor.setControl(velocityRequest.withVelocity(speed));
|
||||
}
|
||||
|
||||
// 设置背板的目标位置，通过PID控制器来调整 范围0-3000
|
||||
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));
|
||||
}
|
||||
|
||||
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控制器来计算背板的运动速度
|
||||
double pidOutput = backboardPID.calculate(getBackboardPosition());
|
||||
pidOutput = Math.max(-Constants.Shooter.backboardSpeedMax, Math.min(Constants.Shooter.backboardSpeedMax, pidOutput)); // 限制输出范围
|
||||
public void outputBackboard() {
|
||||
// 背板仍使用旧外部编码器和 ProfiledPID；Superstructure 只选择目标位置。
|
||||
double backboardPosition = getBackboardPosition();
|
||||
double rawPidOutput = backboardPID.calculate(backboardPosition);
|
||||
double pidOutput = MathUtil.clamp(
|
||||
rawPidOutput,
|
||||
-Constants.Shooter.backboardSpeedMax,
|
||||
Constants.Shooter.backboardSpeedMax
|
||||
);
|
||||
backboardRawPidOutputVolts = rawPidOutput;
|
||||
backboardClampedPidOutputVolts = pidOutput;
|
||||
backboardOutputSaturated = Math.abs(rawPidOutput - pidOutput) > 1e-9;
|
||||
|
||||
if (Math.abs(pidOutput) < 0.8) {
|
||||
pidOutput = 0.0;
|
||||
}//防止抖动
|
||||
// // 设置背板电机的速度，根据PID输出进行调整
|
||||
shooterNetworkTable.getEntry("backboardPIDOutput").setDouble(pidOutput);
|
||||
setBackboardSpeedByRPS(pidOutput);
|
||||
}
|
||||
// 停止所有电机
|
||||
public void stopMotors() {
|
||||
flywheelMotorLeft.set(0);
|
||||
flywheelMotorRight.set(0);
|
||||
conveyorMotor.set(0);
|
||||
backboardMotor.set(0);
|
||||
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);
|
||||
}
|
||||
|
||||
private double calculateBackboardGravityFeedforward(double backboardPosition) {
|
||||
double clampedPosition = MathUtil.clamp(
|
||||
backboardPosition,
|
||||
Constants.Shooter.backboardGravityFeedforwardMinPosition,
|
||||
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 void resetbackboardencoder(){
|
||||
|
||||
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);
|
||||
}
|
||||
public double getConveyorStatorCurrent(){
|
||||
double statorCurrent = conveyorMotor.getStatorCurrent().getValueAsDouble();
|
||||
return statorCurrent;
|
||||
|
||||
/**
|
||||
* 背板回零命令：闭环回到 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());
|
||||
shooterNetworkTable.getEntry("flywheelSpeed").setDouble(flywheelMotorLeft.getVelocity().getValueAsDouble());
|
||||
shooterNetworkTable.getEntry("flywheelPIDReference").setDouble(flywheelMotorLeft.getClosedLoopReference().getValueAsDouble());
|
||||
shooterNetworkTable.getEntry("conveyorSpeed").setDouble(conveyorMotor.getVelocity().getValueAsDouble());
|
||||
shooterNetworkTable.getEntry("conveyorPIDReference").setDouble(conveyorMotor.getClosedLoopReference().getValueAsDouble());
|
||||
shooterNetworkTable.getEntry("backboardCurrentRate").setDouble(getBackboardPosition());
|
||||
shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardPID.getSetpoint().position);
|
||||
double backboardPosition = getBackboardPosition();
|
||||
double backboardGoalPosition = backboardPID.getGoal().position;
|
||||
double backboardSetpointPosition = backboardPID.getSetpoint().position;
|
||||
flywheelOutputVolts = leftFlywheel.getMotorVoltage().getValueAsDouble();
|
||||
shooterNetworkTable.getEntry("flywheelVelocity").setDouble(getAverageFlywheelVelocity());
|
||||
shooterNetworkTable.getEntry("leftFlywheelVelocity").setDouble(getLeftFlywheelVelocity());
|
||||
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(getDistanceToHub());
|
||||
shooterNetworkTable.getEntry("distancetopassball").setDouble(getDistanceToPassball());
|
||||
shooterNetworkTable.getEntry("backboardGoalPosition").setDouble(backboardGoalPosition);
|
||||
shooterNetworkTable.getEntry("backboardSetpointPosition").setDouble(backboardSetpointPosition);
|
||||
shooterNetworkTable.getEntry("backboardSetpointVelocity").setDouble(backboardPID.getSetpoint().velocity);
|
||||
shooterNetworkTable.getEntry("backboardGoalError").setDouble(backboardGoalPosition - backboardPosition);
|
||||
shooterNetworkTable.getEntry("backboardSetpointError").setDouble(backboardSetpointPosition - backboardPosition);
|
||||
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());
|
||||
}
|
||||
|
||||
public double getDistanceToHub(){
|
||||
//获得车子距离Hub的距离
|
||||
return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToHub").getDouble(0.0);
|
||||
}
|
||||
public double getDistanceToPassball(){
|
||||
return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToPassball").getDouble(0.0);
|
||||
}
|
||||
public void stopAll() {
|
||||
setFlywheelSpeedByRPS(0);
|
||||
setConveyorSpeedByRPS(0);
|
||||
backboardMotor.set(0);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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);;
|
||||
table.getDoubleTopic(motorName+"RunningSpeedRPS").publish().set(0);;
|
||||
if (voltageRunEnabled) {
|
||||
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.runningSpeedRPS = table.getDoubleTopic(motorName+"RunningSpeedRPS").subscribe(0);
|
||||
this.enableRuningSub = voltageRunEnabled
|
||||
? 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()) {;
|
||||
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));
|
||||
if (voltageRunEnabled && enableRuningSub.get()) {;
|
||||
double volts = MathUtil.clamp(runningVolts.get(), -12.0, 12.0);
|
||||
motor.setControl(voltageRequest.withOutput(volts));
|
||||
wasRunning = true;
|
||||
// System.out.println("running motor with volts: "+volts+" on motor: "+motorName);
|
||||
}else if (wasRunning){
|
||||
motor.setControl(voltageRequest.withOutput(0));
|
||||
wasRunning = false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -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));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user