diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 5045b50..73f69d9 100644 --- a/.github/workflows/main.yml +++ b/.github/workflows/main.yml @@ -4,9 +4,9 @@ name: CI # events but only for the main branch. on: push: - branches: [ main, District-1, District-2, States, Worlds, iri ] + branches: [ main, District-1, District-2, dcmp, States, Worlds, iri ] pull_request: - branches: [ main, District-1, District-2, States, Worlds, iri ] + branches: [ main, District-1, District-2, dcmp, States, Worlds, iri ] # A workflow run is made up of one or more jobs that can run sequentially or in parallel jobs: # This workflow contains a single job called "build" diff --git a/CompBot/simgui-ds.json b/CompBot/simgui-ds.json index 3140247..2ef491b 100644 --- a/CompBot/simgui-ds.json +++ b/CompBot/simgui-ds.json @@ -1,9 +1,4 @@ { - "Joysticks": { - "window": { - "visible": false - } - }, "System Joysticks": { "window": { "visible": false @@ -14,14 +9,17 @@ "axisConfig": [ { "decKey": 83, + "decayRate": 1.0, "incKey": 87 }, { "decKey": 68, + "decayRate": 1.0, "incKey": 65 }, { "decKey": 93, + "decayRate": 1.0, "incKey": 91 }, { diff --git a/CompBot/src/main/deploy/pathplanner/autos/Alt Left Depot.auto b/CompBot/src/main/deploy/pathplanner/autos/Alt Right.auto.bak similarity index 100% rename from CompBot/src/main/deploy/pathplanner/autos/Alt Left Depot.auto rename to CompBot/src/main/deploy/pathplanner/autos/Alt Right.auto.bak diff --git a/CompBot/src/main/deploy/pathplanner/autos/FLIP Alt Inverse.auto b/CompBot/src/main/deploy/pathplanner/autos/FLIP Alt Inverse.auto new file mode 100644 index 0000000..0a407b2 --- /dev/null +++ b/CompBot/src/main/deploy/pathplanner/autos/FLIP Alt Inverse.auto @@ -0,0 +1,44 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim" + } + }, + { + "type": "path", + "data": { + "pathName": "Alt" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Inverse Right" + } + }, + { + "type": "named", + "data": { + "name": "Aim" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/autos/FLIP Double.auto b/CompBot/src/main/deploy/pathplanner/autos/FLIP Alt Single.auto similarity index 91% rename from CompBot/src/main/deploy/pathplanner/autos/FLIP Double.auto rename to CompBot/src/main/deploy/pathplanner/autos/FLIP Alt Single.auto index 8714de4..069352d 100644 --- a/CompBot/src/main/deploy/pathplanner/autos/FLIP Double.auto +++ b/CompBot/src/main/deploy/pathplanner/autos/FLIP Alt Single.auto @@ -4,26 +4,20 @@ "type": "sequential", "data": { "commands": [ - { - "type": "path", - "data": { - "pathName": "Right" - } - }, { "type": "race", "data": { "commands": [ { - "type": "named", + "type": "wait", "data": { - "name": "Aim" + "waitTime": 0.5 } }, { - "type": "wait", + "type": "named", "data": { - "waitTime": 10.0 + "name": "Aim" } } ] @@ -32,7 +26,13 @@ { "type": "path", "data": { - "pathName": "Right Cleanup" + "pathName": "Alt" + } + }, + { + "type": "path", + "data": { + "pathName": "Right" } }, { diff --git a/CompBot/src/main/deploy/pathplanner/autos/FLIP Far.auto.bak b/CompBot/src/main/deploy/pathplanner/autos/FLIP Far.auto.bak new file mode 100644 index 0000000..7d357d8 --- /dev/null +++ b/CompBot/src/main/deploy/pathplanner/autos/FLIP Far.auto.bak @@ -0,0 +1,38 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "named", + "data": { + "name": "Aim" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "FLIP Crash" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy Inverse.auto b/CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy Inverse.auto new file mode 100644 index 0000000..fba90f7 --- /dev/null +++ b/CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy Inverse.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Loopy to Inverse" + } + }, + { + "type": "named", + "data": { + "name": "Aim" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy Single-side.auto b/CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy Single-side.auto new file mode 100644 index 0000000..b42fe88 --- /dev/null +++ b/CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy Single-side.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Loopy Single-side" + } + }, + { + "type": "named", + "data": { + "name": "Aim" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy.auto b/CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy.auto new file mode 100644 index 0000000..02d734f --- /dev/null +++ b/CompBot/src/main/deploy/pathplanner/autos/FLIP Loopy.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Loopy" + } + }, + { + "type": "named", + "data": { + "name": "Aim" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/autos/FLIP Alt Double.auto b/CompBot/src/main/deploy/pathplanner/autos/FLIP Single + Auto-beach.auto similarity index 71% rename from CompBot/src/main/deploy/pathplanner/autos/FLIP Alt Double.auto rename to CompBot/src/main/deploy/pathplanner/autos/FLIP Single + Auto-beach.auto index 02ecbc7..dc33196 100644 --- a/CompBot/src/main/deploy/pathplanner/autos/FLIP Alt Double.auto +++ b/CompBot/src/main/deploy/pathplanner/autos/FLIP Single + Auto-beach.auto @@ -4,12 +4,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "path", - "data": { - "pathName": "Alt" - } - }, { "type": "path", "data": { @@ -29,7 +23,7 @@ { "type": "wait", "data": { - "waitTime": 10.0 + "waitTime": 9.0 } } ] @@ -38,13 +32,7 @@ { "type": "path", "data": { - "pathName": "Right Cleanup" - } - }, - { - "type": "named", - "data": { - "name": "Aim" + "pathName": "Auto-beach" } } ] diff --git a/CompBot/src/main/deploy/pathplanner/autos/FLIP Single.auto b/CompBot/src/main/deploy/pathplanner/autos/FLIP Single.auto new file mode 100644 index 0000000..feb216f --- /dev/null +++ b/CompBot/src/main/deploy/pathplanner/autos/FLIP Single.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right" + } + }, + { + "type": "named", + "data": { + "name": "Aim" + } + } + ] + } + }, + "resetOdom": false, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/autos/Flip Inverse.auto b/CompBot/src/main/deploy/pathplanner/autos/Flip Inverse.auto new file mode 100644 index 0000000..aa17f33 --- /dev/null +++ b/CompBot/src/main/deploy/pathplanner/autos/Flip Inverse.auto @@ -0,0 +1,44 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "named", + "data": { + "name": "Aim" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Inverse Right" + } + }, + { + "type": "named", + "data": { + "name": "Aim" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/autos/Left Depot.auto b/CompBot/src/main/deploy/pathplanner/autos/Left Depot.auto.bak similarity index 100% rename from CompBot/src/main/deploy/pathplanner/autos/Left Depot.auto rename to CompBot/src/main/deploy/pathplanner/autos/Left Depot.auto.bak diff --git a/CompBot/src/main/deploy/pathplanner/autos/Loop-de-doo.auto b/CompBot/src/main/deploy/pathplanner/autos/Loopy Depot.auto.bak similarity index 96% rename from CompBot/src/main/deploy/pathplanner/autos/Loop-de-doo.auto rename to CompBot/src/main/deploy/pathplanner/autos/Loopy Depot.auto.bak index d7649cd..07f1f93 100644 --- a/CompBot/src/main/deploy/pathplanner/autos/Loop-de-doo.auto +++ b/CompBot/src/main/deploy/pathplanner/autos/Loopy Depot.auto.bak @@ -26,7 +26,7 @@ { "type": "path", "data": { - "pathName": "Center Loop-de-doo" + "pathName": "Loopy" } }, { diff --git a/CompBot/src/main/deploy/pathplanner/autos/Right Depot.auto.bak b/CompBot/src/main/deploy/pathplanner/autos/Right Depot.auto.bak new file mode 100644 index 0000000..d439020 --- /dev/null +++ b/CompBot/src/main/deploy/pathplanner/autos/Right Depot.auto.bak @@ -0,0 +1,63 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Right" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Aim" + } + }, + { + "type": "wait", + "data": { + "waitTime": 10.0 + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Right Depot" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 5.0 + } + }, + { + "type": "named", + "data": { + "name": "Aim" + } + } + ] + } + } + ] + } + }, + "resetOdom": false, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/autos/Simple Middle Depot.auto.bak b/CompBot/src/main/deploy/pathplanner/autos/Simple Middle Depot.auto similarity index 100% rename from CompBot/src/main/deploy/pathplanner/autos/Simple Middle Depot.auto.bak rename to CompBot/src/main/deploy/pathplanner/autos/Simple Middle Depot.auto diff --git a/CompBot/src/main/deploy/pathplanner/paths/Alt.path b/CompBot/src/main/deploy/pathplanner/paths/Alt.path index 692e33c..2d1cf35 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Alt.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Alt.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 3.4290240963855423, + "x": 3.5929397590361445, "y": 5.317665405269765 }, "prevControl": null, "nextControl": { - "x": 2.693437945791726, - "y": 4.85660485021398 + "x": 2.2925421686746983, + "y": 4.821795180722892 }, "isLocked": false, "linkedName": "Left Start" }, { "anchor": { - "x": 3.4361022727272723, + "x": 3.5929397590361445, "y": 2.4168295454545454 }, "prevControl": { - "x": 2.7969472182596284, - "y": 3.1616405135520687 + "x": 2.4783132530120486, + "y": 3.0624337349397592 }, "nextControl": null, "isLocked": false, @@ -31,9 +31,16 @@ "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "Aim", + "waypointRelativePos": 0.1121588089329983, + "endWaypointRelativePos": 1.0, + "command": null + } + ], "globalConstraints": { - "maxVelocity": 4.0, + "maxVelocity": 1.5, "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -42,13 +49,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 40.23685706767951 + "rotation": 0.0 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -62.58308037431244 + "rotation": 0.8493895342646307 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Auto-beach.path b/CompBot/src/main/deploy/pathplanner/paths/Auto-beach.path new file mode 100644 index 0000000..437ac87 --- /dev/null +++ b/CompBot/src/main/deploy/pathplanner/paths/Auto-beach.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.5766626506024095, + "y": 2.4168295454545454 + }, + "prevControl": null, + "nextControl": { + "x": 3.7676116563257303, + "y": 3.1098858773181166 + }, + "isLocked": false, + "linkedName": "Right 2 Start" + }, + { + "anchor": { + "x": 8.25706134094151, + "y": 2.566462196861627 + }, + "prevControl": { + "x": 6.691483594864478, + "y": 1.8418972895863055 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -44.2926806314556 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -41.30950981274552 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Center Loop-de-doo.path b/CompBot/src/main/deploy/pathplanner/paths/Center Loop-de-loo.path similarity index 84% rename from CompBot/src/main/deploy/pathplanner/paths/Center Loop-de-doo.path rename to CompBot/src/main/deploy/pathplanner/paths/Center Loop-de-loo.path index 5df84c2..f230ee5 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Center Loop-de-doo.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Center Loop-de-loo.path @@ -8,24 +8,24 @@ }, "prevControl": null, "nextControl": { - "x": 3.58620542082739, - "y": 2.6052781740370894 + "x": 0.41297590361445735, + "y": 4.155204819277109 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.0057346647646215, - "y": 2.4888302425106996 + "x": 7.668975903614458, + "y": 2.3193493975903605 }, "prevControl": { - "x": 4.0637691044309765, - "y": 2.0343276645602733 + "x": 3.0137710843373497, + "y": 1.3467831325301218 }, "nextControl": { - "x": 6.613851640513552, - "y": 2.631155492154066 + "x": 8.280326416522545, + "y": 2.4470728615547257 }, "isLocked": false, "linkedName": null @@ -36,12 +36,12 @@ "y": 5.594108416547789 }, "prevControl": { - "x": 7.259038516478155, - "y": 4.6988913796095435 + "x": 4.019120481927711, + "y": 4.8108674698795175 }, "nextControl": { - "x": 5.7340228245363765, - "y": 5.788188302425108 + "x": 6.316371263922148, + "y": 5.716579758037213 }, "isLocked": false, "linkedName": null @@ -52,7 +52,7 @@ "y": 5.317665405269765 }, "prevControl": { - "x": 4.852289389845073, + "x": 4.852289389845074, "y": 5.874027744784743 }, "nextControl": null, diff --git a/CompBot/src/main/deploy/pathplanner/paths/Depot.path b/CompBot/src/main/deploy/pathplanner/paths/Depot.path index 861c3db..cc3f10e 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Depot.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Depot.path @@ -8,32 +8,73 @@ }, "prevControl": null, "nextControl": { - "x": 1.8974401712548474, - "y": 5.639258950356227 + "x": 2.6094457831325304, + "y": 6.144048192771083 }, "isLocked": false, "linkedName": "Left 2 Start" }, { "anchor": { - "x": 0.5878192771084336, - "y": 5.969204819277109 + "x": 1.3309036144578312, + "y": 7.466301204819277 }, "prevControl": { - "x": 1.894745611013466, - "y": 5.719045481309168 + "x": 2.637829948362863, + "y": 7.216141866851336 + }, + "nextControl": { + "x": 0.023977280552799574, + "y": 7.7164605427872175 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.5003975903614455, + "y": 5.317665405269765 + }, + "prevControl": { + "x": 0.5331807228915659, + "y": 6.078481927710843 + }, + "nextControl": { + "x": 0.48963522528803627, + "y": 5.067897169703216 }, - "nextControl": null, "isLocked": false, "linkedName": null + }, + { + "anchor": { + "x": 1.9100722891566262, + "y": 5.317665405269765 + }, + "prevControl": { + "x": 1.2653373493975903, + "y": 5.258903614457831 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Depot end" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.6456431535269674, + "rotationDegrees": -131.08312006631627 + }, + { + "waypointRelativePos": 1.7302904564315178, + "rotationDegrees": -90.0 } ], - "rotationTargets": [], "constraintZones": [ { "name": "Constraints Zone", "minWaypointRelativePos": 0.6095783774685815, - "maxWaypointRelativePos": 1.0, + "maxWaypointRelativePos": 1.3607940446650069, "constraints": { "maxVelocity": 1.0, "maxAcceleration": 5.0, @@ -48,19 +89,19 @@ "eventMarkers": [ { "name": "Aim", - "waypointRelativePos": 0, - "endWaypointRelativePos": 1.0, + "waypointRelativePos": 0.14590570719603252, + "endWaypointRelativePos": 3.0, "command": null }, { "name": "Intake", - "waypointRelativePos": 0.2289763289036545, - "endWaypointRelativePos": 1.0, + "waypointRelativePos": 0.9985111662531012, + "endWaypointRelativePos": 2.0, "command": null } ], "globalConstraints": { - "maxVelocity": 0.3414, + "maxVelocity": 1.0, "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -69,7 +110,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 179.04515874612784 + "rotation": -1.8476102659945397 }, "reversed": false, "folder": null, diff --git a/CompBot/src/main/deploy/pathplanner/paths/FLIP Crash.path b/CompBot/src/main/deploy/pathplanner/paths/FLIP Crash.path new file mode 100644 index 0000000..28d1c23 --- /dev/null +++ b/CompBot/src/main/deploy/pathplanner/paths/FLIP Crash.path @@ -0,0 +1,130 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5929397590361445, + "y": 2.4168295454545454 + }, + "prevControl": null, + "nextControl": { + "x": 4.6786326670317635, + "y": 3.226349397590361 + }, + "isLocked": false, + "linkedName": "Right Start" + }, + { + "anchor": { + "x": 9.089578313253012, + "y": 1.128228915662652 + }, + "prevControl": { + "x": 8.01866265060241, + "y": 1.5434819277108467 + }, + "nextControl": { + "x": 10.021939622203154, + "y": 0.7667010611717786 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 9.089578313253012, + "y": 6.417240963855421 + }, + "prevControl": { + "x": 9.876373493975905, + "y": 6.7778554216867475 + }, + "nextControl": { + "x": 7.901769602229587, + "y": 5.872828637969683 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6422289156626504, + "y": 2.5816144578313254 + }, + "prevControl": { + "x": 5.636421686746985, + "y": 1.4560602409638554 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.26141078838174225, + "rotationDegrees": 30.243274429662616 + }, + { + "waypointRelativePos": 1.4265560165975106, + "rotationDegrees": 110.54278821473616 + }, + { + "waypointRelativePos": 2.3427385892116224, + "rotationDegrees": -131.53019707072244 + }, + { + "waypointRelativePos": 2.765975103734439, + "rotationDegrees": -140.00418907423557 + } + ], + "constraintZones": [ + { + "name": "Bump end", + "minWaypointRelativePos": 2.6590570719602775, + "maxWaypointRelativePos": 2.855583126550863, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.8307692307692283, + "endWaypointRelativePos": 2.4327543424317684, + "command": null + }, + { + "name": "Aim", + "waypointRelativePos": 2.8496277915632646, + "endWaypointRelativePos": 3.0, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -178.9187647670302 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Far Right.path b/CompBot/src/main/deploy/pathplanner/paths/Far Right.path index d0db508..b4cc7b2 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Far Right.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Far Right.path @@ -64,11 +64,11 @@ }, { "anchor": { - "x": 2.7187228915662653, + "x": 2.5766626506024095, "y": 2.4168295454545454 }, "prevControl": { - "x": 3.0123097604468962, + "x": 2.8702495194830404, "y": 2.6449383544921874 }, "nextControl": null, @@ -130,7 +130,7 @@ }, "goalEndState": { "velocity": 0.0, - "rotation": -41.30950981274551 + "rotation": -41.30950981274552 }, "reversed": false, "folder": null, diff --git a/CompBot/src/main/deploy/pathplanner/paths/G211 who.path b/CompBot/src/main/deploy/pathplanner/paths/G211 who.path index 594a1ed..f367205 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/G211 who.path +++ b/CompBot/src/main/deploy/pathplanner/paths/G211 who.path @@ -3,16 +3,16 @@ "waypoints": [ { "anchor": { - "x": 3.621625, - "y": 2.39621590909091 + "x": 3.5929397590361445, + "y": 2.4168295454545454 }, "prevControl": null, "nextControl": { - "x": 6.6894642421177455, - "y": 2.7547862025669643 + "x": 6.660779001153889, + "y": 2.7753998389306 }, "isLocked": false, - "linkedName": null + "linkedName": "Right Start" }, { "anchor": { @@ -104,7 +104,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 32.74683427363852 + "rotation": 0.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Inverse Right.path b/CompBot/src/main/deploy/pathplanner/paths/Inverse Right.path new file mode 100644 index 0000000..f811e85 --- /dev/null +++ b/CompBot/src/main/deploy/pathplanner/paths/Inverse Right.path @@ -0,0 +1,176 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5929397590361445, + "y": 2.4168295454545454 + }, + "prevControl": null, + "nextControl": { + "x": 4.5929397590361445, + "y": 2.4168295454545454 + }, + "isLocked": false, + "linkedName": "Right Start" + }, + { + "anchor": { + "x": 5.712915662650602, + "y": 2.4168295454545454 + }, + "prevControl": { + "x": 4.685710843373494, + "y": 2.461409638554217 + }, + "nextControl": { + "x": 6.823767668618597, + "y": 2.368619212990509 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.614337349397591, + "y": 4.668807228915662 + }, + "prevControl": { + "x": 5.428795180722891, + "y": 5.3463253012048195 + }, + "nextControl": { + "x": 9.012246699976645, + "y": 4.235455330236155 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.062373493975903, + "y": 2.4168295454545454 + }, + "prevControl": { + "x": 8.750410708249534, + "y": 2.4738619231276058 + }, + "nextControl": { + "x": 6.095385542168674, + "y": 2.2537831325301205 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.7187228915662653, + "y": 2.4168295454545454 + }, + "prevControl": { + "x": 2.968434228994741, + "y": 2.4048192182449206 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.972614107883791, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.4174273858921158, + "rotationDegrees": 79.22193857481086 + }, + { + "waypointRelativePos": 1.961825726141073, + "rotationDegrees": -23.228432521120364 + }, + { + "waypointRelativePos": 2.7319502074688704, + "rotationDegrees": -140.30640603093977 + }, + { + "waypointRelativePos": 3.5087136929460336, + "rotationDegrees": 133.41473277836218 + } + ], + "constraintZones": [ + { + "name": "Bump End", + "minWaypointRelativePos": 3.513647642679896, + "maxWaypointRelativePos": 3.6406947890818926, + "constraints": { + "maxVelocity": 2.5, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Intake Zone", + "minWaypointRelativePos": 1.2188585607940443, + "maxWaypointRelativePos": 3.2992555831265475, + "constraints": { + "maxVelocity": 2.5, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0, + "constraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 1.0759305210918124, + "endWaypointRelativePos": 3.386600496277901, + "command": null + }, + { + "name": "Aim", + "waypointRelativePos": 3.6803970223325035, + "endWaypointRelativePos": 4.0, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 138.22729829188262 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Left Alt.path b/CompBot/src/main/deploy/pathplanner/paths/Left Alt.path index a9c5a1f..2761ee8 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Left Alt.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Left Alt.path @@ -3,24 +3,24 @@ "waypoints": [ { "anchor": { - "x": 3.4361022727272723, + "x": 3.5929397590361445, "y": 2.4168295454545454 }, "prevControl": null, "nextControl": { - "x": 2.4217261055634807, - "y": 3.2004564907275324 + "x": 2.578565757324572, + "y": 3.200459293817526 }, "isLocked": false, - "linkedName": null + "linkedName": "Right Start" }, { "anchor": { - "x": 3.4290240963855423, + "x": 3.5929397590361445, "y": 5.317665405269765 }, "prevControl": { - "x": 2.4217261055634807, + "x": 2.585641768214083, "y": 5.10243937232525 }, "nextControl": null, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -62.58308037431244 + "rotation": 0.8493895342646307 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -41.98140661033514 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Left.path b/CompBot/src/main/deploy/pathplanner/paths/Left.path index a65b058..b4780bb 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Left.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Left.path @@ -3,45 +3,45 @@ "waypoints": [ { "anchor": { - "x": 3.4290240963855423, + "x": 3.5929397590361445, "y": 5.317665405269765 }, "prevControl": null, "nextControl": { - "x": 6.696409638554217, - "y": 5.073132530120482 + "x": 6.869463009098878, + "y": 5.317665405269765 }, "isLocked": false, "linkedName": "Left Start" }, { "anchor": { - "x": 7.8875301204751285, - "y": 4.898289156624749 + "x": 7.668975903614458, + "y": 4.9638554216867465 }, "prevControl": { - "x": 7.942288744908979, - "y": 9.881323983196895 + "x": 7.853417432344418, + "y": 9.943776697395887 }, "nextControl": { - "x": 7.876602409638554, - "y": 3.9038674698795175 + "x": 7.647120481927711, + "y": 4.373759036144579 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.204662650602408, - "y": 5.1168433734939756 + "x": 6.2265180722891555, + "y": 4.799939759036144 }, "prevControl": { - "x": 7.027837406324716, - "y": 4.20220475602474 + "x": 6.989925655819335, + "y": 3.8456802796234166 }, "nextControl": { - "x": 6.0079638554216865, - "y": 5.3353975903614455 + "x": 5.745698795180723, + "y": 5.400963855421687 }, "isLocked": false, "linkedName": null @@ -61,20 +61,24 @@ } ], "rotationTargets": [ + { + "waypointRelativePos": 0.6099585062240611, + "rotationDegrees": -91.09161788930598 + }, { "waypointRelativePos": 0.968464730290456, - "rotationDegrees": -89.74471187944988 + "rotationDegrees": -85.48797342569422 }, { - "waypointRelativePos": 1.595850622406647, + "waypointRelativePos": 2.686307053941867, "rotationDegrees": 126.4922832788486 } ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.6342431761786578, - "maxWaypointRelativePos": 1.7121588089330073, + "minWaypointRelativePos": 0.6223325062034732, + "maxWaypointRelativePos": 2.3791563275434195, "constraints": { "maxVelocity": 1.0, "maxAcceleration": 5.0, @@ -89,7 +93,7 @@ "eventMarkers": [ { "name": "Intake", - "waypointRelativePos": 0.6342431761786518, + "waypointRelativePos": 0.39602977667494543, "endWaypointRelativePos": 2.128218438538206, "command": null } @@ -110,7 +114,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -62.58308037431244 + "rotation": 0.8493895342646307 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Loopy Single-side.path b/CompBot/src/main/deploy/pathplanner/paths/Loopy Single-side.path new file mode 100644 index 0000000..5bd656b --- /dev/null +++ b/CompBot/src/main/deploy/pathplanner/paths/Loopy Single-side.path @@ -0,0 +1,192 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.54922891566265, + "y": 4.155204819277109 + }, + "prevControl": null, + "nextControl": { + "x": 2.4236746987951805, + "y": 4.461180722891566 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.3690361445783132, + "y": 3.1280000000000006 + }, + "prevControl": { + "x": 2.088097540023956, + "y": 3.359052310287696 + }, + "nextControl": { + "x": 3.5383012048192777, + "y": 2.166361445783133 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.838162072416303, + "y": 2.3540733601710326 + }, + "prevControl": { + "x": 7.031582571415575, + "y": 2.156205820141921 + }, + "nextControl": { + "x": 8.58296681905999, + "y": 2.536786512237935 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.632914392285299, + "y": 4.392238673580787 + }, + "prevControl": { + "x": 8.245827845648694, + "y": 4.413772601937025 + }, + "nextControl": { + "x": 5.44575003411572, + "y": 4.315395458060408 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.363330831513828, + "y": 3.411328295578603 + }, + "prevControl": { + "x": 6.265998851279204, + "y": 3.9566283943131904 + }, + "nextControl": { + "x": 6.661378786844978, + "y": 1.7415217317139742 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.3690361445783132, + "y": 3.729024096385543 + }, + "prevControl": { + "x": 2.270686746987952, + "y": 2.1991445783132537 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.4730290456431554, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 2.008298755186713, + "rotationDegrees": 43.48173935506697 + }, + { + "waypointRelativePos": 2.85, + "rotationDegrees": 156.39894927428526 + }, + { + "waypointRelativePos": 3.485477178423249, + "rotationDegrees": 177.63685215395574 + } + ], + "constraintZones": [ + { + "name": "Preload Shooting", + "minWaypointRelativePos": 0.45, + "maxWaypointRelativePos": 1.3, + "constraints": { + "maxVelocity": 0.6, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Bump End ", + "minWaypointRelativePos": 4.465896869024856, + "maxWaypointRelativePos": 4.766147825047803, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Intake", + "minWaypointRelativePos": 1.854094292803968, + "maxWaypointRelativePos": 3.80414973709369, + "constraints": { + "maxVelocity": 1.75, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Aim", + "waypointRelativePos": 0.33746898263028435, + "endWaypointRelativePos": 1.2754342431761725, + "command": null + }, + { + "name": "Intake", + "waypointRelativePos": 1.6228287841191003, + "endWaypointRelativePos": 4.094292803970218, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -179.2691253430262 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Loopy to Inverse.path b/CompBot/src/main/deploy/pathplanner/paths/Loopy to Inverse.path new file mode 100644 index 0000000..d84da84 --- /dev/null +++ b/CompBot/src/main/deploy/pathplanner/paths/Loopy to Inverse.path @@ -0,0 +1,169 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5254176332787486, + "y": 4.010216691684862 + }, + "prevControl": null, + "nextControl": { + "x": 1.3613411458333338, + "y": 2.4043879639737993 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.712915662650602, + "y": 2.4168295454545454 + }, + "prevControl": { + "x": 4.685710843373494, + "y": 2.461409638554217 + }, + "nextControl": { + "x": 6.823767668618597, + "y": 2.368619212990509 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.406710843373492, + "y": 4.636024096385543 + }, + "prevControl": { + "x": 5.221168674698792, + "y": 5.313542168674701 + }, + "nextControl": { + "x": 8.804620193952545, + "y": 4.202672197706036 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.636192771084337, + "y": 2.2756385542168687 + }, + "prevControl": { + "x": 8.324229985357968, + "y": 2.332670931889929 + }, + "nextControl": { + "x": 5.669204819277108, + "y": 2.1125921412924438 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.0739879518072284, + "y": 3.6088192771084344 + }, + "prevControl": { + "x": 3.013771084337349, + "y": 2.264710843373495 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.972614107883791, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.4174273858921158, + "rotationDegrees": 79.22193857481086 + }, + { + "waypointRelativePos": 1.961825726141073, + "rotationDegrees": -23.228432521120364 + }, + { + "waypointRelativePos": 2.7319502074688704, + "rotationDegrees": -140.30640603093977 + }, + { + "waypointRelativePos": 3.5087136929460336, + "rotationDegrees": 133.41473277836218 + } + ], + "constraintZones": [ + { + "name": "Intake Zone", + "minWaypointRelativePos": 1.2188585607940443, + "maxWaypointRelativePos": 3.2992555831265475, + "constraints": { + "maxVelocity": 2.5, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 0.6287643403441671, + "constraints": { + "maxVelocity": 0.75, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Aim", + "waypointRelativePos": 0.11062978011472276, + "endWaypointRelativePos": 0.6247012428298275, + "command": null + }, + { + "name": "Intake", + "waypointRelativePos": 1.0759305210918124, + "endWaypointRelativePos": 3.386600496277901, + "command": null + }, + { + "name": "Aim", + "waypointRelativePos": 3.6803970223325035, + "endWaypointRelativePos": 4.0, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 138.22729829188262 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Loopy.path b/CompBot/src/main/deploy/pathplanner/paths/Loopy.path new file mode 100644 index 0000000..8526295 --- /dev/null +++ b/CompBot/src/main/deploy/pathplanner/paths/Loopy.path @@ -0,0 +1,189 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.54922891566265, + "y": 4.155204819277109 + }, + "prevControl": null, + "nextControl": { + "x": 2.4236746987951805, + "y": 4.461180722891566 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.3690361445783132, + "y": 3.1280000000000006 + }, + "prevControl": { + "x": 2.088097540023956, + "y": 3.359052310287696 + }, + "nextControl": { + "x": 3.5383012048192777, + "y": 2.166361445783133 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.0079638554216865, + "y": 2.811096385542169 + }, + "prevControl": { + "x": 5.323849123256161, + "y": 2.022877237612325 + }, + "nextControl": { + "x": 6.510638554216868, + "y": 3.3902650602409645 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.0079638554216865, + "y": 5.073132530120482 + }, + "prevControl": { + "x": 6.893108433734941, + "y": 4.22077108433735 + }, + "nextControl": { + "x": 4.747916887146947, + "y": 6.286511092162821 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.3690361445783132, + "y": 4.155204819277109 + }, + "prevControl": { + "x": 2.6968674698795176, + "y": 5.543024096385542 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.4730290456431554, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 2.008298755186713, + "rotationDegrees": 43.48173935506697 + }, + { + "waypointRelativePos": 2.85, + "rotationDegrees": 134.62591321412003 + }, + { + "waypointRelativePos": 3.485477178423249, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [ + { + "name": "Preload Shooting", + "minWaypointRelativePos": 0.45, + "maxWaypointRelativePos": 1.3, + "constraints": { + "maxVelocity": 0.6, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Bump End ", + "minWaypointRelativePos": 3.183622828784128, + "maxWaypointRelativePos": 3.799007444168695, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.672456575682393, + "maxWaypointRelativePos": 3.0843672456575746, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Intake", + "minWaypointRelativePos": 1.854094292803968, + "maxWaypointRelativePos": 3.188089330024804, + "constraints": { + "maxVelocity": 1.75, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Aim", + "waypointRelativePos": 0.33746898263028435, + "endWaypointRelativePos": 1.2754342431761725, + "command": null + }, + { + "name": "Intake", + "waypointRelativePos": 1.6228287841191003, + "endWaypointRelativePos": 4.094292803970218, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -178.9187647670302 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Middle Depot.path b/CompBot/src/main/deploy/pathplanner/paths/Middle Depot.path index d4ef384..3e62805 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Middle Depot.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Middle Depot.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 0.6643132530120479, - "y": 5.892710843373494 + "x": 0.5714978601997143, + "y": 5.982268188302426 }, "prevControl": { - "x": 2.4564578313253, - "y": 5.914566265060241 + "x": 2.3636424385129686, + "y": 6.004123609989173 }, "nextControl": null, "isLocked": false, - "linkedName": "Depot" + "linkedName": null } ], "rotationTargets": [], diff --git a/CompBot/src/main/deploy/pathplanner/paths/Outpost.path b/CompBot/src/main/deploy/pathplanner/paths/Outpost.path index 689d1e1..870975f 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Outpost.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Outpost.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 2.7187228915662653, + "x": 2.5766626506024095, "y": 2.4168295454545454 }, "prevControl": null, "nextControl": { - "x": 2.1518478915662653, + "x": 2.0097876506024095, "y": 1.860261363636364 }, "isLocked": false, @@ -55,7 +55,7 @@ "folder": null, "idealStartingState": { "velocity": 0.0, - "rotation": -41.30950981274551 + "rotation": -41.30950981274552 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Right Cleanup.path b/CompBot/src/main/deploy/pathplanner/paths/Right Cleanup.path index db2495e..384182c 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Right Cleanup.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Right Cleanup.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 2.7187228915662653, + "x": 2.5766626506024095, "y": 2.4168295454545454 }, "prevControl": null, "nextControl": { - "x": 3.718722891566267, + "x": 3.5766626506024113, "y": 2.4168295454545454 }, "isLocked": false, @@ -164,7 +164,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -41.30950981274551 + "rotation": -41.30950981274552 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path b/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path new file mode 100644 index 0000000..b32909d --- /dev/null +++ b/CompBot/src/main/deploy/pathplanner/paths/Right Depot.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.5766626506024095, + "y": 2.4168295454545454 + }, + "prevControl": null, + "nextControl": { + "x": 1.9647108433734934, + "y": 3.182638554216868 + }, + "isLocked": false, + "linkedName": "Right 2 Start" + }, + { + "anchor": { + "x": 0.412975903614458, + "y": 5.991060240963854 + }, + "prevControl": { + "x": 1.8554337349397614, + "y": 6.307963855421685 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 178.89829388479365 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -41.30950981274552 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/CompBot/src/main/deploy/pathplanner/paths/Right.path b/CompBot/src/main/deploy/pathplanner/paths/Right.path index a106981..4166613 100644 --- a/CompBot/src/main/deploy/pathplanner/paths/Right.path +++ b/CompBot/src/main/deploy/pathplanner/paths/Right.path @@ -3,56 +3,56 @@ "waypoints": [ { "anchor": { - "x": 3.4361022727272723, + "x": 3.5929397590361445, "y": 2.4168295454545454 }, "prevControl": null, "nextControl": { - "x": 6.128168674698795, - "y": 2.822024096385543 + "x": 6.171879518072289, + "y": 3.2919156626506028 }, "isLocked": false, "linkedName": "Right Start" }, { "anchor": { - "x": 6.728808593750001, - "y": 1.2268230083626759 + "x": 7.046096385542168, + "y": 0.7457590361445796 }, "prevControl": { - "x": 6.065178019973029, - "y": 2.5536917163810404 + "x": 6.180447232697349, + "y": 0.9501484194551628 }, "nextControl": { - "x": 7.614337349397591, - "y": -0.5437108433734943 + "x": 7.83289156626506, + "y": 0.5599879518072293 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.919806818181818, - "y": 3.9525454545454544 + "x": 6.783831325301205, + "y": 3.586963855421687 }, "prevControl": { - "x": 8.68011542976235, - "y": 5.29958307018095 + "x": 8.65583427376701, + "y": 4.773893384448944 }, "nextControl": { - "x": 4.499939759036145, - "y": 2.100795180722891 + "x": 4.215819277108434, + "y": 1.958734939759037 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.7187228915662653, + "x": 2.5766626506024095, "y": 2.4168295454545454 }, "prevControl": { - "x": 4.897062705366924, + "x": 4.755002464403075, "y": 2.4941927710843377 }, "nextControl": null, @@ -63,7 +63,7 @@ "rotationTargets": [ { "waypointRelativePos": 1.3904619970193783, - "rotationDegrees": 90.0 + "rotationDegrees": 49.27300832426523 }, { "waypointRelativePos": 1.6207468879668165, @@ -74,14 +74,14 @@ "rotationDegrees": -141.73750857193497 }, { - "waypointRelativePos": 2.187633262260129, + "waypointRelativePos": 2.1834024896265594, "rotationDegrees": -35.84339182244795 } ], "constraintZones": [ { "name": "slowdown intake", - "minWaypointRelativePos": 1.2714640198511127, + "minWaypointRelativePos": 0.9439205955335208, "maxWaypointRelativePos": 2.1230769230769058, "constraints": { "maxVelocity": 1.0, @@ -93,24 +93,11 @@ } }, { - "name": "BUMP", - "minWaypointRelativePos": 0.06253101736972864, - "maxWaypointRelativePos": 0.6342431761786589, + "name": "Constraints Zone", + "minWaypointRelativePos": 2.8198511166253133, + "maxWaypointRelativePos": 3.0, "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - }, - { - "name": "MORE BUMPING", - "minWaypointRelativePos": 2.158808933002488, - "maxWaypointRelativePos": 2.7841191066997464, - "constraints": { - "maxVelocity": 2.0, + "maxVelocity": 0.1, "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -126,6 +113,12 @@ "waypointRelativePos": 0.6739864864864873, "endWaypointRelativePos": 2.2652027027027057, "command": null + }, + { + "name": "Aim", + "waypointRelativePos": 2.617369727047151, + "endWaypointRelativePos": 3.0, + "command": null } ], "globalConstraints": { @@ -144,7 +137,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 40.23685706767951 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/CompBot/src/main/java/frc/robot/Constants.java b/CompBot/src/main/java/frc/robot/Constants.java index 2983477..34830c8 100644 --- a/CompBot/src/main/java/frc/robot/Constants.java +++ b/CompBot/src/main/java/frc/robot/Constants.java @@ -15,6 +15,7 @@ import frc.robot.aiming.AimConstraints; import frc.robot.aiming.AimStrategy; import frc.robot.aiming.ToFAim; +import frc.robot.aiming.TuneAim; import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.util.BoundingBox; @@ -46,6 +47,7 @@ public static class AimConstants { public static final AimStrategy kScoringAim = new ToFAim(ShooterConstants.scoringMeasurements, constraints); // public static final AimStrategy kScoringAim = new TuneAim(); public static final AimStrategy kFeedingAim = new ToFAim(ShooterConstants.feedingMeasurements, constraints); + // public static final AimStrategy kFeedingAim = kScoringAim; } diff --git a/CompBot/src/main/java/frc/robot/aiming/ToFAim.java b/CompBot/src/main/java/frc/robot/aiming/ToFAim.java index 895a04b..efa0e70 100644 --- a/CompBot/src/main/java/frc/robot/aiming/ToFAim.java +++ b/CompBot/src/main/java/frc/robot/aiming/ToFAim.java @@ -9,6 +9,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.aiming.AimParams.AimStatus; import frc.robot.aiming.AimParams.SpeedControl; @@ -43,7 +44,7 @@ public ToFAim(List measurements, AimConstraints constraints) { } } - public AimParams update(Pose3d aimTarget, Pose3d shooterPose, Translation2d shooterVelocity) { + public AimParams update(Pose3d aimTarget, Pose3d shooterPose, Translation2d shooterVelocity) { Translation2d target = aimTarget.getTranslation().toTranslation2d(); Translation2d start = shooterPose.getTranslation().toTranslation2d(); diff --git a/CompBot/src/main/java/frc/robot/binding/DriverXboxBindings.java b/CompBot/src/main/java/frc/robot/binding/DriverXboxBindings.java index 6a40945..81363bb 100644 --- a/CompBot/src/main/java/frc/robot/binding/DriverXboxBindings.java +++ b/CompBot/src/main/java/frc/robot/binding/DriverXboxBindings.java @@ -1,7 +1,6 @@ package frc.robot.binding; import java.util.function.DoubleSupplier; - import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -10,7 +9,6 @@ import frc.robot.commands.ResetForwards; import frc.robot.commands.RetractIntake; import frc.robot.commands.RunIntake; -import frc.robot.subsystems.drivetrain.Drivetrain.TeleopDriveMode; import frc.robot.superstructure.Superstructure; import frc.robot.util.RumbleUtil; import frc.robot.util.RumbleUtil.RumbleStrength; @@ -20,7 +18,7 @@ public class DriverXboxBindings implements Binder { private final DoubleSupplier vx, vy, vrot; - private final Trigger shoot, intake, resetPerspective, robotRelativeDrive, retract; + private final Trigger shoot, intake, resetPerspective, antislowbot, retract; public DriverXboxBindings() { controller = new CommandXboxController(Driver.kDriveControllerPort); @@ -32,12 +30,12 @@ public DriverXboxBindings() { shoot = controller.rightBumper(); intake = controller.rightTrigger(); resetPerspective = controller.leftBumper(); - robotRelativeDrive = controller.leftTrigger(); + antislowbot = controller.leftTrigger(); retract = controller.x(); } public void bind(Superstructure superstructure) { - superstructure.bindDrive(vx, vy, vrot, () -> robotRelativeDrive.getAsBoolean() ? TeleopDriveMode.RobotRelative : TeleopDriveMode.FieldRelativeSpin); + superstructure.bindDrive(vx, vy, vrot, antislowbot); shoot.toggleOnTrue(superstructure.build(new AimPrep())); intake.toggleOnTrue(superstructure.build(new RunIntake())); diff --git a/CompBot/src/main/java/frc/robot/binding/KeyboardBindings.java b/CompBot/src/main/java/frc/robot/binding/KeyboardBindings.java index 10c9da2..6f61a80 100644 --- a/CompBot/src/main/java/frc/robot/binding/KeyboardBindings.java +++ b/CompBot/src/main/java/frc/robot/binding/KeyboardBindings.java @@ -9,40 +9,39 @@ import frc.robot.commands.RetractIntake; import frc.robot.commands.RunIndex; import frc.robot.commands.RunIntake; -import frc.robot.subsystems.drivetrain.Drivetrain.TeleopDriveMode; import frc.robot.superstructure.Superstructure; public class KeyboardBindings implements Binder { - private final CommandGenericHID controller; + private final CommandGenericHID controller; - private final Trigger shoot, intake, robotCentricDrive, resetForwards, index, retractIntake; + private final Trigger shoot, intake, antislowbot, resetForwards, index, retractIntake; - private final DoubleSupplier vx, vy, vrot; + private final DoubleSupplier vx, vy, vrot; - public KeyboardBindings() { - controller = new CommandGenericHID(0); + public KeyboardBindings() { + controller = new CommandGenericHID(0); - shoot = controller.button(1); - intake = controller.button(2); - robotCentricDrive = controller.button(3); - resetForwards = controller.button(4); - index = controller.button(5); - retractIntake = controller.button(6); + shoot = controller.button(1); + intake = controller.button(2); + antislowbot = controller.button(3); + resetForwards = controller.button(4); + index = controller.button(5); + retractIntake = controller.button(6); - vx = () -> controller.getRawAxis(0); - vy = () -> controller.getRawAxis(1); - vrot = () -> controller.getRawAxis(2); - } + vx = () -> controller.getRawAxis(0); + vy = () -> controller.getRawAxis(1); + vrot = () -> controller.getRawAxis(2); + } - public void bind(Superstructure superstructure) { - superstructure.bindDrive(vx, vy, vrot, () -> robotCentricDrive.getAsBoolean() ? TeleopDriveMode.RobotRelative : TeleopDriveMode.FieldRelativeSpin); + public void bind(Superstructure superstructure) { + superstructure.bindDrive(vx, vy, vrot, antislowbot); - shoot.toggleOnTrue(superstructure.build(new AimPrep())); - intake.whileTrue(superstructure.build(new RunIntake())); - resetForwards.onTrue(superstructure.build(new ResetForwards())); - index.whileTrue(superstructure.build(new RunIndex())); - retractIntake.onTrue(superstructure.build(new RetractIntake())); + shoot.toggleOnTrue(superstructure.build(new AimPrep())); + intake.whileTrue(superstructure.build(new RunIntake())); + resetForwards.onTrue(superstructure.build(new ResetForwards())); + index.whileTrue(superstructure.build(new RunIndex())); + retractIntake.onTrue(superstructure.build(new RetractIntake())); + + } - } - } diff --git a/CompBot/src/main/java/frc/robot/binding/NamedCommandBindings.java b/CompBot/src/main/java/frc/robot/binding/NamedCommandBindings.java index 2a00f78..c55c74f 100644 --- a/CompBot/src/main/java/frc/robot/binding/NamedCommandBindings.java +++ b/CompBot/src/main/java/frc/robot/binding/NamedCommandBindings.java @@ -4,6 +4,8 @@ import com.pathplanner.lib.events.EventTrigger; import frc.robot.commands.AimPrep; +import frc.robot.commands.DrivetrainShake; +import frc.robot.commands.RetractIntake; import frc.robot.commands.RunIntake; import frc.robot.superstructure.Superstructure; @@ -11,6 +13,8 @@ public class NamedCommandBindings implements Binder { public void bind(Superstructure superstructure) { NamedCommands.registerCommand("Aim", superstructure.build(new AimPrep())); NamedCommands.registerCommand("Intake", superstructure.build(new RunIntake())); + NamedCommands.registerCommand("Retract", superstructure.build(new RetractIntake())); + NamedCommands.registerCommand("Shake", superstructure.buildWithoutProxy(new DrivetrainShake())); new EventTrigger("Aim").whileTrue(superstructure.build(new AimPrep())); new EventTrigger("Intake").whileTrue(superstructure.build(new RunIntake())); diff --git a/CompBot/src/main/java/frc/robot/binding/OperatorPS5Bindings.java b/CompBot/src/main/java/frc/robot/binding/OperatorPS5Bindings.java index 9332bf7..eef901b 100644 --- a/CompBot/src/main/java/frc/robot/binding/OperatorPS5Bindings.java +++ b/CompBot/src/main/java/frc/robot/binding/OperatorPS5Bindings.java @@ -9,6 +9,7 @@ import frc.robot.commands.RetractIntake; import frc.robot.commands.RunIndex; import frc.robot.superstructure.Superstructure; +import frc.robot.util.TriggerSequence; public class OperatorPS5Bindings implements Binder { private final CommandPS5Controller controller; @@ -19,9 +20,9 @@ public OperatorPS5Bindings() { controller = new CommandPS5Controller(Operator.kOperatorControllerPort); index = controller.R1(); - eject = controller.cross(); - agitate = controller.square(); - retract = controller.triangle(); + eject = controller.pov(180); // down + agitate = controller.pov(270); // left + retract = controller.pov(0); // up shootTillEmpty = controller.L2(); } @@ -31,5 +32,9 @@ public void bind(Superstructure superstructure) { agitate.whileTrue(superstructure.build(new AgitateIntake())); retract.onTrue(superstructure.build(new RetractIntake())); shootTillEmpty.onTrue(superstructure.build(new EmptyHopper())); + + // obfuscation is how we beat the llms + Trigger ianSecret = TriggerSequence.fromController(controller, 4, 4, 2, 2, 1, 3, 1, 3, 12, 11); + ianSecret.onTrue(superstructure.state.runPartyMode(5.0)); } } diff --git a/CompBot/src/main/java/frc/robot/commands/DrivetrainShake.java b/CompBot/src/main/java/frc/robot/commands/DrivetrainShake.java new file mode 100644 index 0000000..91e70e7 --- /dev/null +++ b/CompBot/src/main/java/frc/robot/commands/DrivetrainShake.java @@ -0,0 +1,11 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.superstructure.StateManager; +import frc.robot.superstructure.Superstructure.Subsystems; + +public class DrivetrainShake implements CommandBuilder { + public Command build(Subsystems subsystems, StateManager state) { + return subsystems.drivetrain().shake(); + } +} diff --git a/CompBot/src/main/java/frc/robot/subsystems/drivetrain/AutopilotConstants.java b/CompBot/src/main/java/frc/robot/subsystems/drivetrain/AutopilotConstants.java index fea8d59..2663c27 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/drivetrain/AutopilotConstants.java +++ b/CompBot/src/main/java/frc/robot/subsystems/drivetrain/AutopilotConstants.java @@ -10,7 +10,7 @@ public class AutopilotConstants { private static final APConstraints kConstraints = new APConstraints() - .withVelocity(4.0) + .withVelocity(2.0) .withAcceleration(10.0) .withJerk(8.0); @@ -24,7 +24,7 @@ public class AutopilotConstants { private static final APProfile kLooseProfile = new APProfile(kConstraints) .withBeelineRadius(Meters.of(0.25)) .withErrorTheta(Degrees.of(30)) - .withErrorXY(Inches.of(12)); + .withErrorXY(Inches.of(4)); public static final Autopilot kLooseAutopilot = new Autopilot(kLooseProfile); diff --git a/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 1b5970b..811fe0f 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/CompBot/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -38,6 +38,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.DriverStation; @@ -49,6 +50,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Subsystem; +import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.FieldManager; @@ -60,6 +62,7 @@ import frc.robot.subsystems.turret.TurretConstants; import frc.robot.util.FieldUtils; import frc.robot.util.OnboardLogger; +import frc.robot.util.ChassisSpeedRateLimiter; import frc.robot.util.StatusSignalUtil; import frc.robot.vision.localization.LocalizationConstants; import frc.robot.vision.localization.TimestampedPoseEstimate; @@ -87,11 +90,13 @@ public enum TeleopDriveMode { * Drive the robot with a field-relative control for translation and spin control (i.e. control * over how fast we rotate) */ - FieldRelativeSpin, + FieldRelative, /** Drive the robot slower than FieldRelativeSpin */ - SlowFieldRelativeSpin, + SlowFieldRelative, /** Drive robot relative. */ RobotRelative, + /** Drive with limited acceleration */ + AccelerationLimitedFieldRelative } private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() @@ -117,6 +122,8 @@ public enum TeleopDriveMode { .withHeadingPID(HeadingGains.kP, HeadingGains.kI, HeadingGains.kD) .withCenterOfRotation(TurretConstants.kOffset.getTranslation().toTranslation2d()); + private ChassisSpeedRateLimiter rateLimiter = new ChassisSpeedRateLimiter(2.0, 3.0); + private SwerveDriveState state; private double lastOkayVisionUpdateTime; @@ -215,6 +222,7 @@ public Drivetrain( () -> Timer.getTimestamp() - lastOkayVisionUpdateTime); SwerveModule[] actualModules = getModules(); for (int i = 0; i < actualModules.length; i++) { + int j = i; TalonFX drive, steer; SwerveModule module = actualModules[i]; drive = module.getDriveMotor(); @@ -273,9 +281,12 @@ public Drivetrain( ologger.registerMeasurement("Steer " + steer.getDeviceID() + " Position", steer.getPosition(false)::getValue, Rotations); } + ologger.registerSwerveModuleState("Module States", () -> state.ModuleStates); + ologger.registerSwerveModuleState("Module Targets", () -> state.ModuleTargets); + ologger.registerSwerveModulePosition("Module Positions", () -> state.ModulePositions); sysIDCommands(); - // SmartDashboard.putData("Drivetrain/Set Home", setMemorySpot()); - // SmartDashboard.putData("Drivetrain/Go Home", goHome()); + SmartDashboard.putData("Drivetrain/Set Home", setMemorySpot()); + SmartDashboard.putData("Drivetrain/Go Home", goHome()); SmartDashboard.putData("Drivetrain/Shake", shake()); configurePathplanner(); } @@ -342,6 +353,7 @@ public void periodic() { state = getState(); FieldManager.getInstance().getField().setRobotPose(robotPose()); + FieldManager.getInstance().getField().getObject("Memory Spot").setPose(memorySpot); } private void startSimThread() { @@ -408,40 +420,49 @@ public Command teleopDrive(DoubleSupplier vx, DoubleSupplier vy, DoubleSupplier return this.applyRequest(() -> { TeleopDriveMode mode = modeSupplier.get(); // Recalculate the *real* vx and vy to be operator-dependent - Translation2d operatorRelative = - new Translation2d(vx.getAsDouble() * maxSpeed, vy.getAsDouble() * maxSpeed); + ChassisSpeeds operatorRelative = + new ChassisSpeeds( + vx.getAsDouble() * maxSpeed, + vy.getAsDouble() * maxSpeed, + vrot.getAsDouble() * maxRotationalSpeed); - Translation2d fieldRelative = operatorRelative.rotateBy(getOperatorForwardDirection()); - double spin = vrot.getAsDouble() * maxRotationalSpeed; + ChassisSpeeds fieldRelative = ChassisSpeeds.fromRobotRelativeSpeeds(operatorRelative, getOperatorForwardDirection()); if (mode == TeleopDriveMode.RobotRelative) { return robotCentricDrive - .withVelocityX(operatorRelative.getX()) - .withVelocityY(operatorRelative.getY()) - .withRotationalRate(spin); + .withVelocityX(operatorRelative.vxMetersPerSecond) + .withVelocityY(operatorRelative.vyMetersPerSecond) + .withRotationalRate(operatorRelative.omegaRadiansPerSecond); } - if (mode == TeleopDriveMode.SlowFieldRelativeSpin) { + if (mode == TeleopDriveMode.SlowFieldRelative + || mode == TeleopDriveMode.AccelerationLimitedFieldRelative) { fieldRelative = fieldRelative.times(0.3); - spin *= 0.5; + } + + if (mode != TeleopDriveMode.AccelerationLimitedFieldRelative) { + rateLimiter.reset(fieldRelative); + } else { + fieldRelative = rateLimiter.calculate(fieldRelative); } if (override.isPresent()) { - return drivetrainAim.withVelocityX(fieldRelative.getX()) - .withVelocityY(fieldRelative.getY()) + return drivetrainAim + .withVelocityX(fieldRelative.vxMetersPerSecond) + .withVelocityY(fieldRelative.vyMetersPerSecond) .withTargetDirection(override.get().yaw); } return drive - .withVelocityX(fieldRelative.getX()) - .withVelocityY(fieldRelative.getY()) - .withRotationalRate(spin); + .withVelocityX(fieldRelative.vxMetersPerSecond) + .withVelocityY(fieldRelative.vyMetersPerSecond) + .withRotationalRate(fieldRelative.omegaRadiansPerSecond); }) .withName("Teleop Drive"); } public Command teleopDrive(DoubleSupplier vx, DoubleSupplier vy, DoubleSupplier vrot) { - return teleopDrive(vx, vy, vrot, () -> TeleopDriveMode.FieldRelativeSpin); + return teleopDrive(vx, vy, vrot, () -> TeleopDriveMode.FieldRelative); } /** @@ -557,7 +578,7 @@ public Command track(Supplier params) { } public Command setMemorySpot() { - return Commands.runOnce(() -> memorySpot = robotPose()); + return Commands.runOnce(() -> memorySpot = robotPose()).ignoringDisable(true); } public Command goHome() { @@ -573,7 +594,7 @@ private Command shakeAbout(Supplier center) { return Commands.sequence( this.runOnce(() -> { Rotation2d randomAngle = Rotation2d.fromRotations(Math.random()); - double radius = 0.5; + double radius = 0.25; target[0] = new APTarget(center.get().plus(new Transform2d( radius * randomAngle.getCos(), radius * randomAngle.getSin(), @@ -617,7 +638,7 @@ private void configurePathplanner() { new PPHolonomicDriveController( // PPHolonomicController is the built in path following // controller for holonomic // drive trains - new PIDConstants(1.7, 0.0, 0.0), // Translation PID constants + new PIDConstants(3.0, 0.0, 0.0), // Translation PID constants new PIDConstants(3.0, 0.0, 0.0) // Rotation PID constants ), config, // The robot configuration @@ -636,4 +657,9 @@ private void configurePathplanner() { this // Reference to this subsystem to set requirements ); } + + @Override + public void simulationPeriodic() { + SmartDashboard.putNumber("Drivetrain/Speed", robotVelocity().getTranslation().getNorm()); + } } diff --git a/CompBot/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/CompBot/src/main/java/frc/robot/subsystems/indexer/Indexer.java index 9161345..57c66cf 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/CompBot/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -38,9 +38,9 @@ public Command index() { } public Command indexWithVariation() { - final double MIN_SPIN_VOLTAGE = 9.0; + final double MIN_SPIN_VOLTAGE = 6.0; final double MAX_SPIN_VOLTAGE = 12.0; - final double OMEGA = Math.PI; // Make this faster for more variation + final double OMEGA = 4 * Math.PI; // Make this faster for more variation return this.runEnd( () -> { io.setFeedVoltage(FeederConstants.kFeedVoltage); diff --git a/CompBot/src/main/java/frc/robot/subsystems/intake/Intake.java b/CompBot/src/main/java/frc/robot/subsystems/intake/Intake.java index f387df9..a2a9036 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/CompBot/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.subsystems.intake.IntakeConstants.DeployConstants; import frc.robot.subsystems.intake.IntakeConstants.DeployConstants.DeployPosition; diff --git a/CompBot/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/CompBot/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 29afc1e..41d735c 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/CompBot/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -42,6 +42,10 @@ public class IntakeConstants { .withSupplyCurrentLimit(40) .withStatorCurrentLimit(120)); + protected static final TalonFXConfiguration kIntakeMotorConfigDuringAuto = kIntakeMotorConfig.clone().withCurrentLimits( + kIntakeMotorConfig.CurrentLimits.clone().withSupplyCurrentLimit(80) + ); + protected static final Current kJamStatorThreshold = Amps.of(70); protected static final AngularVelocity kJamVelocityThreshold = RotationsPerSecond.of(0.3); @@ -83,7 +87,7 @@ public static final class DeployConstants { .withMagnetSensor(new MagnetSensorConfigs() .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive) .withAbsoluteSensorDiscontinuityPoint(0.5) - .withMagnetOffset(-0.036376953125)); + .withMagnetOffset(-0.37939453125)); protected static final AngularVelocity kMaxVelocity = RotationsPerSecond.of(0.4); protected static final AngularAcceleration kMaxAcceleration = RotationsPerSecondPerSecond.of(4); diff --git a/CompBot/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java b/CompBot/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java index 56b34c0..a7b72a8 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java +++ b/CompBot/src/main/java/frc/robot/subsystems/intake/IntakeIOHardware.java @@ -10,6 +10,8 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import frc.robot.subsystems.intake.IntakeConstants.DeployConstants; import frc.robot.util.StatusSignalUtil; @@ -34,6 +36,12 @@ public IntakeIOHardware() { deployMotor = new TalonFX(DeployConstants.kDeployMotorId, StatusSignalUtil.canivore); deployMotor.getConfigurator().apply(DeployConstants.kDeployMotorConfig); + RobotModeTriggers.autonomous().onTrue(Commands.startEnd(() -> { + intakeMotor.getConfigurator().apply(IntakeConstants.kIntakeMotorConfigDuringAuto); + }, () -> { + intakeMotor.getConfigurator().apply(IntakeConstants.kIntakeMotorConfig); + })); + StatusSignalUtil.registerRioSignals( intakeMotor.getSupplyCurrent(false), diff --git a/CompBot/src/main/java/frc/robot/subsystems/led/Led.java b/CompBot/src/main/java/frc/robot/subsystems/led/Led.java index f347bc6..7b259dc 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/led/Led.java +++ b/CompBot/src/main/java/frc/robot/subsystems/led/Led.java @@ -25,6 +25,7 @@ public Led(LedIO io) { private List hierarchy = List.of( new BadController(), + new PartyMode(), new TestRslEnabled(), new TestRsl(), new EndGameAlert(), diff --git a/CompBot/src/main/java/frc/robot/subsystems/led/ledStates/PartyMode.java b/CompBot/src/main/java/frc/robot/subsystems/led/ledStates/PartyMode.java new file mode 100644 index 0000000..6f5f295 --- /dev/null +++ b/CompBot/src/main/java/frc/robot/subsystems/led/ledStates/PartyMode.java @@ -0,0 +1,20 @@ +package frc.robot.subsystems.led.ledStates; + +import com.ctre.phoenix6.controls.ControlRequest; +import com.ctre.phoenix6.signals.RGBWColor; + +import edu.wpi.first.wpilibj.util.Color; +import frc.robot.subsystems.led.LedIO; +import frc.robot.subsystems.led.LedState; +import frc.robot.subsystems.led.LedIO.AnimationType; +import frc.robot.superstructure.StateManager; + +public class PartyMode implements LedState { + public boolean check(StateManager state) { + return state.partying.getAsBoolean(); + } + + public ControlRequest apply(LedIO io) { + return io.createAnimation(new RGBWColor(Color.kWhite), AnimationType.Rainbow); + } +} diff --git a/CompBot/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/CompBot/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 4024bdd..0a50ae8 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/CompBot/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -55,12 +55,12 @@ public final class ShooterConstants { .withTorqueCurrent(new TorqueCurrentConfigs() .withPeakReverseTorqueCurrent(0.0) - .withPeakForwardTorqueCurrent(80.0)) + .withPeakForwardTorqueCurrent(100.0)) .withCurrentLimits(new CurrentLimitsConfigs() .withSupplyCurrentLimitEnable(true) .withStatorCurrentLimitEnable(true) - .withSupplyCurrentLimit(80) + .withSupplyCurrentLimit(100) .withStatorCurrentLimit(100)); public static final Distance kRadius = Inches.of(2); @@ -69,7 +69,7 @@ public final class ShooterConstants { public static final MotorAlignmentValue kMotor2Alignment = MotorAlignmentValue.Aligned; public static final LinearVelocity kMaxLinearSpeed = MetersPerSecond.of(15.5); - public static final AngularVelocity kMaxRotationalSpeed = RotationsPerSecond.of(85.0); + public static final AngularVelocity kMaxRotationalSpeed = RotationsPerSecond.of(100.0); public static final class HoodConstants { protected static final int kMotorID = 56; @@ -125,29 +125,29 @@ public static final class HoodConstants { } public static final List scoringMeasurements = List.of( - new AimMeasurement(Meters.of(1.70), Rotation2d.fromDegrees(72), 31, Seconds.of(0.962)), - new AimMeasurement(Meters.of(2.41), Rotation2d.fromDegrees(70), 34, Seconds.of(1.006)), - new AimMeasurement(Meters.of(2.93), Rotation2d.fromDegrees(67), 35, Seconds.of(1.016)), - new AimMeasurement(Meters.of(3.33), Rotation2d.fromDegrees(65), 37, Seconds.of(1.014)), - new AimMeasurement(Meters.of(3.81), Rotation2d.fromDegrees(61), 39, Seconds.of(1.07)), - new AimMeasurement(Meters.of(4.29), Rotation2d.fromDegrees(59), 39, Seconds.of(1.018)), - new AimMeasurement(Meters.of(4.77), Rotation2d.fromDegrees(59), 41.5 , Seconds.of(0.924)), - new AimMeasurement(Meters.of(5.26), Rotation2d.fromDegrees(58), 43.5 , Seconds.of(1.136)), - new AimMeasurement(Meters.of(5.73), Rotation2d.fromDegrees(55), 44.75, Seconds.of(1.15)), - new AimMeasurement(Meters.of(6.22), Rotation2d.fromDegrees(52), 47, Seconds.of(1.126)), - new AimMeasurement(Meters.of(6.84), Rotation2d.fromDegrees(50), 50, Seconds.of(1.134))); + new AimMeasurement(Meters.of(1.70), Rotation2d.fromDegrees(72), 28.25, Seconds.of(0.962)), + new AimMeasurement(Meters.of(2.41), Rotation2d.fromDegrees(70), 31.25, Seconds.of(1.006)), + new AimMeasurement(Meters.of(2.93), Rotation2d.fromDegrees(67), 32.25, Seconds.of(1.016)), + new AimMeasurement(Meters.of(3.33), Rotation2d.fromDegrees(65), 34.25, Seconds.of(1.014)), + new AimMeasurement(Meters.of(3.81), Rotation2d.fromDegrees(61), 35.75, Seconds.of(1.07)), + new AimMeasurement(Meters.of(4.23), Rotation2d.fromDegrees(60), 36.75, Seconds.of(1.0325)), + new AimMeasurement(Meters.of(4.77), Rotation2d.fromDegrees(59), 39.75, Seconds.of(1.122)), + new AimMeasurement(Meters.of(5.26), Rotation2d.fromDegrees(58), 40.25, Seconds.of(1.136)), + new AimMeasurement(Meters.of(5.73), Rotation2d.fromDegrees(55), 41.25, Seconds.of(1.15)), + new AimMeasurement(Meters.of(6.22), Rotation2d.fromDegrees(55), 43.25, Seconds.of(1.202)), + new AimMeasurement(Meters.of(6.84), Rotation2d.fromDegrees(53), 45.5, Seconds.of(1.265))); public static final List feedingMeasurements = List.of( - new AimMeasurement(Meters.of(1.70), Rotation2d.fromDegrees(72), 31, Seconds.of(0.962)), - new AimMeasurement(Meters.of(2.41), Rotation2d.fromDegrees(70), 34, Seconds.of(1.006)), - new AimMeasurement(Meters.of(2.93), Rotation2d.fromDegrees(67), 35, Seconds.of(1.016)), - new AimMeasurement(Meters.of(3.33), Rotation2d.fromDegrees(65), 37, Seconds.of(1.014)), - new AimMeasurement(Meters.of(3.81), Rotation2d.fromDegrees(61), 39, Seconds.of(1.07)), - new AimMeasurement(Meters.of(4.29), Rotation2d.fromDegrees(59), 39, Seconds.of(1.018)), - new AimMeasurement(Meters.of(4.77), Rotation2d.fromDegrees(59), 41.5 , Seconds.of(0.924)), - new AimMeasurement(Meters.of(5.26), Rotation2d.fromDegrees(58), 43.5 , Seconds.of(1.136)), - new AimMeasurement(Meters.of(5.73), Rotation2d.fromDegrees(55), 44.75, Seconds.of(1.15)), - new AimMeasurement(Meters.of(6.22), Rotation2d.fromDegrees(52), 47, Seconds.of(1.126)), - new AimMeasurement(Meters.of(6.84), Rotation2d.fromDegrees(50), 50, Seconds.of(1.134)), + new AimMeasurement(Meters.of(1.70), Rotation2d.fromDegrees(72), 29, Seconds.of(0)), + new AimMeasurement(Meters.of(2.41), Rotation2d.fromDegrees(70), 32, Seconds.of(1.006)), + new AimMeasurement(Meters.of(2.93), Rotation2d.fromDegrees(67), 33, Seconds.of(1.016)), + new AimMeasurement(Meters.of(3.33), Rotation2d.fromDegrees(65), 35, Seconds.of(1.014)), + new AimMeasurement(Meters.of(3.81), Rotation2d.fromDegrees(61), 36.5, Seconds.of(1.07)), + new AimMeasurement(Meters.of(4.23), Rotation2d.fromDegrees(60), 37.5, Seconds.of(1.018)), + new AimMeasurement(Meters.of(4.77), Rotation2d.fromDegrees(59), 40.5, Seconds.of(0.924)), + new AimMeasurement(Meters.of(5.26), Rotation2d.fromDegrees(58), 41, Seconds.of(1.136)), + new AimMeasurement(Meters.of(5.73), Rotation2d.fromDegrees(55), 42, Seconds.of(1.15)), + new AimMeasurement(Meters.of(6.22), Rotation2d.fromDegrees(55), 44, Seconds.of(1.126)), + new AimMeasurement(Meters.of(6.84), Rotation2d.fromDegrees(53), 46.25, Seconds.of(1.134)), new AimMeasurement(Meters.of(15), Rotation2d.fromDegrees(50), 80, Seconds.of(2))); } diff --git a/CompBot/src/main/java/frc/robot/subsystems/turret/TurretConstants.java b/CompBot/src/main/java/frc/robot/subsystems/turret/TurretConstants.java index 2255449..e8fa401 100644 --- a/CompBot/src/main/java/frc/robot/subsystems/turret/TurretConstants.java +++ b/CompBot/src/main/java/frc/robot/subsystems/turret/TurretConstants.java @@ -83,7 +83,7 @@ public class TurretConstants { .withKI(0) .withKD(0.1) .withKS(0.6) - .withKV(2.5) + .withKV(4.0) .withKA(0)) .withSoftwareLimitSwitch(new SoftwareLimitSwitchConfigs() diff --git a/CompBot/src/main/java/frc/robot/superstructure/StateManager.java b/CompBot/src/main/java/frc/robot/superstructure/StateManager.java index c2babbd..a4839f9 100644 --- a/CompBot/src/main/java/frc/robot/superstructure/StateManager.java +++ b/CompBot/src/main/java/frc/robot/superstructure/StateManager.java @@ -7,7 +7,9 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.AimConstants; import frc.robot.aiming.AimParams; @@ -41,6 +43,9 @@ public enum ShootMode { private SendableChooser autoChooser = new SendableChooser<>(); + private boolean partyModeEnabled = false; + public final Trigger partying = new Trigger(() -> partyModeEnabled); + public StateManager(Subsystems subsystems) { this.subsystems = subsystems; shootReady = initShootReady(); @@ -68,12 +73,14 @@ private ShootMode calculateWantedShootMode() { // If we're not in alliance zone, then all requirements for feeding have been met. // We want to feed. + boolean testing = DriverStation.isTestEnabled(); + if (!inAllianceZone) { - return FieldUtils.inNoFeedZone(robotPose()) ? ShootMode.Donut : ShootMode.Feeding; + return FieldUtils.inNoFeedZone(robotPose()) && !testing ? ShootMode.Donut : ShootMode.Feeding; } // By now, we're in a scoring position. - if (FieldUtils.inTowerZone(robotPose())) { + if (FieldUtils.inTowerZone(robotPose()) && !testing) { return ShootMode.Donut; } @@ -175,8 +182,12 @@ public Command getAuton() { public void update() { params = new AimParams(AimStatus.Unchecked); - params = aimParams(); wantedShootMode = calculateWantedShootMode(); + params = aimParams(); + SmartDashboard.putString("Wanted Shoot Mode", wantedShootMode.toString()); } + public Command runPartyMode(double duration) { + return Commands.startEnd(() -> partyModeEnabled = true, () -> partyModeEnabled = false).withTimeout(duration).ignoringDisable(true); + } } diff --git a/CompBot/src/main/java/frc/robot/superstructure/Superstructure.java b/CompBot/src/main/java/frc/robot/superstructure/Superstructure.java index 8ebbc2a..67ab225 100644 --- a/CompBot/src/main/java/frc/robot/superstructure/Superstructure.java +++ b/CompBot/src/main/java/frc/robot/superstructure/Superstructure.java @@ -1,9 +1,8 @@ package frc.robot.superstructure; import java.util.List; +import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; -import java.util.function.Supplier; - import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.util.Units; @@ -54,7 +53,7 @@ public record Subsystems( public final StateManager state; public Superstructure() { - subsystems = switch(RobotIdentifier.id()) { + subsystems = switch (RobotIdentifier.id()) { case CompBot -> createCompBotSubsystems(); case SimBot -> createSimBotSubsystems(); case TestBot -> createTestBotSubsystems(); @@ -99,8 +98,13 @@ private Subsystems createTestBotSubsystems() { return new Subsystems(drivetrain, turret, shooter, indexer, intake, climber, led); } - public void bindDrive(DoubleSupplier vx, DoubleSupplier vy, DoubleSupplier vrot, Supplier mode) { - subsystems.drivetrain.setDefaultCommand(subsystems.drivetrain.teleopDrive(vx, vy, vrot, () -> state.shooting(ShootMode.Scoring).getAsBoolean() ? TeleopDriveMode.SlowFieldRelativeSpin : mode.get())); + public void bindDrive(DoubleSupplier vx, DoubleSupplier vy, DoubleSupplier vrot, BooleanSupplier antislowbot) { + subsystems.drivetrain.setDefaultCommand(subsystems.drivetrain.teleopDrive(vx, vy, vrot, () -> { + if (state.shooting(ShootMode.Scoring).getAsBoolean()) { + return antislowbot.getAsBoolean() ? TeleopDriveMode.SlowFieldRelative : TeleopDriveMode.AccelerationLimitedFieldRelative; + } + return TeleopDriveMode.FieldRelative; + })); } /** @@ -127,7 +131,8 @@ public AprilTagVisionHandler createAprilTagVisionHandler() { LocalizationConstants.kRegularBaseCameraConfig.cameraCopy( "cam1", () -> new Transform3d(-0.207, -0.318, 0.473, - new Rotation3d(Units.degreesToRadians(0.7), Units.degreesToRadians(-28.578), Units.degreesToRadians(-67.63)))), + new Rotation3d(Units.degreesToRadians(0.7), Units.degreesToRadians(-28.578), + Units.degreesToRadians(-67.63)))), LocalizationConstants.kRegularBaseCameraConfig.cameraCopy( "cam2", () -> new Transform3d(0.221, -0.262, 0.724, @@ -139,8 +144,8 @@ public AprilTagVisionHandler createAprilTagVisionHandler() { LocalizationConstants.kRegularBaseCameraConfig.cameraCopy( "cam4", () -> new Transform3d(-0.315, 0.138, 0.438, - new Rotation3d(Units.degreesToRadians(-6.5), Units.degreesToRadians(-29.9), Units.degreesToRadians(-169)))) - ); + new Rotation3d(Units.degreesToRadians(-6.5), Units.degreesToRadians(-29.9), + Units.degreesToRadians(-169))))); return new AprilTagVisionHandler(this, configs); } diff --git a/CompBot/src/main/java/frc/robot/util/BetterAutoChooser.java b/CompBot/src/main/java/frc/robot/util/BetterAutoChooser.java index 7e9d51d..1023e93 100644 --- a/CompBot/src/main/java/frc/robot/util/BetterAutoChooser.java +++ b/CompBot/src/main/java/frc/robot/util/BetterAutoChooser.java @@ -16,7 +16,7 @@ public class BetterAutoChooser { private static final boolean RIGHT_IS_FLIPPED = false; private static final String DEFAULT_NAME = "None"; - private static final String PREFIX = "FLIP "; + private static final String FLIP_PREFIX = "FLIP "; private static final double TRANSLATION_ERROR = 0.5; // meters private static final double ROTATION_ERROR = 0.1; // rotations @@ -34,9 +34,9 @@ public static SendableChooser buildAutoChooser() { } for (String name : AutoBuilder.getAllAutoNames()) { - if (name.startsWith(PREFIX)) { + if (name.startsWith(FLIP_PREFIX)) { // Make the flipped paths - String remainder = name.substring(PREFIX.length()); + String remainder = name.substring(FLIP_PREFIX.length()); registerAuto("Right " + remainder + " (Generated)", new PathPlannerAuto(name, RIGHT_IS_FLIPPED)); registerAuto("Left " + remainder + " (Generated)", new PathPlannerAuto(name, !RIGHT_IS_FLIPPED)); } else { diff --git a/CompBot/src/main/java/frc/robot/util/ChassisSpeedRateLimiter.java b/CompBot/src/main/java/frc/robot/util/ChassisSpeedRateLimiter.java new file mode 100644 index 0000000..fd93075 --- /dev/null +++ b/CompBot/src/main/java/frc/robot/util/ChassisSpeedRateLimiter.java @@ -0,0 +1,48 @@ +package frc.robot.util; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.Timer; + +public class ChassisSpeedRateLimiter { + private ChassisSpeeds last; + private double lastTime; + + private final double accelerationLimit; + private final double angularAccelerationLimit; + + public ChassisSpeedRateLimiter(double accelerationLimit, double angularAccelerationLimit) { + this.accelerationLimit = accelerationLimit; + this.angularAccelerationLimit = angularAccelerationLimit; + } + + public ChassisSpeeds calculate(ChassisSpeeds input) { + // Time differences + double currentTime = Timer.getTimestamp(); + double dt = currentTime - lastTime; + // Cap 2d acceleration + double dx = input.vxMetersPerSecond - last.vxMetersPerSecond; + double dy = input.vyMetersPerSecond - last.vyMetersPerSecond; + double domega = MathUtil.angleModulus(input.omegaRadiansPerSecond - last.omegaRadiansPerSecond); + double hypot = Math.hypot(dx, dy); + if (hypot > accelerationLimit * dt) { + // divide by hypotenuse to normalize, then multiply by new length + dx *= accelerationLimit * dt / hypot; + dy *= accelerationLimit * dt / hypot; + } + domega = MathUtil.clamp( + domega, + -angularAccelerationLimit * dt, + angularAccelerationLimit * dt); + last = new ChassisSpeeds( + last.vxMetersPerSecond + dx, + last.vyMetersPerSecond + dy, + last.omegaRadiansPerSecond + domega); + lastTime = currentTime; + return last; + } + + public void reset(ChassisSpeeds seed) { + last = seed; + } +} diff --git a/CompBot/src/main/java/frc/robot/util/OnboardLogger.java b/CompBot/src/main/java/frc/robot/util/OnboardLogger.java index f7a2b11..38edada 100644 --- a/CompBot/src/main/java/frc/robot/util/OnboardLogger.java +++ b/CompBot/src/main/java/frc/robot/util/OnboardLogger.java @@ -5,10 +5,15 @@ import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import java.util.function.Supplier; + +import com.ctre.phoenix6.swerve.SwerveModule; + import edu.wpi.first.math.Pair; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.units.Measure; import edu.wpi.first.units.Unit; import edu.wpi.first.util.datalog.BooleanLogEntry; @@ -38,6 +43,8 @@ public class OnboardLogger { private final List, StructLogEntry>> pose3dEntries; private final List, StructArrayLogEntry>> pose3dArrayEntries; private final List, StructLogEntry>> transform2dEntries; + private final List, StructArrayLogEntry>> swerveModuleStateEntries; + private final List, StructArrayLogEntry>> swerveModulePositionEntries; public OnboardLogger(String name) { this.name = name; @@ -49,6 +56,8 @@ public OnboardLogger(String name) { pose3dEntries = new ArrayList<>(); pose3dArrayEntries = new ArrayList<>(); transform2dEntries = new ArrayList<>(); + swerveModuleStateEntries = new ArrayList<>(); + swerveModulePositionEntries = new ArrayList<>(); loggers.add(this); } @@ -105,6 +114,16 @@ public void registerPoses3d(String name, Supplier supplier) { pose3dArrayEntries.add(new Pair<>(supplier, entry)); } + public void registerSwerveModuleState(String name, Supplier supplier) { + StructArrayLogEntry entry = StructArrayLogEntry.create(datalog, this.name + "/" + name, SwerveModuleState.struct); + swerveModuleStateEntries.add(new Pair<>(supplier, entry)); + } + + public void registerSwerveModulePosition(String name, Supplier supplier) { + StructArrayLogEntry entry = StructArrayLogEntry.create(datalog, this.name + "/" + name, SwerveModulePosition.struct); + swerveModulePositionEntries.add(new Pair<>(supplier, entry)); + } + public void registerTransform2d(String name, Supplier supplier) { StructLogEntry entry = StructLogEntry.create(datalog, this.name + "/" + name, Transform2d.struct); @@ -136,6 +155,12 @@ private void log() { for (Pair, StructLogEntry> pair : transform2dEntries) { pair.getSecond().update(pair.getFirst().get()); } + for (Pair, StructArrayLogEntry> pair : swerveModuleStateEntries) { + pair.getSecond().update(pair.getFirst().get()); + } + for (Pair, StructArrayLogEntry> pair : swerveModulePositionEntries) { + pair.getSecond().update(pair.getFirst().get()); + } } public static void logAll() { diff --git a/CompBot/src/main/java/frc/robot/util/TriggerSequence.java b/CompBot/src/main/java/frc/robot/util/TriggerSequence.java index 107a388..8732c37 100644 --- a/CompBot/src/main/java/frc/robot/util/TriggerSequence.java +++ b/CompBot/src/main/java/frc/robot/util/TriggerSequence.java @@ -20,7 +20,7 @@ private TriggerSequence(Trigger[] triggers, int[] sequence) { count = 0; for (int i = 0; i < triggers.length; i++) { - int j = i; + int j = i; // j is the index of the trigger! triggers[j].onTrue(Commands.runOnce(() -> { if (count == sequence.length) { count = 0; @@ -30,7 +30,7 @@ private TriggerSequence(Trigger[] triggers, int[] sequence) { } else { count = 0; } - })); + }).ignoringDisable(true)); } } @@ -65,10 +65,15 @@ public static Trigger fromController(CommandGenericHID controller, int... sequen // We already have an ID for this one. realSequence[i] = seenTriggers.get(triggerId); } else { + // We haven't seen this trigger before + // The index of the newest item in triggers is simply the size of the current seenTriggers (or triggers) list realSequence[i] = seenTriggers.size(); - seenTriggers.put(sequence[i], realSequence[i]); + seenTriggers.put(triggerId, realSequence[i]); + Trigger button = controller.button(triggerId); + triggers.add(button); } } - return create(triggers.toArray(new Trigger[] {}), realSequence); + Trigger[] triggerArray = triggers.toArray(new Trigger[] {}); + return create(triggerArray, realSequence); } } diff --git a/CompBot/src/main/java/frc/robot/vision/localization/LocalizationConstants.java b/CompBot/src/main/java/frc/robot/vision/localization/LocalizationConstants.java index ec61914..1bbac8b 100644 --- a/CompBot/src/main/java/frc/robot/vision/localization/LocalizationConstants.java +++ b/CompBot/src/main/java/frc/robot/vision/localization/LocalizationConstants.java @@ -24,7 +24,7 @@ public class LocalizationConstants { Degrees.of(77.4), // Horizontal FOV () -> Transform3d.kZero, // Pose supplier new CameraTrustConfig( - VecBuilder.fill(0.3, 0.3, 0.3), // Base std devs + VecBuilder.fill(0.1, 0.1, 0.1), // Base std devs 0.75, // Latency threshold 1.3, // Latency multiplier 2.5, // Field XY margin @@ -78,15 +78,15 @@ public class LocalizationConstants { protected static final double kAvgErr = 0.08; protected static final double kErrStdDevs = 0.02; - public static final boolean kEnableTagFilter = false; + public static final boolean kEnableTagFilter = true; - protected static final Set kApprovedTagIds = Set.of(2, 3, 4, 5, 8, 9, 10, 11); + protected static final Set kApprovedTagIds = Set.of(2,3,4,5,8,9,10,11, 18,19,20,21,24,25,26,27, 15,16,13,14,29,30); public static final Transform3d kTurretAoRToTurretCameraOffset = new Transform3d(0.064, -0.02, 0.038, new Rotation3d(0, Units.degreesToRadians(-30), 0)); /** Maximum time since last pose estimate for odometry to be considered valid */ public static final double kValidOdometryCutoff = 0.5; - + public static final boolean kUsePnPDistanceTrigSolve = false; }