admin6494/FRC6494_2026Regional-offseason
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: admin6494/FRC6494_2026Regional-offseason:main
Branches Tags
admin6494/FRC6494_2026Regional-offseason:7.4 admin6494/FRC6494_2026Regional-offseason:季后赛季后赛季后赛 admin6494/FRC6494_2026Regional-offseason:手搓 admin6494/FRC6494_2026Regional-offseason:1 admin6494/FRC6494_2026Regional-offseason:main
...
compare: admin6494/FRC6494_2026Regional-offseason:1c39a3bc4201322a780185c908e5e5f334e82106
Branches Tags
admin6494/FRC6494_2026Regional-offseason:7.4 admin6494/FRC6494_2026Regional-offseason:季后赛季后赛季后赛 admin6494/FRC6494_2026Regional-offseason:手搓 admin6494/FRC6494_2026Regional-offseason:1 admin6494/FRC6494_2026Regional-offseason:main
68 Commits
main ... 1c39a3bc42
Author SHA1 Message Date
OnDYCLaptop 1c39a3bc42 7.7 2026-07-07 23:25:10 +08:00
OnDYCLaptop 2b14e3f99e 7.6终终 2026-07-06 23:48:00 +08:00
OnDYCLaptop 0053c29893 7.6终 2026-07-06 23:02:19 +08:00
OnDYCLaptop 179913194c tunerconstants修改 2026-07-06 22:52:36 +08:00
OnDYCLaptop 5947fa27d3 shoot逻辑修复 新老状态机合并 2026-07-05 22:52:42 +08:00
OnDYCLaptop a1fc79659e subsystem修复 2026-07-04 22:17:13 +08:00
OnDYCLaptop 0c97452a13 Merge branch '7.4' of https://github.com/HarryWang163/FRC6494_2026Regional into 7.4 2026-07-04 21:08:04 +08:00
OnDYCLaptop a78a71e4ed 小改 2026-07-04 20:58:14 +08:00
dingdoorfiah-ui fad52c40c0 Merge pull request #2 from vic233333/claude/frc-postseason-refactor-wepjot 
Claude/frc postseason refactor wepjot
 2026-07-04 20:56:05 +08:00
Claude 9da20a108c 修复审查发现的自动赛与状态机问题 
- rotater 开机默认按收拢姿态归零（assumeZeroedOnBoot，可关），
  否则自动赛无人能按归零键，收球和射击被未归零保护永久拦下
- 预装球开机默认持球（assumePreloadedAtBoot，可关），
  否则自动赛第一发被 hasNote 门控拦死
- 新增操作员 X 键运行时翻转持球状态，纠正无传感器推断的失真
- 进入禁用前结算持球推断：SHOOTING 中被禁用视为球已射出，
  INDEXING 中被禁用视为球已入传送带
- 未归零被拦下的 intake 转射击不再误置持球
- CLEANUP 结束不再强写 wantedState=IDLE，清理窗口内重按射击键不会被吞
- 机构请求按键加 teleop 门控，防止自动赛误触取消 PathPlanner 自动程序

Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01YTEBmjtkz1ouCmn5yytFti
 2026-07-04 12:30:02 +00:00
Claude 6264ef0762 季后赛重构：补完 Superstructure 状态机与射击门控 
- Superstructure 增加 INDEXING/HOLDING_NOTE 状态，实现完整状态流转：
  DEPLOYING_INTAKE -> INTAKING -> INDEXING -> HOLDING_NOTE ->
  SPINNING_UP -> AIMING -> READY_TO_SHOOT -> SHOOTING -> CLEANUP
- canShoot 门控补全：atSpeed && isAimed && rotaterAtGoal && hasNote && 底盘近静止
  （hasNote/静止门控均可在 Constants 中旁路）
- 无传感器持球跟踪：INDEXING 完成置位、射击/EJECT 后清空，
  未归零被拦下的 intake 不会误置持球
- DriveControls 在既有 hub 方位角计算处同步 setTargetHeading，
  使 isAimed 门控与 AutoAimming 实际瞄准同源；自动阶段经 pushDistanceData 保持更新
- 移除 Superstructure 基于 limelight tx 的瞄准更新（符号错误且无目标时滞留旧值）
- 背板回零命令下沉到 ShooterSubsystem，OperatorControls 不再直接操作电机
- ShooterSubsystem 清理旧别名与闭环遗留接口，背板独立控制请求
- 射表改为 InterpolatingDoubleTreeMap（Constants.Shooter.flywheelRpsByDistance）
- IntakeRotater：未归零时 atGoal 一律 false，新增 blockedByNotZeroed 遥测；
  atGoal 容差改为非零占位值
- 修复 gradlew 缺少可执行权限

Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01YTEBmjtkz1ouCmn5yytFti
 2026-07-04 12:19:50 +00:00
OnDYCLaptop 3c5d17b527 gpt的超级状态机 2026-07-04 15:02:09 +08:00
OnDYCLaptop 132b231c36 新底盘 2026-07-03 21:43:14 +08:00
OnDYCLaptop 99a92af429 计时器 2026-06-16 12:12:24 +08:00
OnDYCLaptop af1095cf50 不知道是个啥 2026-05-05 16:20:56 +08:00
OnDYCLaptop 055642a37e 完整正赛 2026-03-16 16:10:50 +08:00
OnDYCLaptop 6196da8a23 Have Climber 2026-03-15 08:42:33 +08:00
OnDYCLaptop be6342b597 优化自动程序 2026-03-12 15:55:24 +08:00
OnDYCLaptop f66ea4a188 修复自动突然终止 电机速度参数仍存在的bug 2026-03-11 01:13:40 +08:00
OnDYCLaptop be328661c7 修改版 2026-03-11 00:53:35 +08:00
OnDYCLaptop 1d0454dc58 Merge branch '1' of https://github.com/HarryWang163/FRC6494_2026Regional into 1 2026-03-11 00:21:29 +08:00
OnDYCLaptop 95fabf94ff 1 2026-03-11 00:18:47 +08:00
OnDYCLaptop d5db9c75c4 屎1.1 2026-03-10 21:20:47 +08:00
OnDYCLaptop 1f397e6b6e 屎 2026-03-10 21:01:01 +08:00
OnDYCLaptop 50c9af952c 若自动爬升 则手动阶段自动让钩子下降（待测试 2026-03-10 16:30:07 +08:00
OnDYCLaptop 8b70832978 修正可调背板 使默认位置为0（待测试） 2026-03-09 13:08:13 +08:00
OnDYCLaptop fe72a45c4b 蓝方path 2026-03-09 13:04:13 +08:00
OnDYCLaptop 9e00148138 添加自动射球时的shake 2026-03-09 12:27:28 +08:00
OnDYCLaptop 69e86905ba 修复报错 2026-03-09 12:18:18 +08:00
OnDYCLaptop c8b7359b99 自动瞄准+射球已封装 增加conveyor独立控制 2026-03-09 12:17:42 +08:00
OnDYCLaptop 9a426a8f8a 自动2.1 2026-03-09 09:51:26 +08:00
OnDYCLaptop 8b2df6ce7c 自动2.0 2026-03-09 09:47:22 +08:00
OnDYCLaptop 196b3ce77f namedcontrol已调用 2026-03-08 11:16:18 +08:00
OnDYCLaptop 3323fb511e 手动修复已知问题 自动第一版 2026-03-08 10:48:45 +08:00
OnDYCLaptop f68847d3e7 修复已知问题 2026-03-07 22:24:01 +08:00
OnDYCLaptop 056362fe10 climb修改 2026-03-07 12:43:00 +08:00
OnDYCLaptop 093a0b93c9 Merge branch '1' of https://github.com/HarryWang163/FRC6494_2026Regional into 1 2026-03-07 00:11:03 +08:00
OnDYCLaptop 74bf1b992d 2 2026-03-07 00:09:57 +08:00
Alyssum-chenyiyi 31b00a4cf3 Merge branch '1' of https://github.com/HarryWang163/FRC6494_2026Regional into 1 2026-03-06 19:41:28 +08:00
Alyssum-chenyiyi 6fe9613b67 climber limelight 0306 2026-03-06 19:40:10 +08:00
OnDYCLaptop de76680191 Merge branch '1' of https://github.com/HarryWang163/FRC6494_2026Regional into 1 2026-03-06 17:31:52 +08:00
OnDYCLaptop 83c06bd0e6 修复底盘自动瞄准trench和bmp 2026-03-06 17:29:43 +08:00
Alyssum-chenyiyi f8fc037959 climber limelight 2026-03-06 17:27:42 +08:00
OnDYCLaptop 0536a3bcd4 修复已知问题 2026-03-06 15:03:26 +08:00
OnDYCLaptop ce8b17e377 conveyor修复 2026-03-06 14:32:16 +08:00
OnDYCLaptop 7cee19994f 1 2026-03-05 20:44:11 +08:00
OnDYCLaptop f31b614cc3 Merge branch '1' of https://github.com/HarryWang163/FRC6494_2026Regional into 1 2026-03-05 20:30:33 +08:00
OnDYCLaptop 921a11b31a conveyor有bug版 2026-03-05 20:28:43 +08:00
OnDYCLaptop 0da9ee5f36 climber 2026-03-05 19:34:24 +08:00
OnDYCLaptop f7eb1d7d26 底盘修正ing 2026-03-05 19:29:06 +08:00
Alyssum-chenyiyi a8acbce9dc climber 2026-03-05 19:21:24 +08:00
Alyssum-chenyiyi b236f43f4d climb auto 2026-03-05 18:57:57 +08:00
OnDYCLaptop ebcf6ff3c3 for limelight 2026-03-05 16:48:24 +08:00
OnDYCLaptop 118fdc4086 细化传球 待打表 2026-03-05 15:41:05 +08:00
OnDYCLaptop 6d996a35cd 传入了所有场地坐标参数 上层操作手可控制底盘瞄准 传球待测试 2026-03-05 12:39:53 +08:00
OnDYCLaptop 042d483b16 电压闭环的shooter 双操均可触发autoaim intaker已验证 2026-03-05 12:18:12 +08:00
OnDYCLaptop aa323814de 开环intaker 红蓝Drivercontrol（待传蓝方参数 2026-03-03 21:12:34 +08:00
Alyssum-chenyiyi c032236287 0303 2026-03-03 19:38:44 +08:00
Alyssum-chenyiyi 710dfe7a5a Merge branch '1' of https://github.com/HarryWang163/FRC6494_2026Regional into 1 2026-03-03 19:37:43 +08:00
Alyssum-chenyiyi fe6c097947 0303 
0
 2026-03-03 19:37:30 +08:00
OnDYCLaptop a36e226f0d 判断联盟颜色 2026-03-03 17:54:50 +08:00
OnDYCLaptop cae3810ef5 autocommand 2026-03-03 14:40:21 +08:00
OnDYCLaptop 7606402980 添加intaker重力前馈（未启用） 2026-03-03 14:18:44 +08:00
OnDYCLaptop fd4fb39a50 手动完成版（待测试） 2026-03-03 12:37:50 +08:00
OnDYCLaptop 85c7cb9fb3 Merge branch '1' of https://github.com/HarryWang163/FRC6494_2026Regional into 1 2026-03-02 23:07:46 +08:00
OnDYCLaptop 9538052697 0302 2026-03-02 23:06:14 +08:00
Alyssum-chenyiyi 711eec2a41 阿巴阿巴 改了一下climber到3.2的版本 
0302
 2026-03-02 19:38:42 +08:00
OnDYCLaptop 5f3452e091 双手柄版本 2026-03-02 14:35:18 +08:00
68 changed files with 6797 additions and 1217 deletions
Expand all files Collapse all files
build.gradle
+1 -1
View File
@@ -33,7 +33,7 @@ deploy {
frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) {
files = project.fileTree('src/main/deploy')
directory = '/home/lvuser/deploy'
deleteOldFiles = false // Change to true to delete files on roboRIO that no
deleteOldFiles = true // Change to true to delete files on roboRIO that no
// longer exist in deploy directory of this project
}
}
elastic-layout-debug.json
+2361
View File
File diff suppressed because it is too large Load Diff
elastic-layout.json
+116 -29
View File
@@ -91,6 +91,93 @@
"period": 0.06,
"sort_options": false
}
},
{
"title": "HubActive",
"x": 0.0,
"y": 0.0,
"width": 256.0,
"height": 128.0,
"type": "Boolean Box",
"properties": {
"topic": "/GameData2026/HubActive",
"period": 0.06,
"data_type": "boolean",
"true_color": 4283215696,
"false_color": 4294198070,
"true_icon": "None",
"false_icon": "None"
}
},
{
"title": "HubStatus",
"x": 256.0,
"y": 0.0,
"width": 256.0,
"height": 128.0,
"type": "Text Display",
"properties": {
"topic": "/GameData2026/HubStatus",
"period": 0.06,
"data_type": "string",
"show_submit_button": false
}
},
{
"title": "SecondsUntilHubOpens",
"x": 0.0,
"y": 128.0,
"width": 256.0,
"height": 128.0,
"type": "Text Display",
"properties": {
"topic": "/GameData2026/SecondsUntilHubOpens",
"period": 0.06,
"data_type": "double",
"show_submit_button": false
}
},
{
"title": "Shift",
"x": 256.0,
"y": 128.0,
"width": 256.0,
"height": 128.0,
"type": "Text Display",
"properties": {
"topic": "/GameData2026/Shift",
"period": 0.06,
"data_type": "string",
"show_submit_button": false
}
},
{
"title": "FirstInactiveAlliance",
"x": 0.0,
"y": 256.0,
"width": 256.0,
"height": 128.0,
"type": "Text Display",
"properties": {
"topic": "/GameData2026/FirstInactiveAlliance",
"period": 0.06,
"data_type": "string",
"show_submit_button": false
}
},
{
"title": "MatchTime",
"x": 256.0,
"y": 256.0,
"width": 256.0,
"height": 128.0,
"type": "Text Display",
"properties": {
"topic": "/GameData2026/MatchTime",
"period": 0.06,
"data_type": "double",
"show_submit_button": false
}
}
]
}
@@ -681,14 +768,14 @@
}
},
{
"title": "flywheelRunningSpeedRPS",
"title": "flywheelRunningVolts",
"x": 640.0,
"y": 128.0,
"width": 256.0,
"height": 128.0,
"type": "Text Display",
"properties": {
"topic": "/Tuning/flywheel/flywheelRunningSpeedRPS",
"topic": "/Tuning/flywheel/flywheelRunningVolts",
"period": 0.06,
"data_type": "double",
"show_submit_button": true
@@ -758,27 +845,27 @@
"layouts": [],
"containers": [
{
"title": "limelight-intaker",
"title": "limelight-front",
"x": 0.0,
"y": 0.0,
"width": 256.0,
"height": 256.0,
"type": "Camera Stream",
"properties": {
"topic": "/CameraPublisher/limelight-intaker",
"topic": "/CameraPublisher/limelight-front",
"period": 0.06,
"rotation_turns": 0
}
},
{
"title": "limelight-shooter",
"title": "limelight-left",
"x": 768.0,
"y": 512.0,
"width": 640.0,
"height": 384.0,
"type": "Camera Stream",
"properties": {
"topic": "/CameraPublisher/limelight-shooter",
"topic": "/CameraPublisher/limelight-left",
"period": 0.06,
"rotation_turns": 0
}
@@ -822,7 +909,7 @@
"height": 128.0,
"type": "Boolean Box",
"properties": {
"topic": "/Positioning/limelight-intaker/available",
"topic": "/Positioning/limelight-front/available",
"period": 0.06,
"data_type": "boolean",
"true_color": 4283215696,
@@ -839,7 +926,7 @@
"height": 128.0,
"type": "Boolean Box",
"properties": {
"topic": "/Positioning/limelight-shooter/available",
"topic": "/Positioning/limelight-left/available",
"period": 0.06,
"data_type": "boolean",
"true_color": 4283215696,
@@ -947,7 +1034,7 @@
"height": 128.0,
"type": "Text Display",
"properties": {
"topic": "/Positioning/limelight-intaker/Distance",
"topic": "/Positioning/limelight-front/Distance",
"period": 0.06,
"data_type": "double",
"show_submit_button": true
@@ -977,7 +1064,7 @@
"height": 128.0,
"type": "Text Display",
"properties": {
"topic": "/Positioning/limelight-shooter/Distance",
"topic": "/Positioning/limelight-left/Distance",
"period": 0.06,
"data_type": "double",
"show_submit_button": true
@@ -1051,7 +1138,7 @@
"height": 128.0,
"type": "Text Display",
"properties": {
"topic": "/Positioning/limelight-intaker/X",
"topic": "/Positioning/limelight-front/X",
"period": 0.06,
"data_type": "double",
"show_submit_button": true
@@ -1065,7 +1152,7 @@
"height": 128.0,
"type": "Text Display",
"properties": {
"topic": "/Positioning/limelight-intaker/Y",
"topic": "/Positioning/limelight-front/Y",
"period": 0.06,
"data_type": "double",
"show_submit_button": true
@@ -1079,7 +1166,7 @@
"height": 128.0,
"type": "Text Display",
"properties": {
"topic": "/Positioning/limelight-shooter/X",
"topic": "/Positioning/limelight-left/X",
"period": 0.06,
"data_type": "double",
"show_submit_button": true
@@ -1093,7 +1180,7 @@
"height": 128.0,
"type": "Text Display",
"properties": {
"topic": "/Positioning/limelight-shooter/Y",
"topic": "/Positioning/limelight-left/Y",
"period": 0.06,
"data_type": "double",
"show_submit_button": true
@@ -1108,14 +1195,14 @@
"layouts": [],
"containers": [
{
"title": "limelight-shooter",
"title": "limelight-left",
"x": 1152.0,
"y": 0.0,
"width": 384.0,
"height": 256.0,
"type": "Camera Stream",
"properties": {
"topic": "/CameraPublisher/limelight-shooter",
"topic": "/CameraPublisher/limelight-left",
"period": 0.06,
"rotation_turns": 0
}
@@ -1137,14 +1224,14 @@
}
},
{
"title": "conveyorSpeed",
"title": "conveyorVelocity",
"x": 384.0,
"y": 256.0,
"width": 128.0,
"height": 128.0,
"type": "Text Display",
"properties": {
"topic": "/Shooter/conveyorSpeed",
"topic": "/Shooter/conveyorVelocity",
"period": 0.06,
"data_type": "double",
"show_submit_button": true
@@ -1184,14 +1271,14 @@
}
},
{
"title": "flywheelSpeed",
"title": "flywheelVelocity",
"x": 640.0,
"y": 0.0,
"width": 256.0,
"height": 256.0,
"type": "Graph",
"properties": {
"topic": "/Shooter/flywheelSpeed",
"topic": "/Shooter/flywheelVelocity",
"period": 0.033,
"data_type": "double",
"time_displayed": 5.0,
@@ -1235,14 +1322,14 @@
}
},
{
"title": "conveyorSpeed",
"title": "conveyorVelocity",
"x": 384.0,
"y": 0.0,
"width": 256.0,
"height": 256.0,
"type": "Graph",
"properties": {
"topic": "/Shooter/conveyorSpeed",
"topic": "/Shooter/conveyorVelocity",
"period": 0.033,
"data_type": "double",
"time_displayed": 5.0,
@@ -1251,14 +1338,14 @@
}
},
{
"title": "flywheelSpeed",
"title": "flywheelVelocity",
"x": 640.0,
"y": 256.0,
"width": 128.0,
"height": 128.0,
"type": "Text Display",
"properties": {
"topic": "/Shooter/flywheelSpeed",
"topic": "/Shooter/flywheelVelocity",
"period": 0.033,
"data_type": "double",
"show_submit_button": true
@@ -1293,14 +1380,14 @@
}
},
{
"title": "flywheelTargetSpeed",
"title": "flywheelTargetVoltage",
"x": 768.0,
"y": 256.0,
"width": 128.0,
"height": 128.0,
"type": "Text Display",
"properties": {
"topic": "/ShooterControl/flywheelTargetSpeed",
"topic": "/Shooter/flywheelTargetVoltage",
"period": 0.06,
"data_type": "double",
"show_submit_button": true
@@ -1337,14 +1424,14 @@
}
},
{
"title": "flywheelTargetSpeed",
"title": "flywheelTargetVoltage",
"x": 640.0,
"y": 384.0,
"width": 256.0,
"height": 256.0,
"type": "Graph",
"properties": {
"topic": "/ShooterControl/flywheelTargetSpeed",
"topic": "/Shooter/flywheelTargetVoltage",
"period": 0.033,
"data_type": "double",
"time_displayed": 5.0,
@@ -1492,4 +1579,4 @@
}
}
]
}
}
gradlew
Vendored Regular → Executable
View File
simgui-ds.json
+92
View File
@@ -0,0 +1,92 @@
{
"keyboardJoysticks": [
{
"axisConfig": [
{
"decKey": 65,
"incKey": 68
},
{
"decKey": 87,
"incKey": 83
},
{
"decKey": 69,
"decayRate": 0.0,
"incKey": 82,
"keyRate": 0.009999999776482582
}
],
"axisCount": 3,
"buttonCount": 4,
"buttonKeys": [
90,
88,
67,
86
],
"povConfig": [
{
"key0": 328,
"key135": 323,
"key180": 322,
"key225": 321,
"key270": 324,
"key315": 327,
"key45": 329,
"key90": 326
}
],
"povCount": 1
},
{
"axisConfig": [
{
"decKey": 74,
"incKey": 76
},
{
"decKey": 73,
"incKey": 75
}
],
"axisCount": 2,
"buttonCount": 4,
"buttonKeys": [
77,
44,
46,
47
],
"povCount": 0
},
{
"axisConfig": [
{
"decKey": 263,
"incKey": 262
},
{
"decKey": 265,
"incKey": 264
}
],
"axisCount": 2,
"buttonCount": 6,
"buttonKeys": [
260,
268,
266,
261,
269,
267
],
"povCount": 0
},
{
"axisCount": 0,
"buttonCount": 0,
"povCount": 0
}
]
}
src/main/deploy/pathplanner/autos/8 Balls Middle Start Blue.auto
+31
View File
@@ -0,0 +1,31 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Back To Shoot Blue"
}
},
{
"type": "named",
"data": {
"name": "shoot_hub"
}
},
{
"type": "named",
"data": {
"name": "shoot_hub"
}
}
]
}
},
"resetOdom": true,
"folder": "Blue Middle",
"choreoAuto": false
}
src/main/deploy/pathplanner/autos/Line.auto → src/main/deploy/pathplanner/autos/8 Balls Middle Start Red.auto
+9 -3
View File
@@ -7,13 +7,19 @@
{
"type": "path",
"data": {
"pathName": "Line"
"pathName": "Back To Shoot Red"
}
},
{
"type": "named",
"data": {
"name": "shoot_hub"
}
}
]
}
},
"resetOdom": false,
"folder": null,
"resetOdom": true,
"folder": "Red Middle",
"choreoAuto": false
}
src/main/deploy/pathplanner/autos/Copy of Shoot and Depot Blue.auto
+152
View File
@@ -0,0 +1,152 @@
{
"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_hub"
}
},
{
"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": "shoot_hub"
}
},
{
"type": "named",
"data": {
"name": "Shake"
}
},
{
"type": "named",
"data": {
"name": "intaker_up"
}
}
]
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "shoot_hub"
}
},
{
"type": "named",
"data": {
"name": "Shake"
}
},
{
"type": "named",
"data": {
"name": "intaker_up"
}
}
]
}
}
]
}
},
"resetOdom": true,
"folder": "Red Middle",
"choreoAuto": false
}
src/main/deploy/pathplanner/autos/Depot Auto Blue.auto
+88
View File
@@ -0,0 +1,88 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "intaker_down"
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "To Depot Blue"
}
}
]
}
},
{
"type": "named",
"data": {
"name": "start_intake_timelimit"
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "start_intake_timelimit"
}
},
{
"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"
}
}
]
}
},
{
"type": "named",
"data": {
"name": "shoot_hub"
}
}
]
}
},
"resetOdom": true,
"folder": "Blue Middle",
"choreoAuto": false
}
src/main/deploy/pathplanner/autos/Depot Auto Red.auto
+88
View File
@@ -0,0 +1,88 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "intaker_down"
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "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": "wait",
"data": {
"waitTime": 2.0
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "intaker_up"
}
},
{
"type": "path",
"data": {
"pathName": "Depot to Shoot Red"
}
}
]
}
},
{
"type": "named",
"data": {
"name": "shoot_hub"
}
}
]
}
},
"resetOdom": true,
"folder": "Red Middle",
"choreoAuto": false
}
src/main/deploy/pathplanner/autos/New Auto.auto
+21 -2
View File
@@ -3,10 +3,29 @@
"command": {
"type": "sequential",
"data": {
"commands": []
"commands": [
{
"type": "path",
"data": {
"pathName": "New Path"
}
},
{
"type": "named",
"data": {
"name": "AutoAim"
}
},
{
"type": "named",
"data": {
"name": "shoot_hub"
}
}
]
}
},
"resetOdom": true,
"folder": null,
"folder": "Blue Middle",
"choreoAuto": false
}
src/main/deploy/pathplanner/autos/RotateAndLine.auto
-19
View File
@@ -1,19 +0,0 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "RotateAndLine"
}
}
]
}
},
"resetOdom": false,
"folder": null,
"choreoAuto": false
}
src/main/deploy/pathplanner/autos/Shoot and Climb Blue.auto
+67
View File
@@ -0,0 +1,67 @@
{
"version": "2025.0",
"command": {
"type": "sequential",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Back To Shoot Blue"
}
},
{
"type": "named",
"data": {
"name": "shoot_hub"
}
},
{
"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
}
src/main/deploy/pathplanner/autos/Shoot and Climb Red.auto
+80
View File
@@ -0,0 +1,80 @@
{
"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_hub"
}
},
{
"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
}
src/main/deploy/pathplanner/autos/Shoot and Depot Blue.auto
+158
View File
@@ -0,0 +1,158 @@
{
"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_hub"
}
},
{
"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": "shoot_hub"
}
},
{
"type": "named",
"data": {
"name": "Shake"
}
},
{
"type": "named",
"data": {
"name": "intaker_up"
}
}
]
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "shoot_hub"
}
},
{
"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
}
src/main/deploy/pathplanner/autos/Shoot and Depot Red.auto
+151
View File
@@ -0,0 +1,151 @@
{
"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_hub"
}
},
{
"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": "shoot_hub"
}
},
{
"type": "named",
"data": {
"name": "Shake"
}
},
{
"type": "named",
"data": {
"name": "intaker_up"
}
}
]
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "named",
"data": {
"name": "shoot_hub"
}
},
{
"type": "named",
"data": {
"name": "Shake"
}
},
{
"type": "named",
"data": {
"name": "intaker_up"
}
}
]
}
}
]
}
},
"resetOdom": true,
"folder": "Red Middle",
"choreoAuto": false
}
src/main/deploy/pathplanner/autos/Shoot and Depot Without Climb Red.auto
+120
View File
@@ -0,0 +1,120 @@
{
"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_hub"
}
},
{
"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": "shoot_hub"
}
},
{
"type": "named",
"data": {
"name": "Shake"
}
},
{
"type": "named",
"data": {
"name": "intaker_up"
}
}
]
}
}
]
}
},
"resetOdom": true,
"folder": "Red Middle",
"choreoAuto": false
}
src/main/deploy/pathplanner/autos/Shoot and Depot and Climb Blue.auto
+151
View File
@@ -0,0 +1,151 @@
{
"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": "named",
"data": {
"name": "climber_init"
}
}
]
}
},
{
"type": "named",
"data": {
"name": "shoot_hub"
}
},
{
"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": "shoot_hub"
}
},
{
"type": "named",
"data": {
"name": "Shake"
}
},
{
"type": "named",
"data": {
"name": "intaker_up"
}
}
]
}
},
{
"type": "parallel",
"data": {
"commands": [
{
"type": "path",
"data": {
"pathName": "Depot To Climb Blue"
}
},
{
"type": "named",
"data": {
"name": "climb_up"
}
}
]
}
},
{
"type": "path",
"data": {
"pathName": "Climb Step2 Blue"
}
},
{
"type": "path",
"data": {
"pathName": "Climb Step3 Blue"
}
}
]
}
},
"resetOdom": true,
"folder": "Blue Middle",
"choreoAuto": false
}
src/main/deploy/pathplanner/autos/Shoot and Depot and Climb Red.auto
+157
View File
@@ -0,0 +1,157 @@
{
"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_hub"
}
},
{
"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": "shoot_hub"
}
},
{
"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
}
src/main/deploy/pathplanner/autos/LineGoAndBack.auto → src/main/deploy/pathplanner/autos/Test of individual command Blue.auto
+6 -12
View File
@@ -5,33 +5,27 @@
"data": {
"commands": [
{
"type": "path",
"type": "named",
"data": {
"pathName": "Line"
"name": "start_intake"
}
},
{
"type": "wait",
"data": {
"waitTime": 1.0
"waitTime": 2.0
}
},
{
"type": "path",
"type": "named",
"data": {
"pathName": "LineBack"
}
},
{
"type": "wait",
"data": {
"waitTime": 1.0
"name": "stop_intake"
}
}
]
}
},
"resetOdom": false,
"folder": null,
"folder": "Blue Middle",
"choreoAuto": false
}
src/main/deploy/pathplanner/autos/Rotate.auto → src/main/deploy/pathplanner/autos/Test of individual command.auto
+9 -3
View File
@@ -5,15 +5,21 @@
"data": {
"commands": [
{
"type": "path",
"type": "named",
"data": {
"pathName": "Rotate"
"name": "start_intake_and_conveyor"
}
},
{
"type": "wait",
"data": {
"waitTime": 1.0
}
}
]
}
},
"resetOdom": false,
"folder": null,
"folder": "Red Middle",
"choreoAuto": false
}
src/main/deploy/pathplanner/paths/LineBack.path → src/main/deploy/pathplanner/paths/Back To Shoot Blue.path
+13 -13
View File
@@ -3,29 +3,29 @@
"waypoints": [
{
"anchor": {
"x": 14.314,
"y": 6.668017118402283
"x": 3.53,
"y": 4.0
},
"prevControl": null,
"nextControl": {
"x": 14.299415121255347,
"y": 7.128787446504993
"x": 3.9033070768298836,
"y": 3.9994852751840226
},
"isLocked": false,
"linkedName": "Point1"
"linkedName": null
},
{
"anchor": {
"x": 14.314,
"y": 4.15
"x": 3.04,
"y": 4.0
},
"prevControl": {
"x": 14.299415121255347,
"y": 4.597831669044222
"x": 2.783639997726092,
"y": 4.003292975326709
},
"nextControl": null,
"isLocked": false,
"linkedName": "StartingPointRed2"
"linkedName": null
}
],
"rotationTargets": [],
@@ -42,13 +42,13 @@
},
"goalEndState": {
"velocity": 0,
"rotation": 180.0
"rotation": 0.0
},
"reversed": false,
"folder": null,
"folder": "Blue Middle",
"idealStartingState": {
"velocity": 0,
"rotation": 180.0
"rotation": 0.0
},
"useDefaultConstraints": true
}
src/main/deploy/pathplanner/paths/Line.path → src/main/deploy/pathplanner/paths/Back To Shoot Red.path
+9 -9
View File
@@ -3,25 +3,25 @@
"waypoints": [
{
"anchor": {
"x": 14.314,
"y": 4.15
"x": 13.01022182786158,
"y": 4.0
},
"prevControl": null,
"nextControl": {
"x": 14.299415121255347,
"y": 4.61077032810271
"x": 13.865906250551847,
"y": 4.0
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 14.314,
"y": 6.668017118402283
"x": 13.5,
"y": 4.0
},
"prevControl": {
"x": 14.247660485021397,
"y": 6.305734664764623
"x": 13.231071353115471,
"y": 4.0
},
"nextControl": null,
"isLocked": false,
@@ -45,7 +45,7 @@
"rotation": 180.0
},
"reversed": false,
"folder": null,
"folder": "Red Middle",
"idealStartingState": {
"velocity": 0,
"rotation": 180.0
src/main/deploy/pathplanner/paths/Climb Step2 Blue.path
+54
View File
@@ -0,0 +1,54 @@
{
"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
}
src/main/deploy/pathplanner/paths/Climb Step2 Red.path
+54
View File
@@ -0,0 +1,54 @@
{
"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
}
src/main/deploy/pathplanner/paths/Climb Step3 Blue.path
+54
View File
@@ -0,0 +1,54 @@
{
"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
}
src/main/deploy/pathplanner/paths/Climb Step3 Red.path
+54
View File
@@ -0,0 +1,54 @@
{
"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
}
src/main/deploy/pathplanner/paths/Copy of blue depot go 1.path
+63
View File
@@ -0,0 +1,63 @@
{
"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
}
src/main/deploy/pathplanner/paths/Depot Intake Blue.path
+54
View File
@@ -0,0 +1,54 @@
{
"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
}
src/main/deploy/pathplanner/paths/Depot Intake Red.path
+54
View File
@@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 15.4,
"y": 2.1
},
"prevControl": null,
"nextControl": {
"x": 15.65,
"y": 2.1
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 15.9,
"y": 2.1
},
"prevControl": {
"x": 15.65,
"y": 2.1
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 0.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": false
}
src/main/deploy/pathplanner/paths/Depot To Climb Blue.path
+54
View File
@@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 0.64,
"y": 5.97
},
"prevControl": null,
"nextControl": {
"x": 0.6399999999999998,
"y": 4.9700000000000015
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 0.64,
"y": 4.705
},
"prevControl": {
"x": 0.6400000000000002,
"y": 5.705
},
"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": false
}
src/main/deploy/pathplanner/paths/Depot To Climb Red.path
+54
View File
@@ -0,0 +1,54 @@
{
"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
}
src/main/deploy/pathplanner/paths/Depot to Shoot Blue.path
+54
View File
@@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 0.84,
"y": 5.969
},
"prevControl": null,
"nextControl": {
"x": 1.2669518600756762,
"y": 5.612104897874479
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 3.04,
"y": 4.0
},
"prevControl": {
"x": 2.8248463530252574,
"y": 4.180015650217846
},
"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
}
src/main/deploy/pathplanner/paths/Depot to Shoot Red.path
+54
View File
@@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 15.7,
"y": 2.1
},
"prevControl": null,
"nextControl": {
"x": 16.18921636103803,
"y": 1.647541846848796
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 13.5,
"y": 4.0
},
"prevControl": {
"x": 13.247149348869588,
"y": 4.238251681778358
},
"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": 180.0
},
"reversed": false,
"folder": "Red Middle",
"idealStartingState": {
"velocity": 0,
"rotation": 180.0
},
"useDefaultConstraints": false
}
src/main/deploy/pathplanner/paths/New Path.path
+54
View File
@@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 3.5,
"y": 2.5
},
"prevControl": null,
"nextControl": {
"x": 3.8524186062054477,
"y": 2.382931901404091
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 2.0,
"y": 3.0
},
"prevControl": {
"x": 1.7628130762195064,
"y": 3.0790086272982027
},
"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
}
src/main/deploy/pathplanner/paths/Shoot to Climb Step1 Blue.path
+70
View File
@@ -0,0 +1,70 @@
{
"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
}
src/main/deploy/pathplanner/paths/Shoot to Climb Step1 Red.path
+70
View File
@@ -0,0 +1,70 @@
{
"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
}
src/main/deploy/pathplanner/paths/Shoot to Depot Blue.path
+54
View File
@@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 3.04,
"y": 4.0
},
"prevControl": null,
"nextControl": {
"x": 3.4702773700848932,
"y": 3.562911556367572
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 1.14,
"y": 5.969
},
"prevControl": {
"x": 0.8995187790061673,
"y": 6.223926270411972
},
"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": 0.0
},
"reversed": false,
"folder": "Blue Middle",
"idealStartingState": {
"velocity": 0,
"rotation": 0.0
},
"useDefaultConstraints": false
}
src/main/deploy/pathplanner/paths/Shoot to Depot Red.path
+54
View File
@@ -0,0 +1,54 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 13.5,
"y": 4.0
},
"prevControl": null,
"nextControl": {
"x": 13.699377937127288,
"y": 3.7771427571164287
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 15.400452528841061,
"y": 2.1
},
"prevControl": {
"x": 15.073650526430217,
"y": 2.4474296685616617
},
"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": 180.0
},
"reversed": false,
"folder": "Red Middle",
"idealStartingState": {
"velocity": 0,
"rotation": 180.0
},
"useDefaultConstraints": false
}
src/main/deploy/pathplanner/paths/Rotate.path → src/main/deploy/pathplanner/paths/To Depot Blue.path
+11 -11
View File
@@ -3,25 +3,25 @@
"waypoints": [
{
"anchor": {
"x": 14.314,
"y": 4.15
"x": 3.54,
"y": 4.0
},
"prevControl": null,
"nextControl": {
"x": 14.064,
"y": 4.15
"x": 2.539999999999999,
"y": 4.0
},
"isLocked": false,
"linkedName": "StartingPointRed1"
"linkedName": null
},
{
"anchor": {
"x": 14.314,
"y": 4.15
"x": 1.14,
"y": 5.97
},
"prevControl": {
"x": 14.650181888846735,
"y": 4.15
"x": 2.1399999999999992,
"y": 5.97
},
"nextControl": null,
"isLocked": false,
@@ -45,10 +45,10 @@
"rotation": 0.0
},
"reversed": false,
"folder": null,
"folder": "Blue Middle",
"idealStartingState": {
"velocity": 0,
"rotation": 180.0
"rotation": 0.0
},
"useDefaultConstraints": true
}
src/main/deploy/pathplanner/paths/RotateAndLine.path → src/main/deploy/pathplanner/paths/To Depot Red.path
+11 -11
View File
@@ -3,25 +3,25 @@
"waypoints": [
{
"anchor": {
"x": 14.314,
"y": 4.15
"x": 13.0,
"y": 4.0
},
"prevControl": null,
"nextControl": {
"x": 14.312353780313838,
"y": 4.74015691868759
"x": 14.000000000000002,
"y": 4.0
},
"isLocked": false,
"linkedName": "StartingPointRed1"
"linkedName": null
},
{
"anchor": {
"x": 14.314,
"y": 6.357489300998574
"x": 15.400452528841061,
"y": 2.1
},
"prevControl": {
"x": 14.312353780313838,
"y": 6.098716119828817
"x": 14.400452528841061,
"y": 2.1
},
"nextControl": null,
"isLocked": false,
@@ -42,10 +42,10 @@
},
"goalEndState": {
"velocity": 0,
"rotation": 0.0
"rotation": 180.0
},
"reversed": false,
"folder": null,
"folder": "Red Middle",
"idealStartingState": {
"velocity": 0,
"rotation": 180.0
src/main/deploy/pathplanner/paths/blue depot go 1.path
+95
View File
@@ -0,0 +1,95 @@
{
"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
}
src/main/deploy/pathplanner/settings.json
+8 -2
View File
@@ -2,8 +2,14 @@
"robotWidth": 0.85,
"robotLength": 0.85,
"holonomicMode": true,
"pathFolders": [],
"autoFolders": [],
"pathFolders": [
"Blue Middle",
"Red Middle"
],
"autoFolders": [
"Red Middle",
"Blue Middle"
],
"defaultMaxVel": 1.5,
"defaultMaxAccel": 3.0,
"defaultMaxAngVel": 360.0,
src/main/java/frc/robot/Constants.java
+106 -78
View File
@@ -2,70 +2,85 @@ package frc.robot;
import com.ctre.phoenix6.configs.Slot0Configs;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
public final class Constants {
private Constants() {}
public static final boolean LEDUsing = false;
public enum RobotStatus{
Stopped,
AllTelop,
PassingBall,
AutoAimming,
Climbing,
CrossingBump,
CrossingTrench
}
public class Shooter {
public static final double conveyorSpeed = 20;
public static class Shooter {
// 机构 CAN ID 目前是占位值；季后赛最终接线确认后，只需要改这里。
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 defaultFlywheelVoltage = 3.34;
public static final double flywheelReadyVoltageThreshold = 1.0;
public static final double flywheelReadyDelaySeconds = 0.5;
// 距离(米) -> 飞轮电压(V) 射表。当前为占位点；
// 真车试射后按实测数据加点即可，查表自动线性插值并在两端取边界值。
public static final InterpolatingDoubleTreeMap flywheelVoltageByDistance = new InterpolatingDoubleTreeMap();
static {
flywheelVoltageByDistance.put(1.2, 5.82);
flywheelVoltageByDistance.put(2.5, 6.34);
flywheelVoltageByDistance.put(3.5, 6.73);
flywheelVoltageByDistance.put(5.0, 7.33);
}
public static final double shooterConveyorFeedVoltage = 3.0;
public static final double shooterConveyorReverseVoltage = -2.0;
public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs()
.withKP(0.0).withKI(0).withKD(0)
.withKV(0.0088).withKA(0.0).withKS(0.0);
.withKV(0.099).withKA(0.0).withKS(0.0);
public static final Slot0Configs conveyorSlot0Configs = new Slot0Configs()
.withKP(0.0).withKI(0).withKD(0)
.withKV(0.02).withKA(0.15).withKS(0);
.withKV(0.16).withKA(0.15).withKS(0);
public static final Slot0Configs backboardSlot0Configs = new Slot0Configs()
.withKP(0.01).withKI(0).withKD(0)
.withKV(0.008).withKA(0.0).withKS(0.010);
.withKV(0.03).withKA(0.0).withKS(0.010);
public class backboardPositionPID {
public static final double kP = 0.1;
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 = 30; //也就是output
// 背板的上下限位角度
public static final double backboardUpLimit = 3000.0; // 背板上限角度（根据实际需求设置）
public static final double backboardDownLimit = 0.0; // 背板下限角度（根据实际需求设置）
public static final double backboardSpeedMax = 5.0;
public static final double backboardSpeedMin = 0.4;
public static final double backboardUpLimit = 2200.0;
public static final double backboardDownLimit = 0.0;
}
public class ShooterCalculation{
public static final double positionNear = 0; // 近距离背板角度
public static final double positionMid = 1600; // 中距离背板角度
public static final double postionFar = 3200; // 远距离背板角度
}
public class AutoPositioning {
public static final double autoRotationkP = 0.1;
public static class AutoPositioning {
public static final double autoRotationkP = 0.08;
public static final double autoRotationForBumpTargetDegrees = 45;
public static final double autoRotationForTrenchTargetDegrees = 0;
public static final boolean teleopOffsetEnabled = true;
public static final double teleopOffsetkP = 15;
public static final double autoPositioningkP = 1.0;
public static final double autoPositioningAngleError = 1.0;
public static final double[] bumpY = {2.51,5.556};
public static final double[] 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 {
@@ -74,68 +89,81 @@ public final class Constants {
SYSID,
OTHER
}
public class Pigeon {
public static class Pigeon {
public static final int CANID = 0;
}
public class Limelight {
public static final String LIMELIGHT_NAME_Shooter = "limelight-shooter";
public static final String LIMELIGHT_NAME_Intaker = "limelight-intaker";
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 class Field {
public static class Field {
public static final double RedHubPositionX = 11.914;
public static final double RedHubPositionY = 4.034;
public static final double PassingBallPosX = 0;
public static final double PassingBallPosY1 = 2.51;
public static final double PassingBallPosY2 = 5.556;
public static final double BlueHubPositionX = 4.625;
public static final double BlueHubPositionY = 4.034;
public static final double PassingBallPosY1 = 1.405;
public static final double PassingBallPosY2 = 6.664;
public static final double RedPassingBallPosX = 13.355;
public static final double BluePassingBallPosX = 3.184;
}
public class Intaker {
public static class Intaker {
// 进球机构 CAN ID 同样是占位值，真实 ID 确认后集中在这里修改。
public static final int leftIntakeRollerID = 20;
public static final int rightIntakeRollerID = 47;
public static final int leftIntakeRotaterID = 48;
public static final int rightIntakeRotaterID = 43;
public static final double intakeRotaterUpLimit = 0.0; // rotator上限角度
public static final double intakeRotaterDownLimit = 0.0; // rotator下限角度
public static final double intakeRollerIntakeVoltage = -6.0;
public static final double intakeRollerOuttakeVoltage = 3.0;
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 double intakeRotaterRaiseVoltage = 0.8;
public static final double intakeRotaterLowerVoltage = 0.05;
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 intakeRotaterShootAssistPeriodSeconds = 0.4;
public static final double intakeRotaterShootAssistUpDutyCycle = 0.5;
public static final double IntakeGetterSpeed = 0.0;
// 单位RPS
public static final double positionToleranceRotations = 0.00;
//rotater容差
}
public static class Climber {
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 int motorID = 49;
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 boolean inverted = false;
public static final double IntakeGetterSpeedforAuto = 0.5;
// 这里给出可用的占位值，真车调好位置闭环后再收紧。
}
public static final double climbUpTicks = 94; // TODO: 实测后调整
public static final double climbDownTicks = 0; // TODO: 实测后调整
public static final double holdToleranceRot = 0.5;
public static final double deltaDownTicks = 1;
public static class Conveyor {
public static final int mainConveyorID = 42;
public static final double feedVoltage = 2.5;
public static final double reverseVoltage = -2.0;
public static final Slot0Configs slot0Configs = new Slot0Configs();
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);
}
static {
// TODO: PID/F 值你们实测后填
slot0Configs.kP = 0.2;
slot0Configs.kI = 0.0;
slot0Configs.kD = 0.0;
public static class Superstructure {
// 状态机时间和门控容差。真车基础动作验证后，需要现场调这些值。
public static final double shooterFeedVoltage = Shooter.shooterConveyorFeedVoltage;
public static final double shooterReverseVoltage = Shooter.shooterConveyorReverseVoltage;
public static final double shootTimeoutSeconds = 1.0;
public static final double aimToleranceDegrees = 1.5;
public static final double stationarySpeedToleranceMetersPerSecond = 0.5;
public static final double passBallFlywheelVoltage = Shooter.defaultFlywheelVoltage;
public static final double passBallBackboardPosition = 175.0;
// TODO: 如果要前馈（速度闭环一般建议有 kV）
slot0Configs.kS = 0.0;
slot0Configs.kV = 0.1;
slot0Configs.kA = 0.0;
// 射击时背板的目标位置（外接编码器计数）。
public static final double backboardShootPosition = 175.0;
// 射击门控是否要求底盘接近静止。
public static final boolean stationaryGateEnabled = true;
}
}
}
src/main/java/frc/robot/Main.java
+3 -3
View File
@@ -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;
src/main/java/frc/robot/Robot.java
+17 -5
View File
@@ -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;
import edu.wpi.first.wpilibj.TimedRobot;
@@ -20,11 +20,21 @@ public class Robot extends TimedRobot {
public void robotPeriodic() {
CommandScheduler.getInstance().run();
m_robotContainer.tunerPeriodic();
m_robotContainer.dashboardPeriodic();
}
@Override
public void disabledInit() {}
public void disabledInit() {
System.out.println("Robot disabledInit called");
// 进入 disabled 时强制所有重构后的机构回到安全状态，
// 即使上一周期还有手动或自动请求也不能继续动作。
m_robotContainer.superstructure.requestIdle();
m_robotContainer.shooterSubsystem.stopAll();
m_robotContainer.intakeRollerSubsystem.stop();
m_robotContainer.intakeRotaterSubsystem.stop();
m_robotContainer.conveyorSubsystem.stop();
}
@Override
public void disabledPeriodic() {}
@@ -42,7 +52,9 @@ public class Robot extends TimedRobot {
}
@Override
public void autonomousPeriodic() {}
public void autonomousPeriodic() {
m_robotContainer.driveControls.pushDistanceData();
}
@Override
public void autonomousExit() {}
src/main/java/frc/robot/RobotContainer.java
+154 -74
View File
@@ -12,66 +12,102 @@ import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.Constants.RobotStatus;
import frc.robot.controls.Autocommand;
import frc.robot.controls.DriveControls;
import frc.robot.controls.ShooterControls;
import frc.robot.controls.OperatorControls;
import frc.robot.generated.TunerConstants;
import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.subsystems.ConveyorSubsystem;
import frc.robot.subsystems.IntakeRollerSubsystem;
import frc.robot.subsystems.IntakeRotaterSubsystem;
import frc.robot.subsystems.LEDSubsystem;
import frc.robot.subsystems.LimelightSubsystem;
import frc.robot.subsystems.ShooterSubsystem;
import frc.robot.subsystems.Superstructure;
import frc.robot.tuning.ConfigTalonFXMotorTuner;
import frc.robot.tuning.ConfigTalonFXSMotorTuner;
import frc.robot.tuning.DriveGainsTuner;
import frc.robot.utils.GameData2026;
import frc.robot.utils.LedBindings;
import frc.robot.controls.ClimberControls;
import frc.robot.subsystems.ClimberSubsystem;
public class RobotContainer {
/* ====================== */
/* 手柄 */
/* ====================== */
// 只使用一个 Xbox 手柄（端口 0）
private final CommandXboxController controller = new CommandXboxController(0);
// 使用两个 Xbox 手柄：0 号给底盘驾驶，1 号给机构操作员。
private final CommandXboxController controllerlower = new CommandXboxController(0);
private final CommandXboxController controllerupper = new CommandXboxController(1);
/* ====================== */
/* 子系统 */
/* ====================== */
// Phoenix TunerX 生成的底盘
// 这里继续使用 Phoenix TunerX 生成的底盘，避免改动已经验证过的 swerve 底层。
public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
public final ClimberSubsystem climberSubsystem = new ClimberSubsystem();
public final ShooterSubsystem shooterSubsystem = new ShooterSubsystem();
public final IntakeRollerSubsystem intakeRollerSubsystem = new IntakeRollerSubsystem();
public final IntakeRotaterSubsystem intakeRotaterSubsystem = new IntakeRotaterSubsystem();
public final ConveyorSubsystem conveyorSubsystem = new ConveyorSubsystem();
public final LimelightSubsystem limelightSubsystem = new LimelightSubsystem();
// LED 子系统
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();
/* ====================== */
/* 控制封装/日志 */
/* 控制封装/调参 */
/* ====================== */
private final DriveControls driveControls = new DriveControls(drivetrain, controller,robotStatusManager );
private final ShooterControls shooterControls = new ShooterControls(shooterSubsystem, controller,robotStatusManager);
private final ClimberControls climberControls = new ClimberControls(climberSubsystem, controller);
public final DriveControls driveControls = new DriveControls(drivetrain, controllerlower, superstructure);
// 操作员意图层替代旧的射击/进球分离控制类，让复杂动作判断集中留在 Superstructure。
private final OperatorControls operatorControls = new OperatorControls(superstructure, shooterSubsystem);
private final DriveGainsTuner driveGainsTuner = new DriveGainsTuner(drivetrain);
private ConfigTalonFXMotorTuner configFlywheelTuner = new ConfigTalonFXMotorTuner(shooterSubsystem.flywheelMotorLeft, "flywheel", Constants.Shooter.flyWheelSlot0Configs);
private ConfigTalonFXMotorTuner configConveyorTuner = new ConfigTalonFXMotorTuner(shooterSubsystem.conveyorMotor, "conveyor", Constants.Shooter.conveyorSlot0Configs);
private ConfigTalonFXSMotorTuner configBackboardTuner = new ConfigTalonFXSMotorTuner(shooterSubsystem.backboardMotor, "backboard", Constants.Shooter.backboardSlot0Configs);
private final GameData2026 gameData2026 = new GameData2026();
private ConfigTalonFXMotorTuner configFlywheelTuner =
new ConfigTalonFXMotorTuner(shooterSubsystem.leftFlywheel, "flywheel", Constants.Shooter.flyWheelSlot0Configs);
private ConfigTalonFXMotorTuner configConveyorTuner =
new ConfigTalonFXMotorTuner(shooterSubsystem.leftConveyor, "conveyor", Constants.Shooter.conveyorSlot0Configs);
private ConfigTalonFXSMotorTuner configBackboardTuner =
new ConfigTalonFXSMotorTuner(shooterSubsystem.backboardMotor, "backboard", Constants.Shooter.backboardSlot0Configs);
private ConfigTalonFXMotorTuner configMainConveyorTuner =
new ConfigTalonFXMotorTuner(conveyorSubsystem.mainConveyor, "mainconveyor", Constants.Conveyor.mainConveyorSlot0Configs);
private ConfigTalonFXMotorTuner configIntakerRollerTuner =
new ConfigTalonFXMotorTuner(intakeRollerSubsystem.leftIntakeRoller, "intakerroller", Constants.Intaker.intakeRollerSlot0Configs);
private ConfigTalonFXMotorTuner configIntakerRotaterTuner =
new ConfigTalonFXMotorTuner(intakeRotaterSubsystem.leftIntakeRotater, "intakerrotater", Constants.Intaker.intakeRotaterSlot0Configs);
private final SendableChooser<Command> autoChooser;
private final Trigger enableTrigger = new Trigger(DriverStation::isEnabled);
public RobotContainer() {
Pose2d startingPose = Constants.StartingPoints.Red.Point2;
drivetrain.resetPose(startingPose);
configueSwerve();
configueShooter();
bingdingLED();
configueIntaker();
Autocommand.preNameCommands(
superstructure,
intakeRotaterSubsystem,
driveControls,
operatorControls
);
SignalLogger.enableAutoLogging(false);
autoChooser = AutoBuilder.buildAutoChooser();
SmartDashboard.putData("Auto Chooser", autoChooser);
configueSwerve();
configueShooter();
configueClimber();
bingdingLED();
}
/* ====================== */
@@ -79,68 +115,85 @@ public class RobotContainer {
/* ====================== */
private void configueSwerve() {
enableTrigger.onTrue(driveControls.getAllianceColorCommand());
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());
// //左保险：按住刹车
// controller.leftBumper()
// .onTrue(Commands.runOnce(driveControls::emergencyStop))
// .whileTrue(driveControls.brakeWhileHeld());
// // 右保险：按住加速（Boost）
// controller.rightBumper()
// .onTrue(Commands.runOnce(() -> driveControls.setBoostEnabled(true)))
// .onFalse(Commands.runOnce(() -> driveControls.setBoostEnabled(false)));
// 左保险：按住刹车。
controllerlower.leftBumper()
.onTrue(Commands.runOnce(driveControls::emergencyStop))
.whileTrue(driveControls.brakeWhileHeld());
// Back：重置场地坐标系角度
controller.back().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric));
// Start：强行使用metaTag2全场定位
controller.start().whileTrue(drivetrain.run(drivetrain::forceUsingLimelightMT2));
// 右保险：按住开启加速模式，松开后恢复普通速度。
controllerlower.rightBumper()
.onTrue(Commands.runOnce(() -> driveControls.setBoostEnabled(true)))
.onFalse(Commands.runOnce(() -> driveControls.setBoostEnabled(false)));
//LM 按下时使用机器人坐标系
controller.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming));
controller.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
// Back 键：重置场地坐标系角度。
controllerlower.back().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric));
// Start 键：强行使用 shooter 侧 Limelight 的 MegaTag2 全场定位。
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");
}));
// 预留的左摇杆自动瞄准切换，当前保持注释，避免改变驾驶员已习惯的按键。
// controllerlower.leftStick().onTrue(operatorControls.requestAimHubCommand());
// controllerlower.leftStick().onFalse(operatorControls.requestIdleCommand());
// 上操作员右保险：按住进入自动瞄准模式，松开回到全手动模式。
Trigger autoAim = controllerupper.rightBumper(); // 可按需要再并入驾驶员左摇杆。
autoAim.onTrue(operatorControls.requestAimHubCommand());
autoAim.onFalse(operatorControls.requestIdleCommand());
// 上操作员左保险：按住进入传球模式，松开回到全手动模式。
Trigger autoAim2 = controllerupper.leftBumper(); // 可按需要再并入驾驶员左摇杆。
autoAim2.onTrue(operatorControls.requestPassBallCommand());
autoAim2.onFalse(operatorControls.requestIdleCommand());
controller.rightStick().and(controller.leftBumper())
.onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingBump))
.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
controller.rightStick().and(controller.rightBumper())
.onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingTrench))
.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
}
private void configueShooter(){
shooterSubsystem.setDefaultCommand(shooterControls.defaultShooterCommand());
controller.povLeft().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(-1)));
controller.povRight().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(1)));
controller.povUp().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(100)));
controller.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-100)));
private void configueShooter() {
// 射击相关按钮只请求 Superstructure 状态；是否喂球由 Superstructure
// 根据飞轮速度和底盘瞄准统一判断。
// 这些命令 require Superstructure，与自动程序共用需求；
// 用 teleop 门控避免自动赛阶段误触直接取消整个 PathPlanner 自动。
Trigger teleopOnly = RobotModeTriggers.teleop();
controllerupper.rightTrigger().and(teleopOnly)
.onTrue(operatorControls.requestShootHubCommand())
.onFalse(operatorControls.requestIdleCommand());
controllerupper.b().and(teleopOnly)
.onTrue(operatorControls.requestEjectCommand())
.onFalse(operatorControls.requestIdleCommand());
controllerupper.povLeft().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVoltageOffset(-0.1)));
controllerupper.povRight().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVoltageOffset(0.1)));
controllerupper.povUp().onTrue(Commands.runOnce(() -> operatorControls.adjustBackboardRateOffset(100)));
controllerupper.povDown().onTrue(Commands.runOnce(() -> operatorControls.adjustBackboardRateOffset(-100)));
controllerupper.back().onTrue(Commands.runOnce(() -> operatorControls.resetOffsets()));
controllerupper.start().onTrue(operatorControls.resetBackboardCommand());
}
/* ====================== */
/* LED 绑定 */
/* ====================== */
private void bingdingLED() {
LedBindings.bindModeIndicators(leds);
}
public Command getAutonomousCommand() {
public Command getAutonomousCommand() {
return autoChooser.getSelected();
}
@@ -149,14 +202,41 @@ public class RobotContainer {
configFlywheelTuner.periodic(() -> shooterSubsystem.applyLeftConfigurationToRight());
configConveyorTuner.periodic(null);
configBackboardTuner.periodic();
shooterSubsystem.setRightFollowLeft();
shooterSubsystem.debugBackboardTargetPeriodic();
configMainConveyorTuner.periodic(null);
configIntakerRollerTuner.periodic(null);
configIntakerRotaterTuner.periodic(null);
shooterSubsystem.setRightConveyorFollowLeft();
}
private void configueClimber() {
enableTrigger.onTrue(climberControls.enableInitCommand());
climberSubsystem.setDefaultCommand(climberControls.defaultClimberCommand());
public void dashboardPeriodic() {
gameData2026.periodic();
}
private void configueIntaker() {
// 进球相关按钮也只请求 Superstructure 状态；intaker 开赛后默认保持下放。
Trigger teleopOnly = RobotModeTriggers.teleop();
controllerupper.leftTrigger().and(teleopOnly)
.onTrue(operatorControls.requestIntakeCommand())
.onFalse(operatorControls.requestIdleCommand());
Trigger manualRaiseIntaker = controllerupper.x().and(teleopOnly);
manualRaiseIntaker
.onTrue(operatorControls.requestManualCommand())
.whileTrue(operatorControls.manualRaiseIntakeRotaterCommand(intakeRotaterSubsystem))
.onFalse(operatorControls.requestIdleCommand());
Trigger manualLowerIntaker = controllerupper.a().and(teleopOnly);
manualLowerIntaker
.onTrue(operatorControls.requestManualCommand())
.whileTrue(operatorControls.manualLowerIntakeRotaterCommand(intakeRotaterSubsystem))
.onFalse(operatorControls.requestIdleCommand());
controllerupper.y().and(teleopOnly)
.onTrue(Commands.sequence(
operatorControls.requestManualCommand(),
operatorControls.lowerIntakeRotaterForMatchCommand(intakeRotaterSubsystem),
operatorControls.requestIdleCommand()
));
}
}
src/main/java/frc/robot/RobotStatusManager.java
-25
View File
@@ -1,25 +0,0 @@
package frc.robot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import frc.robot.Constants.RobotStatus;
public class RobotStatusManager {
private RobotStatus robotStatus;
public RobotStatusManager(){
this.robotStatus = RobotStatus.AllTelop;
}
public RobotStatus getStatus(){
return this.robotStatus;
}
public void setStatus(RobotStatus rs){
this.robotStatus = rs;
}
public Command setStatusCommand(RobotStatus rs){
return Commands.runOnce(()->setStatus(rs));
}
}
src/main/java/frc/robot/controls/Autocommand.java
+77
View File
@@ -0,0 +1,77 @@
package frc.robot.controls;
import com.pathplanner.lib.auto.NamedCommands;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc.robot.Constants;
import frc.robot.subsystems.IntakeRotaterSubsystem;
import frc.robot.subsystems.Superstructure;
/**
* 注册 PathPlanner 的 NamedCommands。
*
* 自动路径只请求机器人级动作，不直接设置 shooter、intake 或 conveyor 电机；
* 这样自动和手动都会走同一套 Superstructure 状态机。
*/
public class Autocommand extends SequentialCommandGroup {
public static void preNameCommands(
Superstructure superstructure,
IntakeRotaterSubsystem intakeRotater,
DriveControls driveControls,
OperatorControls operatorControls
) {
NamedCommands.registerCommand("start_intake", startIntake(superstructure));
NamedCommands.registerCommand("start_intake_and_conveyor", startIntake(superstructure));
NamedCommands.registerCommand("stop_intake_and_conveyor", idle(superstructure));
NamedCommands.registerCommand("start_intake_timelimit", timedIntake(superstructure));
NamedCommands.registerCommand("stop_intake", idle(superstructure));
NamedCommands.registerCommand("intaker_down", lowerIntaker(intakeRotater));
NamedCommands.registerCommand("intaker_up", raiseIntaker(intakeRotater));
NamedCommands.registerCommand("shoot_hub", shootHub(superstructure));
NamedCommands.registerCommand("Shake", shake(driveControls));
NamedCommands.registerCommand("AutoAim", autoAim(driveControls));
NamedCommands.registerCommand("reset_backboard0", operatorControls.resetBackboard0Command());
}
public static Command startIntake(Superstructure superstructure) {
return superstructure.requestIntakeCommand();
}
public static Command timedIntake(Superstructure superstructure) {
return Commands.sequence(
superstructure.requestIntakeCommand(),
Commands.waitSeconds(1.5),
superstructure.requestIdleCommand()
);
}
public static Command idle(Superstructure superstructure) {
return superstructure.requestIdleCommand();
}
public static Command lowerIntaker(IntakeRotaterSubsystem intakeRotater) {
return intakeRotater.lowerForMatchCommand();
}
public static Command raiseIntaker(IntakeRotaterSubsystem intakeRotater) {
return intakeRotater.raiseForMatchCommand();
}
// 自动射击和手动射击使用同一套 gated shooting 流程。
public static Command shootHub(Superstructure superstructure) {
return Commands.sequence(
superstructure.requestShootHubCommand(),
Commands.waitSeconds(Constants.Superstructure.shootTimeoutSeconds),
superstructure.requestIdleCommand()
);
}
public static Command autoAim(DriveControls driveControls) {
return driveControls.autoAimCommand();
}
public static Command shake(DriveControls driveControls) {
return driveControls.shakeCommand();
}
}
src/main/java/frc/robot/controls/ClimberControls.java
-53
View File
@@ -1,53 +0,0 @@
package frc.robot.controls;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.subsystems.ClimberSubsystem;
public class ClimberControls {
private final ClimberSubsystem climber;
private final CommandXboxController controller;
private final NetworkTable table;
public ClimberControls(ClimberSubsystem climber, CommandXboxController controller) {
this.climber = climber;
this.controller = controller;
this.table = NetworkTableInstance.getDefault().getTable("ClimberControls");
}
public Command defaultClimberCommand() {
return Commands.run(()->{
//climber.climbUp();
// 优先级：如果同时按，优先上升（你也可以反过来）
if(controller.y().getAsBoolean()&&controller.x().getAsBoolean()){
climber.zeroEncoder();
}
else if (controller.y().getAsBoolean()) {
climber.climbUp();
} else if (controller.x().getAsBoolean()) {
climber.climbDownNonStop();
} else {
// 松开：记录当前位置 + 锁死
climber.captureHoldPosition();
climber.holdPosition();
}
// 调试用
table.getEntry("xHeld_up").setBoolean(controller.x().getAsBoolean());
table.getEntry("yHeld_down").setBoolean(controller.y().getAsBoolean());
}, climber);
}
public Command enableInitCommand() {
return Commands.run(climber::initialize, climber)
.withTimeout(0.5)
.andThen(Commands.runOnce(climber::zeroEncoder, climber));
}
}
src/main/java/frc/robot/controls/DriveControls.java
+187 -173
View File
@@ -11,48 +11,60 @@ import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.Constants;
import frc.robot.RobotStatusManager;
import frc.robot.Constants.DriveMode;
import frc.robot.Constants.RobotStatus;
import frc.robot.generated.TunerConstants;
import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.subsystems.Superstructure;
import frc.robot.subsystems.Superstructure.WantedState;
import java.util.Optional;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.Timer;
public class DriveControls {
public boolean isRedAlliance;
private final CommandSwerveDrivetrain drivetrain;
private final CommandXboxController driver;
private final double maxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
private final double maxAngularRate = RotationsPerSecond.of(0.75).in(edu.wpi.first.units.Units.RadiansPerSecond);
// 默认速度比例（平时开车）
// 默认速度比例，普通驾驶使用。
private double normalDriveScale = 0.3;
private double normalTurnScale = 0.7;
// 加速模式比例（按住 RB）
// 加速模式比例，按住 RB 时使用。
private double boostDriveScale = 1.0;
private double boostTurnScale = 1.0;
// 自动瞄准时降低平移速度。
private double slowDriveScale =0.1;
private boolean boostEnabled = false;
private final SlewRateLimiter vxLimiter = new SlewRateLimiter(5.0); // m/s^2 等效（调）
private final SlewRateLimiter vxLimiter = new SlewRateLimiter(5.0); // m/s^2 等效加速度限制
private final SlewRateLimiter vyLimiter = new SlewRateLimiter(5.0);
private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(9.0); // rad/s^2 等效（调）
private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(9.0); // rad/s^2 等效角加速度限制
// ====== 类成员变量里新增：记录上一次输出 ======
// 记录上一周期输出，用于只限制加速、不限制减速。
private double lastVx = 0.0;
private double lastVy = 0.0;
private double lastOmega = 0.0;
private NetworkTable autoControlNetworkTable;
private final RobotStatusManager robotStatusManager;
private final Superstructure superstructure;
public void setBoostEnabled(boolean enabled) {
boostEnabled = enabled;
}
// 新增：当前是否使用场地坐标系（默认 true）
// 当前是否使用场地坐标系，默认启用。
private boolean fieldCentricEnabled = true;
private final SwerveRequest.FieldCentric fieldCentric =
@@ -74,10 +86,10 @@ public class DriveControls {
.withRotationalDeadband(maxAngularRate * 0.05)
.withDriveRequestType(DriveRequestType.Velocity);
public DriveControls(CommandSwerveDrivetrain drivetrain, CommandXboxController driver, RobotStatusManager rsm) {
public DriveControls(CommandSwerveDrivetrain drivetrain, CommandXboxController driver, Superstructure superstructure) {
this.drivetrain = drivetrain;
this.driver = driver;
this.robotStatusManager = rsm;
this.superstructure = superstructure;
autoControlNetworkTable = NetworkTableInstance.getDefault().getTable("AutoControl");
}
@@ -89,16 +101,16 @@ public class DriveControls {
return fieldCentricEnabled;
}
/** 底盘的默认驾驶命令（teleop 期间持续执行） */
/** 底盘默认驾驶命令，在 teleop 期间持续执行。 */
public Command defaultDriveCommand() {
return drivetrain.applyRequest(() -> {
SmartDashboard.putString("robotStatus",robotStatusManager.getStatus().name());
WantedState wantedState = superstructure.getWantedState();
SmartDashboard.putString("wantedState", wantedState.name());
double driveScale = boostEnabled ? boostDriveScale : normalDriveScale;
double turnScale = boostEnabled ? boostTurnScale : normalTurnScale;
double vx = -driver.getLeftY() * maxSpeed * driveScale;
double vy = -driver.getLeftX() * maxSpeed * driveScale;
double vomega = -driver.getRightX() * maxAngularRate * turnScale;
// ====== execute 内替换 limiter：只限加速、不限减速 ======
double limitedVx = limitAccelOnly(vx, lastVx, vxLimiter);
double limitedVy = limitAccelOnly(vy, lastVy, vyLimiter);
double limitedOmega = limitAccelOnly(vomega, lastOmega, omegaLimiter);
@@ -111,108 +123,40 @@ public class DriveControls {
vy = limitedVy;
vomega = limitedOmega;
double[] distanceAndRotation = calculateDistanceAndRotationToHub();
// 将距离值写入 NetworkTable
autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceAndRotation[0]);
autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(distanceAndRotation[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;
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 PassingBall:
vomega = calculateRotationSpeedFromRotationAngle(calculateDistanceAndRotationToPassBall()[1]);
case PASS_BALL:
vomega = calculateRotationSpeedFromRotationAngle(passAim[1]);
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
break;
case AutoAimming:
vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]);
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
break;
case AllTelop:
default:
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(Double.NaN);
break;
case Climbing:
break;
case CrossingBump:
double differenceBump = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.bumpY[0], Constants.AutoPositioning.bumpY[1]);
if (Math.abs(differenceBump) < 0.1) {
vy = 0;
} else {
double raw = differenceBump * Constants.AutoPositioning.autoPositioningkP;
vy = Math.copySign(
Math.max(Math.abs(raw), 0.4),
raw
);
}
autoControlNetworkTable.getEntry("BumpDifference").setDouble(differenceBump);
Rotation2d BumpcurrentAngle = drivetrain.getState().Pose.getRotation();
double BumpcurrentRad = BumpcurrentAngle.getRadians();
double targetA = Math.toRadians(45);
double targetB = Math.toRadians(-45);
// 计算误差
double errorA = targetA - BumpcurrentRad;
double errorB = targetB - BumpcurrentRad;
// 角度归一化到 [-pi, pi]
errorA = Math.atan2(Math.sin(errorA), Math.cos(errorA));
errorB = Math.atan2(Math.sin(errorB), Math.cos(errorB));
// 选误差绝对值更小的那个
double chosenError = (Math.abs(errorA) <= Math.abs(errorB)) ? errorA : errorB;
if (Math.abs(chosenError) < Math.toRadians(2.0)) {
vomega = 0.0;
} else {
vomega = chosenError * Constants.AutoPositioning.TurningkP;
}
// 限制最大角速度
vomega = Math.max(-maxAngularRate, Math.min(maxAngularRate, vomega));
//vomega = calculateRotationSpeedForBump();
break;
case CrossingTrench:
double differenceTrench = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.trenchY[0], Constants.AutoPositioning.trenchY[1]);
if (Math.abs(differenceTrench) < 0.1) {
vy = 0;
} else {
double raw = differenceTrench * Constants.AutoPositioning.autoPositioningkP;
vy = Math.copySign(
Math.max(Math.abs(raw), 0.4),
raw
);
}
autoControlNetworkTable.getEntry("TrenchDifference").setDouble(differenceTrench);
Rotation2d currentAngle = drivetrain.getState().Pose.getRotation();
double currentRad = currentAngle.getRadians();
double targetRad = Math.toRadians(Constants.AutoPositioning.TrenchtargetAngle);
double TrenchAngleerror = targetRad - currentRad;
if (Math.abs(TrenchAngleerror) < Math.toRadians(1.0)) {
vomega = 0.0;
} else {
vomega = TrenchAngleerror * Constants.AutoPositioning.TurningkP;
}
// 限幅（防止转太快）
vomega = Math.max(-maxAngularRate, Math.min(maxAngularRate, vomega));
//vomega = calculateRotationSpeedForTrench();
break;
}
}
autoControlNetworkTable.getEntry("ActualVX").setDouble(vx);
autoControlNetworkTable.getEntry("ActualVY").setDouble(vy);
autoControlNetworkTable.getEntry("ActualVOmega").setDouble(vomega);
boolean isAutoLike =
robotStatusManager.getStatus() == RobotStatus.CrossingBump ||
robotStatusManager.getStatus() == RobotStatus.CrossingTrench;
if (fieldCentricEnabled || isAutoLike) {
// 自动状态用 BlueAlliance（不翻转），手柄 field-centric 仍用原 fieldCentric（OperatorPerspective）
var chosen = isAutoLike ? fieldCentricAuto : fieldCentric;
return chosen
if (fieldCentricEnabled) {
return fieldCentric
.withVelocityX(vx)
.withVelocityY(vy)
.withRotationalRate(vomega);
@@ -222,60 +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 calculateRotationSpeedForTrench(){
Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
double currentAngle = currentPose.getRotation().getDegrees();
double targetAngle = Constants.AutoPositioning.autoRotationForTrenchTargetDegrees; // 转换为度
double angleDifference = targetAngle - currentAngle;
// Normalize angle difference to the range [-180, 180]
if (angleDifference > 180) {
angleDifference -= 360;
} else if (angleDifference < -180) {
angleDifference += 360;
}
return calculateRotationSpeedFromRotationAngle(angleDifference);
}
public double calculateRotationSpeedForBump(){
Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
double currentAngle = currentPose.getRotation().getDegrees();
double targetAngle = Constants.AutoPositioning.autoRotationForBumpTargetDegrees; // 转换为度
double angleDifference = targetAngle - currentAngle;
// Normalize angle difference to the range [-180, 180]
if (angleDifference > 180) {
angleDifference -= 360;
} else if (angleDifference < -180) {
angleDifference += 360;
}
return calculateRotationSpeedFromRotationAngle(angleDifference);
}
// Calculate distance and rotation (angle difference)
public double[] calculateDistanceAndRotationToHub() {
Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
double targetX = Constants.Field.RedHubPositionX; // 目标 X 坐标
double targetY = Constants.Field.RedHubPositionY; // 目标 Y 坐标
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]
@@ -285,27 +195,26 @@ public class DriveControls {
angleDifference += 360;
}
// 计算距离
// 计算距离。
double distance = Math.sqrt(deltaX * deltaX + deltaY * deltaY);
// 计算距离和旋转角度
// 返回距离和旋转角度误差。
double[] distanceAndRotation = new double[]{distance, angleDifference};
// 返回距离和角度差
// 距离单位为米，角度单位为度。
return distanceAndRotation;
}
public double[] calculateDistanceAndRotationToPassBall() {
Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
double targetX = Constants.Field.PassingBallPosX; // 目标 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]
@@ -315,16 +224,15 @@ public class DriveControls {
angleDifference += 360;
}
// 计算距离
// 计算距离。
double distance = Math.sqrt(deltaX * deltaX + deltaY * deltaY);
// 计算距离和旋转角度
// 返回距离和旋转角度误差。
double[] distanceAndRotation = new double[]{distance, angleDifference};
// 返回距离和角度差
// 距离单位为米，角度单位为度。
return distanceAndRotation;
}
// Calculate rotation speed from rotation angle difference
public double calculateRotationSpeedFromRotationAngle(double angleDifference) {
double calDifference = angleDifference;
@@ -341,24 +249,34 @@ public class DriveControls {
return 0;
}
}
private double calculatevomegaFromRotationAngle(double error) {
double vomega;
if (Math.abs(error) < Math.toRadians(Constants.AutoPositioning.autoPositioningAngleError)) {
vomega = 0.0;
} else {
vomega = error * Constants.AutoPositioning.TurningkP;
}
// 限制最大角速度。
vomega = Math.max(-maxAngularRate, Math.min(maxAngularRate, vomega));
return vomega;
}
/**
* 只限制“加速”（幅值变大），不限制“减速”（幅值变小）
* 这样松杆/刹车会更及时，同时 boost 仍然能抑制电流冲击
/**
* 只限制加速，也就是输出幅值变大时；不限制减速。
* 这样松杆或刹车能更及时，同时 boost 仍能抑制电流冲击。
*/
private double limitAccelOnly(double target, double last, SlewRateLimiter limiter) {
// 如果正在减速（幅值变小），直接放行到目标值
// 如果正在减速，直接放行到目标值。
if (Math.abs(target) < Math.abs(last)) {
limiter.reset(target); // 同步 limiter 内部状态，防止下一次突变
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);
@@ -395,5 +313,101 @@ public class DriveControls {
public double getMaxAngularRate() {
return maxAngularRate;
}
public void setTeamColors() {
Optional<Alliance> ally = DriverStation.getAlliance(); // 获取联盟颜色
if (ally.isPresent()) {
// 根据联盟颜色更新自动瞄准目标。
if (ally.get() == Alliance.Red) {
isRedAlliance = true; // 红队
} else if (ally.get() == Alliance.Blue) {
isRedAlliance = false; // 蓝队
}
} else {
isRedAlliance = true;
}
if (isRedAlliance) {
System.out.println("Robot is on the Red Alliance.");
} else {
System.out.println("Robot is on the Blue Alliance.");
}
}
public Command getAllianceColorCommand() {
return Commands.runOnce(this::setTeamColors);
}
public Command autoAimCommand() {
return drivetrain.applyRequest(() -> {
double[] distanceAndRotation = calculateDistanceAndRotationToHub();
double vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]);
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));
}
public Command shakeCommand(double amplitude, double switchPeriod, double totalTime) {
Timer timer = new Timer();
return Commands.sequence(
new InstantCommand(timer::restart),
drivetrain.applyRequest(() -> {
double t = timer.get();
int phase = ((int) (t / switchPeriod)) % 4;
double vx = 0.0;
double vy = 0.0;
double[] distanceAndRotation = calculateDistanceAndRotationToHub();
double vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]);
switch (phase) {
case 0:
vx = amplitude; // 前
vy = 0.0;
break;
case 1:
vx = 0.0;
vy = amplitude; // 左
break;
case 2:
vx = -amplitude; // 后
vy = 0.0;
break;
case 3:
vx = 0.0;
vy = -amplitude; // 右
break;
}
return robotCentric
.withVelocityX(vx)
.withVelocityY(vy)
.withRotationalRate(vomega);
}, () -> DriveMode.ROBOT_CENTRIC).withTimeout(totalTime),
new InstantCommand(() -> {
timer.stop();
drivetrain.stop();
}, drivetrain)
);
}
public Command shakeCommand() {
return shakeCommand(0.3, 0.15, 5.0);
}
public void pushDistanceData(){
double[] distanceAndRotation = calculateDistanceAndRotationToHub();
// 将距离数据写入 NetworkTable。
autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceAndRotation[0]);
autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(distanceAndRotation[1]);
// 自动阶段默认驾驶命令不运行，这里同样保持目标朝向最新，
// 让自动程序里的射击门控 isAimed() 有效。
drivetrain.setTargetHeading(
drivetrain.getRotation().plus(Rotation2d.fromDegrees(distanceAndRotation[1])));
}
}
src/main/java/frc/robot/controls/IntakerControls.java
-71
View File
@@ -1,71 +0,0 @@
package frc.robot.controls;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.subsystems.IntakerSubsystem;
public class IntakerControls {
private final IntakerSubsystem intakerSubsystem;
private final CommandXboxController controller;
private static final double ROTATER_STEP_PER_CYCLE = 0.00; // 需要你按实际速度调：越大越快（fromAI）
public IntakerControls(IntakerSubsystem intakerSubsystem, CommandXboxController controller) {
this.intakerSubsystem = intakerSubsystem;
this.controller = controller;
}
public void configureBindings() {
//B.A：按住下降 + getter转；松开后 rotater保持当前位置，getter继续转
controller.a()
.whileTrue(
Commands.run(() -> {
// rotater下降：范围会被 subsystem 限制
double cur = intakerSubsystem.getIntakerotaterPosition();
intakerSubsystem.setIntakerRotaterPosition(cur - ROTATER_STEP_PER_CYCLE);
// getter 开
intakerSubsystem.setIntakeGetterOn(true);
}, intakerSubsystem)
)
.onFalse(
Commands.runOnce(() -> {
// 松：把当前位置作为target保持
double cur = intakerSubsystem.getIntakerotaterPosition();
intakerSubsystem.setIntakerRotaterPosition(cur);
// gtter继续转
intakerSubsystem.setIntakeGetterOn(true);
}, intakerSubsystem)
);
// === B.B：按住上升+getter停；松开后rotater保持当前位置
controller.b()
.whileTrue(
Commands.run(() -> {
// rotater升
double cur = intakerSubsystem.getIntakerotaterPosition();
intakerSubsystem.setIntakerRotaterPosition(cur + ROTATER_STEP_PER_CYCLE);
// getter关
intakerSubsystem.setIntakeGetterOn(false);
}, intakerSubsystem)
)
.onFalse(
Commands.runOnce(() -> {
// 松：保持当前位置
double cur = intakerSubsystem.getIntakerotaterPosition();
intakerSubsystem.setIntakerRotaterPosition(cur);
// getter仍然停止
intakerSubsystem.setIntakeGetterOn(false);
}, intakerSubsystem)
);
}
public Command defaultIntakerCommand() {
return Commands.none();
}
}
src/main/java/frc/robot/controls/OperatorControls.java
+96
View File
@@ -0,0 +1,96 @@
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.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 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 manualLowerIntakeRotaterCommand(IntakeRotaterSubsystem intakeRotater) {
return Commands.runEnd(intakeRotater::manualLower, intakeRotater::stop, intakeRotater);
}
public Command lowerIntakeRotaterForMatchCommand(IntakeRotaterSubsystem intakeRotater) {
return intakeRotater.lowerForMatchCommand();
}
// PathPlanner 自动射击辅助：先预备到所有射击条件满足，再请求喂球，
// 最后回到空闲状态。真正的“是否能射”判断仍然放在 Superstructure。
public Command autoShootToHubCommand() {
return Commands.sequence(
superstructure.requestShootHubCommand(),
Commands.waitSeconds(Constants.Superstructure.shootTimeoutSeconds),
superstructure.requestIdleCommand()
);
}
// 保留旧背板回零动作，方便调参和现场恢复；
// 具体电机操作在 ShooterSubsystem 内实现，这里只转发命令。
public Command resetBackboard0Command() {
return shooterSubsystem.homeBackboardCommand();
}
}
src/main/java/frc/robot/controls/ShooterControls.java
-222
View File
@@ -1,222 +0,0 @@
package frc.robot.controls;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.Constants;
import frc.robot.RobotStatusManager;
import frc.robot.Constants.Intaker;
import frc.robot.subsystems.IntakerSubsystem;
import frc.robot.subsystems.ShooterSubsystem;
public class ShooterControls {
private final ShooterSubsystem shooterSubsystem;
private final CommandXboxController controller;
private double flywheelSpeedOffset = 0.0;
private double backboardPositionOffset = 0.0;
private NetworkTable shooterControlTable;
private double conveyorSpeed = 0.0;
private double flywheelSpeed = 0;
private double backboardPosition = 0;
private final RobotStatusManager robotStatusManager;
public ShooterControls(ShooterSubsystem shooterSubsystem, CommandXboxController controller,RobotStatusManager rsm) {
this.shooterSubsystem = shooterSubsystem;
this.controller = controller;
this.robotStatusManager = rsm;
shooterControlTable = NetworkTableInstance.getDefault().getTable("ShooterControl");
}
public Command defaultShooterCommand() {
return Commands.run(() -> {
switch (robotStatusManager.getStatus()) {
case Stopped:
shooterSubsystem.stopMotors();
break;
case AllTelop:
case PassingBall:
var xP = shooterSubsystem.getDistance();
//xP = Math.max(1.2, Math.min(4.5, xP));
flywheelSpeed = 0.6956596811733782*xP*xP*xP*xP*xP*xP*xP-14.164408860183267*xP*xP*xP*xP*xP*xP+119.7115375240918*xP*xP*xP*xP*xP-543.0984876677477*xP*xP*xP*xP+1425.1928543240738*xP*xP*xP-2156.199296490315*xP*xP+1736.568139600292*xP-516.097598057351;
flywheelSpeed += flywheelSpeedOffset;
backboardPosition = xP*xP;//todo:实测传球的时候的背板位置 打表
backboardPosition += backboardPositionOffset;
conveyorSpeed = Constants.Shooter.conveyorSpeed;
if (controller.getRightTriggerAxis() < 0.1) {
flywheelSpeed = 0;
conveyorSpeed = 0;
}
if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.7){
conveyorSpeed = 0;
}
// flywheelSpeed = calculateFlywheelSpeedOnlywithOffset(60);
// double flywheelSpeed = calculateFlywheelSpeed(0);
// double BackboardPosition = calculateBackboardPosition(0);
shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed);
shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed);
shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition);
shooterSubsystem.setFlywheelSpeedByRPS(flywheelSpeed);
shooterSubsystem.setConveyorSpeedByRPS(conveyorSpeed);
shooterSubsystem.setBackboardPosition(backboardPosition);
if(shooterSubsystem.isBackboardAtTarget()){
shooterSubsystem.backboardMotor.set(0);
}else{
shooterSubsystem.outputBackboard();
}
break;
case AutoAimming:
var x = shooterSubsystem.getDistance();
x = Math.max(1.2, Math.min(4.5, x));
flywheelSpeed = 0.6956596811733782*x*x*x*x*x*x*x-14.164408860183267*x*x*x*x*x*x+119.7115375240918*x*x*x*x*x-543.0984876677477*x*x*x*x+1425.1928543240738*x*x*x-2156.199296490315*x*x+1736.568139600292*x-516.097598057351;
flywheelSpeed += flywheelSpeedOffset;
backboardPosition = -122.11889023593778*x*x*x*x*x*x*x+2453.385583841308*x*x*x*x*x*x-20553.05413872581*x*x*x*x*x+92963.33175750206*x*x*x*x-244905.25916147238*x*x*x+375150.1363997868*x*x-308031.51715612336*x+104203.52690842684;
backboardPosition += backboardPositionOffset;
conveyorSpeed = Constants.Shooter.conveyorSpeed;
if (controller.getRightTriggerAxis() < 0.1) {
flywheelSpeed = 0;
conveyorSpeed = 0;
}
if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.7){
conveyorSpeed = 0;
}
// flywheelSpeed = calculateFlywheelSpeedOnlywithOffset(60);
// double flywheelSpeed = calculateFlywheelSpeed(0);
// double BackboardPosition = calculateBackboardPosition(0);
shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed);
shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed);
shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition);
shooterSubsystem.setFlywheelSpeedByRPS(flywheelSpeed);
shooterSubsystem.setConveyorSpeedByRPS(conveyorSpeed);
shooterSubsystem.setBackboardPosition(backboardPosition);
if(shooterSubsystem.isBackboardAtTarget()){
shooterSubsystem.backboardMotor.set(0);
}else{
shooterSubsystem.outputBackboard();
}
break;
case Climbing:
case CrossingBump:
case CrossingTrench:
if(shooterSubsystem.getBackboardPosition() <= 20)
shooterSubsystem.stopMotors();
else{
shooterSubsystem.setBackboardPosition(0);
shooterSubsystem.outputBackboard();
}
break;
}
}, shooterSubsystem);
}
// public Command resetBackboardCommand(){
// return
// Commands.runOnce(()->{
// shooterSubsystem.backboardMotor.set(-0.1);
// shooterSubsystem.currentBackboardInalyzeMode = Mode.Inalyzing;}, shooterSubsystem)
// .andThen(
// Commands.waitSeconds(0.5))
// .andThen(Commands.runOnce(()->{
// shooterSubsystem.backboardMotor.set(0);
// shooterSubsystem.backboardEncoder.reset();
// shooterSubsystem.backboardPID.reset(0);}, shooterSubsystem))
// .andThen(
// Commands.waitSeconds(1))
// .andThen(Commands.runOnce(()->{
// shooterSubsystem.currentBackboardInalyzeMode = Mode.Inalyzed;}, shooterSubsystem));
// }
public void outputInfo(){
double currentConveyorSpeed = shooterSubsystem.conveyorMotor.getVelocity().getValueAsDouble();
double currentFlywheelSpeed = shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble();
double currentBackboardRate = shooterSubsystem.getBackboardPosition();
System.out.println("Conveyor: " + currentConveyorSpeed + "\t Flywheel: " + currentFlywheelSpeed + "\t Backboard:" + currentBackboardRate);
}
public Command resetBackboardCommand(){
return
Commands.runOnce(()->{
shooterSubsystem.backboardEncoder.reset();
shooterSubsystem.backboardPID.reset(0);}, shooterSubsystem);
}
//打表参数
// private static final double[][] FLYWHEEL_TABLE = {
// {1.5, 0.42},
// {2.0, 0.46},
// {2.5, 0.51},
// {3.0, 0.57},
// {3.5, 0.64},
// {4.0, 0.72}
// };
// //线性插值函数
// private static double lerp(double x0, double y0, double x1, double y1, double x) {
// if (Math.abs(x1 - x0) < 1e-9) {
// return y0;
// }
// double t = (x - x0) / (x1 - x0);
// return y0 + t * (y1 - y0);
// }
// private double calculateFlywheelSpeedOnlywithOffset(double defaultSpeed) {
// return defaultSpeed + flywheelSpeedOffset;
// }
// private double calculateBackboardPositionOnlywithOffset(double defaultPosition) {
// return defaultPosition + backboardPositionOffset;
// }
// private double calculateFlywheelSpeed(double distance) {
// // 小于最小距离：用第一个点
// if (distance <= FLYWHEEL_TABLE[0][0]) {
// return FLYWHEEL_TABLE[0][1] + flywheelSpeedOffset;
// }
// // 大于最大距离：用最后一个点
// int last = FLYWHEEL_TABLE.length - 1;
// if (distance >= FLYWHEEL_TABLE[last][0]) {
// return FLYWHEEL_TABLE[last][1] + flywheelSpeedOffset;
// }
// // 查找区间并插值
// for (int i = 0; i < FLYWHEEL_TABLE.length - 1; i++) {
// double d0 = FLYWHEEL_TABLE[i][0];
// double s0 = FLYWHEEL_TABLE[i][1];
// double d1 = FLYWHEEL_TABLE[i + 1][0];
// double s1 = FLYWHEEL_TABLE[i + 1][1];
// if (distance >= d0 && distance <= d1) {
// return lerp(d0, s0, d1, s1, distance) + flywheelSpeedOffset;
// }
// }
// return 0;
// }
// private double calculateBackboardPosition(double distance) {
// double BackboardPosition;
// // 距离阈值（单位自己按distance进行修改）
// final double DISTANCE_NEAR = 2.0; // 近距离阈值
// final double DISTANCE_MID = 4.0; // 中距离阈值
// if (distance < DISTANCE_NEAR) {
// BackboardPosition = Constants.ShooterCalculation.positionNear;
// }
// else if (distance < DISTANCE_MID) {
// BackboardPosition = Constants.ShooterCalculation.positionMid;
// }
// else {
// BackboardPosition = Constants.ShooterCalculation.postionFar;
// }
// BackboardPosition += backboardPositionOffset;
// return BackboardPosition;
// }
public void adjustFlywheelSpeedOffset(double offset) {
flywheelSpeedOffset += offset;
}
public void adjustBackboardRateOffset(double offset) {
backboardPositionOffset += offset;
}
}
src/main/java/frc/robot/generated/TunerConstants.java
+35 -18
View File
@@ -42,7 +42,7 @@ public class TunerConstants {
// The type of motor used for the drive motor
private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated;
// The type of motor used for the drive motor
// The type of motor used for the steer motor
private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated;
// The remote sensor feedback type to use for the steer motors;
@@ -55,7 +55,14 @@ public class TunerConstants {
// Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null.
// Some configs will be overwritten; check the `with*InitialConfigs()` API documentation.
private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration();
private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration()
.withCurrentLimits(
new CurrentLimitsConfigs()
// Default supply current limit is 70 A, but it can be lowered to avoid brownouts.
// Supply current limits can be larger than the breaker current rating.
.withSupplyCurrentLimit(Amps.of(70))
.withSupplyCurrentLimitEnable(true)
);
private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration()
.withCurrentLimits(
new CurrentLimitsConfigs()
@@ -66,13 +73,23 @@ public class TunerConstants {
);
private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration();
// Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
private static final Pigeon2Configuration pigeonConfigs = null;
private static final Pigeon2Configuration pigeonConfigs =
new Pigeon2Configuration()
.withGyroTrim(
new GyroTrimConfigs()
.withGyroScalarZ(-5.0)
)
.withPigeon2Features(
new Pigeon2FeaturesConfigs()
.withEnableCompass(false)
);
// CAN bus that the devices are located on;
// All swerve devices must share the same CAN bus
public static final CANBus kCANBus = new CANBus("", "./logs/example.hoot");
// Theoretical free speed (m/s) at 12 V applied output;
// Measured robot speed (m/s) at 12 V applied output;
// This is NOT the desired max robot speed - see MaxSpeed in RobotContainer instead;
// This needs to be tuned to your individual robot
public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.73);
@@ -126,10 +143,10 @@ public class TunerConstants {
// Front Left
private static final int kFrontLeftDriveMotorId = 11;
private static final int kFrontLeftSteerMotorId = 12;
private static final int kFrontLeftEncoderId = 13;
private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.412841796875);
private static final int kFrontLeftDriveMotorId = 1;
private static final int kFrontLeftSteerMotorId = 2;
private static final int kFrontLeftEncoderId = 3;
private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.18115234375);
private static final boolean kFrontLeftSteerMotorInverted = false;
private static final boolean kFrontLeftEncoderInverted = false;
@@ -137,10 +154,10 @@ public class TunerConstants {
private static final Distance kFrontLeftYPos = Inches.of(11.375);
// Front Right
private static final int kFrontRightDriveMotorId = 21;
private static final int kFrontRightSteerMotorId = 22;
private static final int kFrontRightEncoderId = 23;
private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.012939453125);
private static final int kFrontRightDriveMotorId = 11;
private static final int kFrontRightSteerMotorId = 12;
private static final int kFrontRightEncoderId = 13;
private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.16162109375);
private static final boolean kFrontRightSteerMotorInverted = false;
private static final boolean kFrontRightEncoderInverted = false;
@@ -148,10 +165,10 @@ public class TunerConstants {
private static final Distance kFrontRightYPos = Inches.of(-11.375);
// Back Left
private static final int kBackLeftDriveMotorId = 1;
private static final int kBackLeftSteerMotorId = 2;
private static final int kBackLeftEncoderId = 3;
private static final Angle kBackLeftEncoderOffset = Rotations.of(0.07275390625);
private static final int kBackLeftDriveMotorId = 21;
private static final int kBackLeftSteerMotorId = 22;
private static final int kBackLeftEncoderId = 23;
private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.017822265625);
private static final boolean kBackLeftSteerMotorInverted = false;
private static final boolean kBackLeftEncoderInverted = false;
@@ -162,7 +179,7 @@ public class TunerConstants {
private static final int kBackRightDriveMotorId = 31;
private static final int kBackRightSteerMotorId = 32;
private static final int kBackRightEncoderId = 33;
private static final Angle kBackRightEncoderOffset = Rotations.of(-0.4892578125);
private static final Angle kBackRightEncoderOffset = Rotations.of(-0.490234375);
private static final boolean kBackRightSteerMotorInverted = false;
private static final boolean kBackRightEncoderInverted = false;
@@ -193,7 +210,7 @@ public class TunerConstants {
/**
* Creates a CommandSwerveDrivetrain instance.
* This should only be called once in your robot program,.
* This should only be called once in your robot program.
*/
public static CommandSwerveDrivetrain createDrivetrain() {
return new CommandSwerveDrivetrain(
src/main/java/frc/robot/subsystems/ClimberSubsystem.java
-103
View File
@@ -1,103 +0,0 @@
package frc.robot.subsystems;
import com.ctre.phoenix6.configs.MotorOutputConfigs;
import com.ctre.phoenix6.controls.DutyCycleOut;
import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.controls.VelocityDutyCycle;
import com.ctre.phoenix6.controls.VelocityVoltage;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.InvertedValue;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
public class ClimberSubsystem extends SubsystemBase {
private final TalonFX climberMotor;
private double targetPositionTicks;
private final PositionVoltage positionHoldRequest =
new PositionVoltage(0).withSlot(0);
private final VelocityVoltage velocityRequest = new VelocityVoltage(0).withSlot(0);
private double holdPositionTicks = 0.0;
private double deltaPositionTicks = 0.0;
private final NetworkTable climberTable;
public ClimberSubsystem() {
climberMotor = new TalonFX(Constants.Climber.motorID);
climberMotor.getConfigurator().apply(Constants.Climber.slot0Configs);
var cfg = new MotorOutputConfigs();
cfg.Inverted = Constants.Climber.inverted
? InvertedValue.Clockwise_Positive
: InvertedValue.CounterClockwise_Positive;
climberMotor.getConfigurator().apply(cfg);
climberTable = NetworkTableInstance.getDefault().getTable("Climber");
holdPositionTicks = getPositionTicks();
}
public void initialize() {
climberMotor.set(-0.07);
}
public void zeroEncoder(){
climberMotor.setPosition(0);
stopMotor();
}
public double getPositionTicks() {
return climberMotor.getPosition().getValueAsDouble();
}
public double getVelocityRps() {
return climberMotor.getVelocity().getValueAsDouble();
}
public void climbUp() {
targetPositionTicks = Constants.Climber.climbUpTicks;
climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks));
}
public void climbDown() {
targetPositionTicks = Constants.Climber.climbDownTicks;
climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks));
}
public void climbDownNonStop(){
deltaPositionTicks = Constants.Climber.deltaDownTicks;
climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks-deltaPositionTicks));
}
public void captureHoldPosition() {
holdPositionTicks = getPositionTicks();
}
public void holdPosition() {
climberMotor.setControl(
positionHoldRequest.withPosition(holdPositionTicks)
);
}
public void stopMotor() {
climberMotor.set(0);
}
@Override
public void periodic() {
climberTable.getEntry("velocityRps").setDouble(getVelocityRps());
climberTable.getEntry("positionTicks").setDouble(getPositionTicks());
climberTable.getEntry("holdPositionTicks").setDouble(holdPositionTicks);
}
}
src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
+143 -98
View File
@@ -1,4 +1,5 @@
package frc.robot.subsystems;
import java.util.Optional;
import java.util.function.Supplier;
@@ -18,46 +19,48 @@ import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.StructPublisher;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Subsystem;
import frc.robot.Constants;
import frc.robot.Constants.DriveMode;
import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain;
import frc.robot.Constants;
import edu.wpi.first.networktables.*;
/**
* Class that extends the Phoenix 6 SwerveDrivetrain class and implements
* 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 Rotation2d targetHeading = Rotation2d.kZero;
private DriveMode m_driveMode = DriveMode.OTHER;
private NetworkTable positioningNetworkTable;
private NetworkTable ll_shooter_NT;
private NetworkTable ll_intaker_NT;
private NetworkTable driveNetworkTable;
private final StructPublisher<Pose2d> posePub =
NetworkTableInstance.getDefault()
.getStructTopic("/Odometry/RobotPose", Pose2d.struct)
.publish();
NetworkTableInstance.getDefault()
.getStructTopic("/Odometry/RobotPose", Pose2d.struct)
.publish();
private Field2d m_field;
private Field2d shooter_ll_field;
@@ -74,20 +77,17 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
SmartDashboard.putData("Field", m_field);
SmartDashboard.putData("PositioningShooterll", shooter_ll_field);
SmartDashboard.putData("PositioningIntakerll", intaker_ll_field);
LimelightHelpers.SetIMUMode(Constants.Limelight.LIMELIGHT_NAME_Shooter, 1);
LimelightHelpers.SetIMUMode(Constants.Limelight.LIMELIGHT_NAME_Intaker, 1);
LimelightHelpers.SetIMUMode(Constants.Limelight.LIMELIGHT_NAME_Shooter, 0);
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();
@@ -96,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;
}
@@ -118,11 +118,37 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
return getState().Speeds;
}
public ChassisSpeeds getChassisSpeeds() {
// 给上层代码使用的速度别名；原机器人坐标系方法继续留给 PathPlanner。
return getRobotRelativeSpeeds();
}
public ChassisSpeeds getFieldRelativeSpeeds() {
return ChassisSpeeds.fromRobotRelativeSpeeds(getRobotRelativeSpeeds(), getRotation());
}
public Rotation2d getRotation() {
return getState().Pose.getRotation();
}
public void setTargetHeading(Rotation2d targetHeading) {
this.targetHeading = targetHeading;
}
public double getHeadingError() {
return targetHeading.minus(getRotation()).getDegrees();
}
public boolean isAimed() {
// 上层状态机只读取这个门控，不直接控制 swerve 模块。
return Math.abs(getHeadingError()) <= Constants.Superstructure.aimToleranceDegrees;
}
public void driveRobotRelative(ChassisSpeeds speeds) {
ChassisSpeeds scaled = new ChassisSpeeds(
speeds.vxMetersPerSecond, // X 不变
speeds.vyMetersPerSecond, // Y 不变
speeds.omegaRadiansPerSecond * 1.3 // 只放大旋转
speeds.vxMetersPerSecond, // X 方向速度保持原值。
speeds.vyMetersPerSecond, // Y 方向速度保持原值。
speeds.omegaRadiansPerSecond * 1.3 // 只放大旋转输出，沿用原驾驶手感。
);
setControl(
@@ -130,17 +156,32 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
);
}
public void stop() {
setControl(
new SwerveRequest.ApplyRobotSpeeds().withSpeeds(new ChassisSpeeds(0, 0, 0))
);
}
public void lockWheels() {
setControl(new SwerveRequest.SwerveDriveBrake());
}
public void aimToTarget() {
// 给未来命令预留的高层辅助接口，仍走现有底盘请求路径，不触碰模块底层逻辑。
double headingErrorDegrees = getHeadingError();
double omega = Math.toRadians(headingErrorDegrees) * Constants.AutoPositioning.TurningkP;
setControl(
new SwerveRequest.ApplyRobotSpeeds().withSpeeds(new ChassisSpeeds(0, 0, omega))
);
}
private static final double kMaxVisionJumpMeters = 1.0;
private static final double kMaxOmegaDegPerSec = 360.0;
/**
* Returns a command that applies the specified control request to this swerve drivetrain.
* 返回一个持续下发指定 swerve 请求的命令。
*
* @param request Function returning the request to apply
* @return Command to run
* @param request 生成待下发请求的函数。
* @return 可调度执行的 Command。
*/
public Command applyRequest(Supplier<SwerveRequest> request) {
return run(() -> this.setControl(request.get()));
@@ -153,10 +194,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
this.setControl(requestSupplier.get());
});
}
public void forceUsingLimelightMT2() {
public void forceUsingLimelightmt2() {
LimelightHelpers.PoseEstimate mt2 =
LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Shooter);
LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Shooter);
if (mt2 == null || mt2.tagCount == 0) {
return;
@@ -165,15 +206,27 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
var stdDevs = VecBuilder.fill(0.7, 0.7, 9999999);
addVisionMeasurement(mt2.pose, mt2.timestampSeconds, stdDevs);
}
private void fuseLimelightMT2() {
public void forceUsingLimelightmt2WithllName(String llString) {
LimelightHelpers.PoseEstimate mt2 =
LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(llString);
if (mt2 == null || mt2.tagCount == 0) {
return;
}
var stdDevs = VecBuilder.fill(0.7, 0.7, 9999999);
addVisionMeasurement(mt2.pose, mt2.timestampSeconds, stdDevs);
}
private void fuseLimelightmt2() {
Pose2d current = getState().Pose;
LimelightHelpers.PoseEstimate mt2_shooter =
getLimelightPoseEstimate(Constants.Limelight.LIMELIGHT_NAME_Shooter);
getLimelightPoseEstimate(Constants.Limelight.LIMELIGHT_NAME_Shooter);
LimelightHelpers.PoseEstimate mt2_intaker =
getLimelightPoseEstimate(Constants.Limelight.LIMELIGHT_NAME_Intaker);
getLimelightPoseEstimate(Constants.Limelight.LIMELIGHT_NAME_Intaker);
boolean shooterAvailable = ifLimelightPositioningAvailable(mt2_shooter, current);
boolean intakerAvailable = ifLimelightPositioningAvailable(mt2_intaker, current);
@@ -181,35 +234,34 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
ll_intaker_NT.getEntry("available").setBoolean(intakerAvailable);
ll_shooter_NT.getEntry("TagCount").setNumber(mt2_shooter != null ? mt2_shooter.tagCount : 0);
ll_intaker_NT.getEntry("TagCount").setNumber(mt2_intaker != null ? mt2_intaker.tagCount : 0);
LimelightHelpers.PoseEstimate mt2_final = null;
if(!shooterAvailable && !intakerAvailable) {
if (!shooterAvailable && !intakerAvailable) {
positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(false);
positioningNetworkTable.getEntry("Usingll").setString("None");
//positioningNetworkTable.getEntry("MetaTag2DistanceDifference").setDouble(Double.NaN);
// 未使用视觉时不发布距离差，避免仪表盘误读旧值。
return;
}else if (shooterAvailable && !intakerAvailable) {
} else if (shooterAvailable && !intakerAvailable) {
double shooterDistance = current.getTranslation().getDistance(mt2_shooter.pose.getTranslation());
ll_shooter_NT.getEntry("Distance").setDouble(shooterDistance);
ll_shooter_NT.getEntry("X").setDouble(mt2_shooter.pose.getX());
ll_shooter_NT.getEntry("Y").setDouble(mt2_shooter.pose.getY());
positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(true);
positioningNetworkTable.getEntry("Usingll").setString("Shooter");
//positioningNetworkTable.getEntry("MetaTag2DistanceDifference").setDouble(Double.NaN);
// 单个 Limelight 可用时直接采用该测量。
mt2_final = mt2_shooter;
}else if (!shooterAvailable && intakerAvailable) {
} else if (!shooterAvailable && intakerAvailable) {
double intakerDistance = current.getTranslation().getDistance(mt2_intaker.pose.getTranslation());
ll_intaker_NT.getEntry("Distance").setDouble(intakerDistance);
ll_intaker_NT.getEntry("X").setDouble(mt2_intaker.pose.getX());
ll_intaker_NT.getEntry("Y").setDouble(mt2_intaker.pose.getY());
positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(true);
positioningNetworkTable.getEntry("Usingll").setString("Intaker");
//positioningNetworkTable.getEntry("MetaTag2DistanceDifference").setDouble(Double.NaN);
// 单个 Limelight 可用时直接采用该测量。
mt2_final = mt2_intaker;
}else {
// 两个都可用时的策略：这里简单选更近的，你也
} else {
// 两个 Limelight 都可用时，先选择与当前位姿更接近的测量。
double shooterDistance = current.getTranslation().getDistance(mt2_shooter.pose.getTranslation());
double intakerDistance = current.getTranslation().getDistance(mt2_intaker.pose.getTranslation());
double difference = mt2_shooter.pose.getTranslation().getDistance(mt2_intaker.pose.getTranslation());
@@ -227,7 +279,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
mt2_final = shooterDistance <= intakerDistance ? mt2_shooter : mt2_intaker;
}
Pose2d mt2Pose = mt2_final.pose;
double distance = current.getTranslation().getDistance(mt2Pose.getTranslation());
@@ -240,11 +292,11 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
positioningNetworkTable.getEntry("PoseDistance").setDouble(distance);
positioningNetworkTable.getEntry("RotationDifference").setDouble(Math.abs(deltaRotation.getDegrees()));
// 4) 设置测量噪声（示例：theta 基本不信任就给极大）
// 你也可以根据 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);
}
@@ -255,30 +307,28 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
}
return LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName);
}
private boolean ifLimelightPositioningAvailable(LimelightHelpers.PoseEstimate mt2, Pose2d currentPose) {
// 1) 没看到 tag：拒绝
// 没看到 tag 时拒绝该帧视觉定位。
if (mt2 == null || mt2.tagCount == 0) {
return false;
}
// 2) 转太快：拒绝（角速度你可以从 Pigeon2 或 drivetrain state 拿）
double omegaDegPerSec = getPigeon2().getAngularVelocityZWorld().getValueAsDouble(); // 若你有这个信号
if (Math.abs(omegaDegPerSec) > kMaxOmegaDegPerSec) {
System.out.println("NoUsingMetaTag2ForTooFastRotationSpeed");
return false;
}
// 3) 跳变太大：拒绝（Phoenix6 推荐的稳定策略）
//Pose2d current = getState().Pose;
if (currentPose.getTranslation().getDistance(mt2.pose.getTranslation()) > kMaxVisionJumpMeters) {
System.out.println("NoUsingMetaTag2ForTooChangingPose");
// 旋转太快时拒绝该帧，避免高速转动时的视觉跳变。
double omegaDegPerSec = getPigeon2().getAngularVelocityZWorld().getValueAsDouble();
if (Math.abs(omegaDegPerSec) > kMaxOmegaDegPerSec) {
return false;
}
return true;
}
private double clamp(double value, double min, double max) {
return Math.max(min, Math.min(max, value));
}
@Override
public void periodic() {
Pose2d pose = getState().Pose;
m_field.setRobotPose(pose);
posePub.set(pose);
@@ -288,19 +338,22 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
driveNetworkTable.getEntry("DriveMode").setString(m_driveMode.name());
driveNetworkTable.getEntry("IsFieldCentric").setBoolean(m_driveMode == DriveMode.FIELD_CENTRIC);
// 1) 每帧喂给 LL：机器人当前 yaw（度），其余先全 0
// 保留 Positioning 表中的基础状态输出。
positioningNetworkTable.getEntry("interestPointsCl").setBoolean(false);
// 每帧向 Limelight 写入当前 yaw，供 MegaTag2 使用。
LimelightHelpers.SetRobotOrientation(
Constants.Limelight.LIMELIGHT_NAME_Shooter,
pose.getRotation().getDegrees(), // 关键：要符合 wpiBlue 约定
pose.getRotation().getDegrees(), // 符合 wpiBlue 坐标约定。
0, 0, 0, 0, 0
);
LimelightHelpers.SetRobotOrientation(
Constants.Limelight.LIMELIGHT_NAME_Intaker,
pose.getRotation().getDegrees(), // 关键：要符合 wpiBlue 约定
pose.getRotation().getDegrees(), // 符合 wpiBlue 坐标约定。
0, 0, 0, 0, 0
);
fuseLimelightMT2();
fuseLimelightmt2();
if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) {
DriverStation.getAlliance().ifPresent(allianceColor -> {
@@ -312,15 +365,13 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
m_hasAppliedOperatorPerspective = true;
});
}
}
/**
* Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
* while still accounting for measurement noise.
}
/**
* 添加一帧视觉位姿测量，用于修正里程计估计。
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param timestampSeconds The timestamp of the vision measurement in seconds.
* @param visionRobotPoseMeters 视觉测得的机器人位姿。
* @param timestampSeconds 视觉测量时间戳，单位秒。
*/
@Override
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
@@ -328,17 +379,11 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
}
/**
* Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
* while still accounting for measurement noise.
* <p>
* Note that the vision measurement standard deviations passed into this method
* will continue to apply to future measurements until a subsequent call to
* {@link #setVisionMeasurementStdDevs(Matrix)} or this method.
* 添加一帧带标准差的视觉位姿测量。
*
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
* @param 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(
@@ -350,10 +395,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
}
/**
* Return the pose at a given timestamp, if the buffer is not empty.
* 从位姿缓存中读取指定时间戳的机器人位姿。
*
* @param timestampSeconds The timestamp of the pose in seconds.
* @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) {
src/main/java/frc/robot/subsystems/ConveyorSubsystem.java
+44
View File
@@ -0,0 +1,44 @@
package frc.robot.subsystems;
import com.ctre.phoenix6.controls.VoltageOut;
import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
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 NetworkTable table = NetworkTableInstance.getDefault().getTable("Conveyor");
private double targetOutputVolts = 0.0;
public ConveyorSubsystem() {
mainConveyor = new TalonFX(Constants.Conveyor.mainConveyorID);
mainConveyor.getConfigurator().apply(Constants.Conveyor.mainConveyorSlot0Configs);
}
public void feedToShooter() {
setVoltage(Constants.Conveyor.feedVoltage);
}
public void reverse() {
setVoltage(Constants.Conveyor.reverseVoltage);
}
public void stop() {
setVoltage(0.0);
}
public void setVoltage(double outputVolts) {
targetOutputVolts = MathUtil.clamp(outputVolts, -12.0, 12.0);
mainConveyor.setControl(voltageRequest.withOutput(targetOutputVolts));
}
@Override
public void periodic() {
table.getEntry("velocity").setDouble(mainConveyor.getVelocity().getValueAsDouble());
table.getEntry("targetOutputVolts").setDouble(targetOutputVolts);
}
}
src/main/java/frc/robot/subsystems/IntakeRollerSubsystem.java
+73
View File
@@ -0,0 +1,73 @@
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.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 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 outtake() {
setVoltage(Constants.Intaker.intakeRollerOuttakeVoltage);
}
public void stop() {
setVoltage(0.0);
}
public void setVoltage(double outputVolts) {
targetOutputVolts = MathUtil.clamp(outputVolts, -12.0, 12.0);
leftIntakeRoller.setControl(voltageRequest.withOutput(targetOutputVolts));
followLeft();
}
private void followLeft() {
rightIntakeRoller.setControl(rightFollower);
}
private double getVelocityDifference() {
double leftVelocity = leftIntakeRoller.getVelocity().getValueAsDouble();
double rightVelocity = rightIntakeRoller.getVelocity().getValueAsDouble();
return Math.abs(leftVelocity) - Math.abs(rightVelocity);
}
@Override
public void periodic() {
table.getEntry("leftVelocity").setDouble(leftIntakeRoller.getVelocity().getValueAsDouble());
table.getEntry("rightVelocity").setDouble(rightIntakeRoller.getVelocity().getValueAsDouble());
table.getEntry("velocityDifference").setDouble(getVelocityDifference());
table.getEntry("targetOutputVolts").setDouble(targetOutputVolts);
}
}
src/main/java/frc/robot/subsystems/IntakeRotaterSubsystem.java
+131
View File
@@ -0,0 +1,131 @@
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 period = Constants.Intaker.intakeRotaterShootAssistPeriodSeconds;
double phase = (Timer.getFPGATimestamp() % period) / period;
double volts = phase < Constants.Intaker.intakeRotaterShootAssistUpDutyCycle
? Constants.Intaker.intakeRotaterRaiseVoltage
: Constants.Intaker.intakeRotaterLowerVoltage;
setVoltage(volts);
}
public void manualRaise() {
setVoltage(Constants.Intaker.intakeRotaterRaiseVoltage);
}
public void manualLower() {
setVoltage(Constants.Intaker.intakeRotaterLowerVoltage);
}
public void stop() {
setVoltage(0.0);
}
public boolean isRunning() {
return Math.abs(outputVoltage) > 1e-6;
}
public double getPosition() {
return leftIntakeRotater.getPosition().getValueAsDouble();
}
public double getLeftVelocity() {
return leftIntakeRotater.getVelocity().getValueAsDouble();
}
public double getRightVelocity() {
return rightIntakeRotater.getVelocity().getValueAsDouble();
}
private double getVelocityDifference() {
return Math.abs(getLeftVelocity()) - Math.abs(getRightVelocity());
}
private void setVoltage(double volts) {
outputVoltage = MathUtil.clamp(volts, -12.0, 12.0);
leftIntakeRotater.setControl(voltageRequest.withOutput(outputVoltage));
followLeft();
}
private void followLeft() {
rightIntakeRotater.setControl(rightFollower);
}
@Override
public void periodic() {
table.getEntry("position").setDouble(getPosition());
table.getEntry("isRunning").setBoolean(isRunning());
table.getEntry("outputVoltage").setDouble(outputVoltage);
table.getEntry("leftVelocity").setDouble(getLeftVelocity());
table.getEntry("rightVelocity").setDouble(getRightVelocity());
table.getEntry("velocityDifference").setDouble(getVelocityDifference());
}
}
src/main/java/frc/robot/subsystems/IntakerSubsystem.java
-67
View File
@@ -1,67 +0,0 @@
package frc.robot.subsystems;
import com.ctre.phoenix6.controls.PositionVoltage;
import com.ctre.phoenix6.controls.VelocityVoltage;
import com.ctre.phoenix6.hardware.TalonFX;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
public class IntakerSubsystem extends SubsystemBase{
//定义intakerotater&intakegetter
private TalonFX intakerotater;
private TalonFX intakegetter;
//PID
private final VelocityVoltage velocityRequest = new VelocityVoltage(0);
private final PositionVoltage positionRequest = new PositionVoltage(0);
private double intakeRotaterTargetPosition = 0.0;
public IntakerSubsystem() {
//初始化rotater&getter
intakegetter = new TalonFX(47);
intakerotater = new TalonFX(48);
intakegetter.getConfigurator().apply(Constants.Intaker.intakeGetterSlot0Configs);
intakerotater.getConfigurator().apply(Constants.Intaker.intakeRotaterSlot0Configs);
}
public void setIntakerRotaterPosition(double targetPosition) {
//目标position
if (targetPosition > Constants.Intaker.intakeRotaterUpLimit) {
targetPosition = Constants.Intaker.intakeRotaterUpLimit; // 限制最大上限
} else if (targetPosition < Constants.Intaker.intakeRotaterDownLimit) {
targetPosition = Constants.Intaker.intakeRotaterDownLimit; // 限制最小下限
}
intakeRotaterTargetPosition = targetPosition;
intakerotater.setControl(positionRequest.withPosition(targetPosition)); // 设置rotater位置（TalonFX 内部闭环 PID）
}
// 获取intakerotater当前角度
public double getIntakerotaterPosition() {
return intakerotater.getPosition().getValueAsDouble();
}
public boolean isIntakerotaterAtTarget() {
// 到位判断容差
double error = Math.abs(intakeRotaterTargetPosition - getIntakerotaterPosition());
return error <= Constants.Intaker.positionToleranceRotations;
}
public void stopintakerotater() {
intakerotater.setControl(velocityRequest.withVelocity(0));
// 如果你希望停止后保持当前位置，建议改用中性模式/或维持 positionRequest（这句fromAI）
}
//intakegetter的on/off
public void setIntakeGetterOn(boolean on) {
if (on) {
intakegetter.setControl(velocityRequest.withVelocity(Constants.Intaker.IntakeGetterSpeed));
}
else {
intakegetter.setControl(velocityRequest.withVelocity(0));
}
}
}
src/main/java/frc/robot/subsystems/LEDSubsystem.java
+12 -5
View File
@@ -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);
src/main/java/frc/robot/subsystems/LimelightSubsystem.java
+54
View File
@@ -0,0 +1,54 @@
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;
/**
* 对现有 LimelightHelpers 的薄只读封装。
*
* 本类不做视觉融合，也不控制任何机构；只给 Superstructure 提供稳定的
* 目标和距离读取接口。
*/
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 已经把基于场地位姿计算的距离发布到 AutoControl；
// 这里复用该值，保留旧定位和目标计算逻辑。
return NetworkTableInstance.getDefault()
.getTable("AutoControl")
.getEntry("distanceToHub")
.getDouble(0.0);
}
public Pose2d getEstimatedPose() {
return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName).pose;
}
public double getShooterVoltageByDistance() {
// 查 Constants 里的插值射表；真车试射后往表里加实测点即可。
return Constants.Shooter.flywheelVoltageByDistance.get(getDistanceToTarget());
}
}
src/main/java/frc/robot/subsystems/ShooterSubsystem.java
+272 -97
View File
@@ -3,165 +3,340 @@ 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.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 edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.Encoder;
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;
/**
* 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 VoltageOut leftFlywheelVoltageRequest = new VoltageOut(0);
private final VoltageOut rightFlywheelVoltageRequest = new VoltageOut(0);
private final VoltageOut conveyorVoltageRequest = new VoltageOut(0);
private final VoltageOut backboardVoltageRequest = new VoltageOut(0);
private final VelocityDutyCycle velocityRequest = new VelocityDutyCycle(0);
private final Follower conveyorFollower;
private final NetworkTable shooterNetworkTable;
private final NetworkTable backboardTuningTable;
private double flywheelOutputVolts = 0.0;
private double flywheelVoltageStartTimestamp = 0.0;
private double shooterConveyorOutputVolts = 0.0;
private double backboardOutputVolts = 0.0;
private double backboardRawPidOutputVolts = 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);
var configs2 = new MotorOutputConfigs();
configs2.Inverted = InvertedValue.Clockwise_Positive;
conveyorMotor.getConfigurator().apply(configs2);
// 初始化背板电机和编码器
backboardMotor = new TalonFXS(44);
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 motorOutputConfigs = new MotorOutputConfigs();
motorOutputConfigs.Inverted = InvertedValue.Clockwise_Positive;
leftFlywheel.getConfigurator().apply(motorOutputConfigs);
leftConveyor.getConfigurator().apply(motorOutputConfigs);
conveyorFollower = new Follower(leftConveyor.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50);
setRightConveyorFollowLeft();
backboardMotor = new TalonFXS(Constants.Shooter.backboardMotorID);
backboardMotor.getConfigurator().apply(Constants.Shooter.backboardSlot0Configs);
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.setTolerance(10.0);
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");
backboardTuningTable = NetworkTableInstance.getDefault().getTable("Tuning/backboard");
backboardTuningTable.getEntry("backboardPositionEnable").setDefaultBoolean(false);
backboardTuningTable.getEntry("backboardTargetPosition").setDefaultDouble(0.0);
}
public void setRightFollowLeft(){
flywheelMotorRight.setControl(flyWheelFollower);
/* ====================== */
/* 飞轮 */
/* ====================== */
public void setRightConveyorFollowLeft() {
// 成对机构机械方向相反，因此右侧电机以反向 follower 跟随左侧电机。
rightConveyor.setControl(conveyorFollower);
}
public void applyLeftConfigurationToRight(){
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 setFlywheelVoltage(double outputVolts) {
double clampedOutputVolts = MathUtil.clamp(outputVolts, -12.0, 12.0);
if (Math.abs(clampedOutputVolts - flywheelOutputVolts) > 1e-6) {
flywheelVoltageStartTimestamp = Timer.getFPGATimestamp();
}
flywheelOutputVolts = clampedOutputVolts;
leftFlywheel.setControl(leftFlywheelVoltageRequest.withOutput(flywheelOutputVolts));
rightFlywheel.setControl(rightFlywheelVoltageRequest.withOutput(-flywheelOutputVolts));
}
// 设置传动的速度
public void setConveyorSpeedByRPS(double speed) {
conveyorMotor.setControl(velocityRequest.withVelocity(speed)); // 设置传动电机速度
public void setFlywheelVoltageByDistance(double distance) {
double targetVolts = Constants.Shooter.flywheelVoltageByDistance.get(distance);
setFlywheelVoltage(targetVolts);
}
//调试用
public void setBackboardSpeedByRPS(double speed){
backboardMotor.setControl(velocityRequest.withVelocity(speed));
public void stopFlywheel() {
setFlywheelVoltage(0.0);
}
public boolean atVoltage() {
if (Math.abs(flywheelOutputVolts) < Constants.Shooter.flywheelReadyVoltageThreshold) {
return false;
}
return Timer.getFPGATimestamp() - flywheelVoltageStartTimestamp
>= Constants.Shooter.flywheelReadyDelaySeconds;
}
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 getTargetFlywheelVoltage() {
return flywheelOutputVolts;
}
/* ====================== */
/* shooter conveyor */
/* ====================== */
public void runShooterConveyorVoltage(double outputVolts) {
shooterConveyorOutputVolts = MathUtil.clamp(outputVolts, -12.0, 12.0);
leftConveyor.setControl(conveyorVoltageRequest.withOutput(shooterConveyorOutputVolts));
setRightConveyorFollowLeft();
}
public void stopShooterConveyor() {
runShooterConveyorVoltage(0.0);
}
public double getConveyorStatorCurrent() {
return leftConveyor.getStatorCurrent().getValueAsDouble();
}
/* ====================== */
/* 背板 */
/* ====================== */
public void setBackboardVoltage(double outputVolts) {
backboardOutputVolts = MathUtil.clamp(
outputVolts,
-Constants.Shooter.backboardSpeedMax,
Constants.Shooter.backboardSpeedMax
);
backboardMotor.setControl(backboardVoltageRequest.withOutput(backboardOutputVolts));
}
// 设置背板的目标位置，通过PID控制器来调整 范围0-3000
public void setBackboardPosition(double targetPosition) {
// 限制目标位置在上下限之间
// 保留旧背板机构的软件限位。
if (targetPosition > Constants.Shooter.backboardUpLimit) {
targetPosition = Constants.Shooter.backboardUpLimit; // 限制最大上限
targetPosition = Constants.Shooter.backboardUpLimit;
} else if (targetPosition < Constants.Shooter.backboardDownLimit) {
targetPosition = Constants.Shooter.backboardDownLimit; // 限制最小下限
targetPosition = Constants.Shooter.backboardDownLimit;
}
backboardPID.setGoal(targetPosition);
}
public void outputBackboard(){
// 使用PID控制器来计算背板的运动速度
double pidOutput = backboardPID.calculate(getBackboardPosition());
pidOutput = Math.max(-Constants.Shooter.backboardSpeedMax, Math.min(Constants.Shooter.backboardSpeedMax, pidOutput)); // 限制输出范围
if (Math.abs(pidOutput) < 0.8) {
pidOutput = 0.0;
}//防止抖动
// // 设置背板电机的速度，根据PID输出进行调整
public void outputBackboard() {
// 背板仍使用旧外部编码器和 ProfiledPID；Superstructure 只选择目标位置。
double rawPidOutput = backboardPID.calculate(getBackboardPosition());
double pidOutput = MathUtil.clamp(
rawPidOutput,
-Constants.Shooter.backboardSpeedMax,
Constants.Shooter.backboardSpeedMax
);
backboardRawPidOutputVolts = rawPidOutput;
backboardClampedPidOutputVolts = pidOutput;
backboardOutputSaturated = Math.abs(rawPidOutput - pidOutput) > 1e-9;
if (pidOutput != 0.0 && Math.abs(pidOutput) < Constants.Shooter.backboardSpeedMin) {
pidOutput = Math.copySign(Constants.Shooter.backboardSpeedMin, pidOutput);
backboardMinimumOutputActive = true;
} else {
backboardMinimumOutputActive = false;
}
shooterNetworkTable.getEntry("backboardPIDOutput").setDouble(pidOutput);
setBackboardSpeedByRPS(pidOutput);
setBackboardVoltage(pidOutput);
}
// 停止所有电机
public void stopMotors() {
flywheelMotorLeft.set(0);
flywheelMotorRight.set(0);
conveyorMotor.set(0);
backboardMotor.set(0);
public void holdBackboardAt(double targetPosition) {
// 上层只给目标位置；到位后停止输出，未到位时继续闭环。
setBackboardPosition(targetPosition);
if (isBackboardAtTarget()) {
backboardRawPidOutputVolts = 0.0;
backboardClampedPidOutputVolts = 0.0;
backboardOutputSaturated = false;
backboardMinimumOutputActive = false;
setBackboardVoltage(0.0);
} else {
outputBackboard();
}
}
public void stopBackboard() {
backboardRawPidOutputVolts = 0.0;
backboardClampedPidOutputVolts = 0.0;
backboardOutputSaturated = false;
backboardMinimumOutputActive = false;
setBackboardVoltage(0.0);
}
// 获取背板当前角度
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 void resetbackboardencoder() {
backboardEncoder.reset();
backboardPID.reset(0);
}
public void conveyorRun(){
setConveyorSpeedByRPS(Constants.Shooter.conveyorSpeed);
/**
* 背板回零命令：闭环回到 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 positionEnable = 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);
}
}
@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;
shooterNetworkTable.getEntry("flywheelVelocity").setDouble(getAverageFlywheelVelocity());
shooterNetworkTable.getEntry("leftFlywheelVelocity").setDouble(getLeftFlywheelVelocity());
shooterNetworkTable.getEntry("rightFlywheelVelocity").setDouble(getRightFlywheelVelocity());
shooterNetworkTable.getEntry("flywheelVelocityDifference").setDouble(getFlywheelVelocityDifference());
shooterNetworkTable.getEntry("flywheelTargetVoltage").setDouble(getTargetFlywheelVoltage());
shooterNetworkTable.getEntry("flywheelOutputVolts").setDouble(flywheelOutputVolts);
shooterNetworkTable.getEntry("flywheelAtVoltage").setBoolean(atVoltage());
shooterNetworkTable.getEntry("shooterConveyorOutputVolts").setDouble(shooterConveyorOutputVolts);
shooterNetworkTable.getEntry("conveyorVelocity").setDouble(leftConveyor.getVelocity().getValueAsDouble());
shooterNetworkTable.getEntry("backboardCurrentRate").setDouble(backboardPosition);
shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardSetpointPosition);
shooterNetworkTable.getEntry("backboardOutputVolts").setDouble(backboardOutputVolts);
shooterNetworkTable.getEntry("isBackboardAtTarget").setBoolean(isBackboardAtTarget());
shooterNetworkTable.getEntry("distanceGetted").setDouble(getDistance());
}
public double getDistance(){
//获得车子距离Hub的距离
return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToHub").getDouble(0.0);
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("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("backboardPositionTolerance").setDouble(10.0);
shooterNetworkTable.getEntry("backboardShootPosition").setDouble(Constants.Superstructure.backboardShootPosition);
shooterNetworkTable.getEntry("backboardPassPosition").setDouble(Constants.Superstructure.passBallBackboardPosition);
shooterNetworkTable.getEntry("backboardMotorPosition").setDouble(backboardMotor.getPosition().getValueAsDouble());
shooterNetworkTable.getEntry("backboardMotorVelocity").setDouble(backboardMotor.getVelocity().getValueAsDouble());
shooterNetworkTable.getEntry("backboardMotorVoltage").setDouble(backboardMotor.getMotorVoltage().getValueAsDouble());
shooterNetworkTable.getEntry("backboardMotorStatorCurrent").setDouble(backboardMotor.getStatorCurrent().getValueAsDouble());
shooterNetworkTable.getEntry("statorCurrent").setDouble(getConveyorStatorCurrent());
}
}
src/main/java/frc/robot/subsystems/Superstructure.java
+356
View File
@@ -0,0 +1,356 @@
package frc.robot.subsystems;
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 {
IDLE,
INTAKE,
SHOOT_HUB,
AIM_HUB,
PASS_BALL,
EJECT,
MANUAL
}
// 机器人内部实际执行状态，用于动作排序和仪表盘诊断。
public enum SystemState {
DISABLED,
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 flywheelVoltageOffset = 0.0;
private double backboardPositionOffset = 0.0;
private boolean shotCompletedThisRequest = false;
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() {
wantedState = WantedState.IDLE;
}
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 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() {
boolean stationaryGate = !Constants.Superstructure.stationaryGateEnabled || isDriveStationary();
return shooter.atVoltage()
&& drive.isAimed()
&& stationaryGate;
}
private boolean isDriveStationary() {
ChassisSpeeds speeds = drive.getChassisSpeeds();
double translationSpeed = Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond);
return translationSpeed <= Constants.Superstructure.stationarySpeedToleranceMetersPerSecond;
}
/* ====================== */
/* 现场微调 offset */
/* ====================== */
public void adjustFlywheelVoltageOffset(double offsetVolts) {
flywheelVoltageOffset += offsetVolts;
}
public void adjustBackboardPositionOffset(double offset) {
backboardPositionOffset += offset;
}
public void resetOffsets() {
flywheelVoltageOffset = 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 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 handleIdle() {
// 安全默认状态：球路不动，intaker 不主动动作，背板保持受控。
conveyor.stop();
intakeRoller.stop();
intakeRotater.stop();
shooter.stopFlywheel();
shooter.stopShooterConveyor();
shooter.holdBackboardAt(0.0);
setSystemState(SystemState.STOWED);
}
private void handleIntake() {
shooter.stopFlywheel();
shooter.stopShooterConveyor();
shooter.holdBackboardAt(0.0);
// Intaker 比赛开始后保持下放，收球何时结束由操作员松开按键决定。
intakeRoller.intake();
conveyor.feedToShooter();
setSystemState(SystemState.INTAKING);
}
private void handleAimHub() {
intakeRoller.stop();
conveyor.stop();
shooter.stopFlywheel();
shooter.stopShooterConveyor();
shooter.holdBackboardAt(0.0);
setSystemState(SystemState.STOWED);
}
private void handleShootHub() {
double targetVolts = limelight.getShooterVoltageByDistance() + flywheelVoltageOffset;
double backboardPosition = Constants.Superstructure.backboardShootPosition + backboardPositionOffset;
handlePreparedFeed(targetVolts, backboardPosition);
}
private void handlePassBall() {
double targetVolts = Constants.Superstructure.passBallFlywheelVoltage + flywheelVoltageOffset;
double backboardPosition = Constants.Superstructure.passBallBackboardPosition + backboardPositionOffset;
handlePreparedFeed(targetVolts, backboardPosition);
}
private void handlePreparedFeed(double targetVolts, double backboardPosition) {
if (systemState != SystemState.PREP_SHOOT
&& systemState != SystemState.SHOOTING) {
setSystemState(SystemState.PREP_SHOOT);
}
intakeRoller.stop();
conveyor.stop();
shooter.stopShooterConveyor();
// 距离到射速的映射由 LimelightSubsystem 提供；操作员 offset 用于现场微调，
// 但不会绕过 Superstructure 状态机。
shooter.setFlywheelVoltage(targetVolts);
shooter.holdBackboardAt(backboardPosition);
if (systemState == SystemState.SHOOTING) {
// 只有 canShoot() 通过后才会进入本状态；喂球期间不再复查门控，
// 避免球接触飞轮导致的掉速中断喂球。
intakeRotater.shootAssist();
conveyor.feedToShooter();
shooter.runShooterConveyorVoltage(Constants.Superstructure.shooterFeedVoltage);
if (timeInState() > Constants.Superstructure.shootTimeoutSeconds) {
shotCompletedThisRequest = true;
setSystemState(SystemState.PREP_SHOOT);
}
return;
}
// 未开始喂球前，球路保持静止。
conveyor.stop();
shooter.stopShooterConveyor();
// 门控条件实时刷新：条件回落时状态同步回退，仪表盘能看到卡在哪一关。
if (canShoot()) {
setSystemState(SystemState.SHOOTING);
}
}
private void handleEject() {
// Eject 反转整条球路，飞轮保持停止。
setSystemState(SystemState.EJECTING);
intakeRoller.outtake();
conveyor.reverse();
shooter.runShooterConveyorVoltage(Constants.Superstructure.shooterReverseVoltage);
shooter.stopFlywheel();
shooter.holdBackboardAt(0.0);
}
private void handleManual() {
// 手动模式：状态机不再驱动机构，交给现场手动/调参工具；
// 只保持飞轮与喂球停止，避免意外射出。
setSystemState(SystemState.MANUAL_OVERRIDE);
shooter.stopFlywheel();
shooter.stopShooterConveyor();
conveyor.stop();
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();
}
}
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("shooterAtVoltage").setBoolean(shooter.atVoltage());
table.getEntry("driveAimed").setBoolean(drive.isAimed());
table.getEntry("driveHeadingError").setDouble(drive.getHeadingError());
table.getEntry("driveStationary").setBoolean(isDriveStationary());
table.getEntry("flywheelVoltageOffset").setDouble(flywheelVoltageOffset);
table.getEntry("backboardPositionOffset").setDouble(backboardPositionOffset);
}
}
src/main/java/frc/robot/tuning/ConfigTalonFXMotorTuner.java
+10 -9
View File
@@ -2,8 +2,9 @@ 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 {
@@ -22,10 +23,10 @@ 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 final VelocityDutyCycle velocityRequest = new VelocityDutyCycle(0);
private final VoltageOut voltageRequest = new VoltageOut(0);
public ConfigTalonFXMotorTuner(TalonFX motor_, String motorName_, Slot0Configs motorSlot0Configs_) {
this.motor = motor_;
this.motorName = motorName_;
@@ -42,7 +43,7 @@ public class ConfigTalonFXMotorTuner {
table.getDoubleTopic(motorName+"kS").publish().set(motorSlot0Configs.kS);
table.getBooleanTopic(motorName+"EnableRunning").publish().set(false);;
table.getDoubleTopic(motorName+"RunningSpeedRPS").publish().set(0);;
table.getDoubleTopic(motorName+"RunningVolts").publish().set(0);;
this.enableSub = table.getBooleanTopic(motorName+"Enable").subscribe(false);
this.KpSub = table.getDoubleTopic(motorName+"kP").subscribe(motorSlot0Configs.kP);
@@ -52,7 +53,7 @@ public class ConfigTalonFXMotorTuner {
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.runningVolts = table.getDoubleTopic(motorName+"RunningVolts").subscribe(0);
}
public void periodic(Runnable func) {
@@ -80,11 +81,11 @@ 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);
double volts = MathUtil.clamp(runningVolts.get(), -12.0, 12.0);
motor.setControl(voltageRequest.withOutput(volts));
// System.out.println("running motor with volts: "+volts+" on motor: "+motorName);
}else{
motor.setControl(velocityRequest.withVelocity(0));
motor.setControl(voltageRequest.withOutput(0));
}
}
src/main/java/frc/robot/tuning/ConfigTalonFXSMotorTuner.java
-1
View File
@@ -1,7 +1,6 @@
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.*;
src/main/java/frc/robot/utils/GameData2026.java
+117
View File
@@ -0,0 +1,117 @@
package frc.robot.utils;
import java.util.Optional;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
public class GameData2026 {
private static final double SHIFT_1_END_TIME = 105.0;
private static final double SHIFT_2_END_TIME = 80.0;
private static final double SHIFT_3_END_TIME = 55.0;
private static final double SHIFT_4_END_TIME = 30.0;
private static final double TRANSITION_END_TIME = 130.0;
private final NetworkTable table = NetworkTableInstance.getDefault().getTable("GameData2026");
public void periodic() {
Optional<Alliance> alliance = DriverStation.getAlliance();
double matchTime = DriverStation.getMatchTime();
String gameData = DriverStation.getGameSpecificMessage();
HubWindow window = calculateHubWindow(alliance, matchTime, gameData);
table.getEntry("HubActive").setBoolean(window.hubActive);
table.getEntry("HubStatus").setString(window.status);
table.getEntry("SecondsUntilHubOpens").setDouble(window.secondsUntilHubOpens);
table.getEntry("SecondsUntilHubChanges").setDouble(window.secondsUntilHubChanges);
table.getEntry("Shift").setString(window.shift);
table.getEntry("MatchTime").setDouble(matchTime);
table.getEntry("GameData").setString(gameData);
table.getEntry("Alliance").setString(alliance.map(Enum::name).orElse("Unknown"));
table.getEntry("FirstInactiveAlliance").setString(getFirstInactiveAlliance(gameData));
}
private HubWindow calculateHubWindow(Optional<Alliance> alliance, double matchTime, String gameData) {
if (alliance.isEmpty()) {
return new HubWindow(false, "NO_ALLIANCE", "Disabled", Double.NaN, Double.NaN);
}
if (DriverStation.isAutonomousEnabled()) {
return new HubWindow(true, "OPEN", "Auto", 0.0, Double.NaN);
}
if (!DriverStation.isTeleopEnabled()) {
return new HubWindow(false, "DISABLED", "Disabled", Double.NaN, Double.NaN);
}
if (matchTime < 0.0) {
return new HubWindow(true, "OPEN_TIME_UNKNOWN", "Unknown", 0.0, Double.NaN);
}
Boolean redInactiveFirst = getRedInactiveFirst(gameData);
if (redInactiveFirst == null) {
return new HubWindow(true, "WAITING_DATA", "WaitingData", 0.0, Double.NaN);
}
boolean shift1Active = switch (alliance.get()) {
case Red -> !redInactiveFirst;
case Blue -> redInactiveFirst;
};
if (matchTime > TRANSITION_END_TIME) {
return new HubWindow(true, "OPEN", "Transition", 0.0, matchTime - TRANSITION_END_TIME);
} else if (matchTime > SHIFT_1_END_TIME) {
return fromShift("Shift1", shift1Active, matchTime, SHIFT_1_END_TIME);
} else if (matchTime > SHIFT_2_END_TIME) {
return fromShift("Shift2", !shift1Active, matchTime, SHIFT_2_END_TIME);
} else if (matchTime > SHIFT_3_END_TIME) {
return fromShift("Shift3", shift1Active, matchTime, SHIFT_3_END_TIME);
} else if (matchTime > SHIFT_4_END_TIME) {
return fromShift("Shift4", !shift1Active, matchTime, SHIFT_4_END_TIME);
} else {
return new HubWindow(true, "OPEN", "Endgame", 0.0, Math.max(matchTime, 0.0));
}
}
private HubWindow fromShift(String shift, boolean hubActive, double matchTime, double nextBoundary) {
double secondsUntilChange = Math.max(matchTime - nextBoundary, 0.0);
double secondsUntilOpen = hubActive ? 0.0 : secondsUntilChange;
String status = hubActive ? "OPEN" : "CLOSED";
return new HubWindow(hubActive, status, shift, secondsUntilOpen, secondsUntilChange);
}
private Boolean getRedInactiveFirst(String gameData) {
if (gameData.isEmpty()) {
return null;
}
return switch (gameData.charAt(0)) {
case 'R' -> true;
case 'B' -> false;
default -> null;
};
}
private String getFirstInactiveAlliance(String gameData) {
if (gameData.isEmpty()) {
return "None";
}
return switch (gameData.charAt(0)) {
case 'R' -> "Red";
case 'B' -> "Blue";
default -> "Invalid";
};
}
private record HubWindow(
boolean hubActive,
String status,
String shift,
double secondsUntilHubOpens,
double secondsUntilHubChanges
) {}
}