diff --git a/src/main/deploy/pathplanner/autos/DRUM-Shoot Preload.auto b/src/main/deploy/pathplanner/autos/DRUM-Shoot Preload.auto new file mode 100644 index 00000000..fcbe6622 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/DRUM-Shoot Preload.auto @@ -0,0 +1,38 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "hub-center shot" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 5.0 + } + }, + { + "type": "named", + "data": { + "name": "VariableShot" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/DRUM-Trench FL double bump.auto b/src/main/deploy/pathplanner/autos/DRUM-Trench FL double bump.auto index ef5a5924..6bb8e7d8 100644 --- a/src/main/deploy/pathplanner/autos/DRUM-Trench FL double bump.auto +++ b/src/main/deploy/pathplanner/autos/DRUM-Trench FL double bump.auto @@ -11,7 +11,7 @@ { "type": "wait", "data": { - "waitTime": 8.0 + "waitTime": 8.5 } }, { @@ -169,12 +169,6 @@ } ] } - }, - { - "type": "named", - "data": { - "name": "ExtendIntake" - } } ] } diff --git a/src/main/deploy/pathplanner/autos/DRUM-Trench FR double bump.auto b/src/main/deploy/pathplanner/autos/DRUM-Trench FR double bump.auto index f2efa27d..7c7a380a 100644 --- a/src/main/deploy/pathplanner/autos/DRUM-Trench FR double bump.auto +++ b/src/main/deploy/pathplanner/autos/DRUM-Trench FR double bump.auto @@ -11,7 +11,7 @@ { "type": "wait", "data": { - "waitTime": 8.0 + "waitTime": 8.5 } }, { diff --git a/src/main/deploy/pathplanner/autos/DEPRECATED_center depot.auto b/src/main/deploy/pathplanner/autos/DRUM-center depot.auto similarity index 95% rename from src/main/deploy/pathplanner/autos/DEPRECATED_center depot.auto rename to src/main/deploy/pathplanner/autos/DRUM-center depot.auto index 9368e271..de189b7d 100644 --- a/src/main/deploy/pathplanner/autos/DEPRECATED_center depot.auto +++ b/src/main/deploy/pathplanner/autos/DRUM-center depot.auto @@ -17,7 +17,7 @@ { "type": "path", "data": { - "pathName": "DEPRECATED_hub-depot" + "pathName": "hub-depot" } }, { @@ -42,7 +42,7 @@ { "type": "path", "data": { - "pathName": "DEPRECATED_depot-center_shot" + "pathName": "depot-center_shot" } }, { diff --git a/src/main/deploy/pathplanner/autos/WIP-DRUM-Trench FL 2.5 bump.auto b/src/main/deploy/pathplanner/autos/WIP-DRUM-Trench FL 2.5 bump.auto new file mode 100644 index 00000000..f7ced68c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/WIP-DRUM-Trench FL 2.5 bump.auto @@ -0,0 +1,185 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 8.5 + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SpinRollerIntake" + } + }, + { + "type": "path", + "data": { + "pathName": "Trench FL-mid-bump-left shot pos" + } + }, + { + "type": "named", + "data": { + "name": "ExtendIntake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StopRoller" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + }, + { + "type": "named", + "data": { + "name": "RetractIntake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "VariableShot" + } + } + ] + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 10.5 + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "left shot pos-trench-hairpin-bump-left shot pos" + } + }, + { + "type": "named", + "data": { + "name": "ExtendIntake" + } + }, + { + "type": "named", + "data": { + "name": "SpinRollerIntake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StopRoller" + } + }, + { + "type": "named", + "data": { + "name": "VariableShot" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + }, + { + "type": "named", + "data": { + "name": "RetractIntake" + } + } + ] + } + } + ] + } + } + ] + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "left 2.5 pass" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/WIP-DRUM-Trench FR 2.5 bump.auto b/src/main/deploy/pathplanner/autos/WIP-DRUM-Trench FR 2.5 bump.auto new file mode 100644 index 00000000..a3fa3bca --- /dev/null +++ b/src/main/deploy/pathplanner/autos/WIP-DRUM-Trench FR 2.5 bump.auto @@ -0,0 +1,185 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 8.5 + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "SpinRollerIntake" + } + }, + { + "type": "path", + "data": { + "pathName": "Trench FR-mid-bump-right shot pos" + } + }, + { + "type": "named", + "data": { + "name": "ExtendIntake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StopRoller" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + }, + { + "type": "named", + "data": { + "name": "RetractIntake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "VariableShot" + } + } + ] + } + } + ] + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 10.5 + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "right shot pos-trench-hairpin-bump-right shot pos" + } + }, + { + "type": "named", + "data": { + "name": "ExtendIntake" + } + }, + { + "type": "named", + "data": { + "name": "SpinRollerIntake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "StopRoller" + } + }, + { + "type": "named", + "data": { + "name": "VariableShot" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + }, + { + "type": "named", + "data": { + "name": "RetractIntake" + } + } + ] + } + } + ] + } + } + ] + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "right 2.5 pass" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Trench FL-mid-bump-left shot pos.path b/src/main/deploy/pathplanner/paths/Trench FL-mid-bump-left shot pos.path index 4e9c5e21..c36c18a0 100644 --- a/src/main/deploy/pathplanner/paths/Trench FL-mid-bump-left shot pos.path +++ b/src/main/deploy/pathplanner/paths/Trench FL-mid-bump-left shot pos.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.45853476821192, - "y": 7.563327814569536 + "x": 4.466034236804564, + "y": 7.521968616262482 }, "prevControl": null, "nextControl": { - "x": 8.567971527836313, - "y": 7.563327814569536 + "x": 8.541711840228244, + "y": 6.422182596291013 }, "isLocked": false, "linkedName": "DRUM-Trench FL" @@ -20,12 +20,12 @@ "y": 5.719304635761589 }, "prevControl": { - "x": 8.089384464706532, - "y": 7.000986826917645 + "x": 8.101797432239657, + "y": 6.034022824536376 }, "nextControl": { - "x": 8.132061258278146, - "y": 4.383476821192053 + "x": 8.146283780905618, + "y": 4.383784458978991 }, "isLocked": false, "linkedName": null @@ -94,8 +94,8 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.5151793928242889, - "rotationDegrees": -90.0 + "waypointRelativePos": 0.40029112081513835, + "rotationDegrees": -45.0 }, { "waypointRelativePos": 1.0855565777369083, @@ -113,10 +113,10 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.8363448631905099, + "minWaypointRelativePos": 0.19021739130434828, "maxWaypointRelativePos": 1.8688693856479086, "constraints": { - "maxVelocity": 3.0, + "maxVelocity": 2.5, "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -143,7 +143,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -90.0 + "rotation": 0.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Trench FR-mid-bump-right shot pos.path b/src/main/deploy/pathplanner/paths/Trench FR-mid-bump-right shot pos.path index 04bc272e..49f1fe27 100644 --- a/src/main/deploy/pathplanner/paths/Trench FR-mid-bump-right shot pos.path +++ b/src/main/deploy/pathplanner/paths/Trench FR-mid-bump-right shot pos.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.480314569536423, - "y": 0.46311258278145717 + "x": 4.453095577746077, + "y": 0.5221540656205423 }, "prevControl": null, "nextControl": { - "x": 8.066721854304635, - "y": 0.4921523178807956 + "x": 7.855962910128387, + "y": 1.337289586305278 }, "isLocked": false, "linkedName": "DRUM-Trench FR" @@ -20,12 +20,12 @@ "y": 2.212756622516557 }, "prevControl": { - "x": 8.132061258278146, - "y": 1.2956743856436232 + "x": 8.14061340941512, + "y": 1.7383880171184036 }, "nextControl": { - "x": 8.132061258278146, - "y": 3.6653966245756404 + "x": 8.105876603009214, + "y": 3.6651606088133 }, "isLocked": false, "linkedName": null @@ -73,7 +73,7 @@ }, "nextControl": { "x": 2.0264569536423838, - "y": 2.466854304635761 + "y": 2.4668543046357607 }, "isLocked": false, "linkedName": null @@ -94,7 +94,11 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.9751609935602532, + "waypointRelativePos": 0.39058709364387384, + "rotationDegrees": 45.0 + }, + { + "waypointRelativePos": 1.137797185832109, "rotationDegrees": 90.0 }, { @@ -109,10 +113,10 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.8363448631905099, + "minWaypointRelativePos": 0.19798136645962736, "maxWaypointRelativePos": 1.8688693856479086, "constraints": { - "maxVelocity": 3.0, + "maxVelocity": 2.5, "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -139,7 +143,7 @@ "folder": null, "idealStartingState": { "velocity": 0.0, - "rotation": 90.0 + "rotation": 0.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/DEPRECATED_depot-center_shot.path b/src/main/deploy/pathplanner/paths/depot-center_shot.path similarity index 98% rename from src/main/deploy/pathplanner/paths/DEPRECATED_depot-center_shot.path rename to src/main/deploy/pathplanner/paths/depot-center_shot.path index 57cccbbf..2d858046 100644 --- a/src/main/deploy/pathplanner/paths/DEPRECATED_depot-center_shot.path +++ b/src/main/deploy/pathplanner/paths/depot-center_shot.path @@ -42,7 +42,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/hub-center shot.path b/src/main/deploy/pathplanner/paths/hub-center shot.path new file mode 100644 index 00000000..caa53c95 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/hub-center shot.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5467141162514837, + "y": 4.028530670470756 + }, + "prevControl": null, + "nextControl": { + "x": 2.658714116251484, + "y": 4.028530670470756 + }, + "isLocked": false, + "linkedName": "hub-backwards" + }, + { + "anchor": { + "x": 2.680499286733238, + "y": 4.028530670470756 + }, + "prevControl": { + "x": 3.672686350191503, + "y": 4.028530670470756 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "center_shot" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/DEPRECATED_hub-depot.path b/src/main/deploy/pathplanner/paths/hub-depot.path similarity index 91% rename from src/main/deploy/pathplanner/paths/DEPRECATED_hub-depot.path rename to src/main/deploy/pathplanner/paths/hub-depot.path index 978fb59f..0d432178 100644 --- a/src/main/deploy/pathplanner/paths/DEPRECATED_hub-depot.path +++ b/src/main/deploy/pathplanner/paths/hub-depot.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.508573466476462, - "y": 4.054407988587732 + "x": 3.5467141162514837, + "y": 4.028530670470756 }, "prevControl": null, "nextControl": { - "x": 3.612082738940132, - "y": 5.464721825963732 + "x": 3.650223388715154, + "y": 5.438844507846756 }, "isLocked": false, "linkedName": "hub-backwards" diff --git a/src/main/deploy/pathplanner/paths/left 2.5 pass.path b/src/main/deploy/pathplanner/paths/left 2.5 pass.path new file mode 100644 index 00000000..0789cdd9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/left 2.5 pass.path @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.9339486754966884, + "y": 5.051390728476821 + }, + "prevControl": null, + "nextControl": { + "x": 1.6643285940496975, + "y": 6.549526550648494 + }, + "isLocked": false, + "linkedName": "DRUM-Left Shot pos" + }, + { + "anchor": { + "x": 3.8802491103202854, + "y": 7.461803084223013 + }, + "prevControl": { + "x": 3.0948279952550424, + "y": 7.504839857651245 + }, + "nextControl": { + "x": 4.2063337147220725, + "y": 7.443935434666751 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.5915658362989324, + "y": 7.257378410438909 + }, + "prevControl": { + "x": 5.246948166589718, + "y": 7.610325359883776 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.767213114754099, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.255081967213113, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 6.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 6.0, + "rotation": -46.46880071438582 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 144.37209270857912 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/left shot pos-trench-hairpin-bump-left shot pos.path b/src/main/deploy/pathplanner/paths/left shot pos-trench-hairpin-bump-left shot pos.path index 64d0c095..d2b5f6d6 100644 --- a/src/main/deploy/pathplanner/paths/left shot pos-trench-hairpin-bump-left shot pos.path +++ b/src/main/deploy/pathplanner/paths/left shot pos-trench-hairpin-bump-left shot pos.path @@ -64,15 +64,15 @@ }, { "anchor": { - "x": 6.925266666666667, + "x": 7.5, "y": 4.4778559602649 }, "prevControl": { - "x": 6.227601890664112, + "x": 6.802335223997445, "y": 4.492492284236983 }, "nextControl": { - "x": 7.963437196467992, + "x": 8.538170529801324, "y": 4.456076158940396 }, "isLocked": false, @@ -80,16 +80,32 @@ }, { "anchor": { - "x": 7.079370860927153, - "y": 5.588625827814569 + "x": 7.5, + "y": 6.0 + }, + "prevControl": { + "x": 8.551669676211034, + "y": 5.973598250806417 + }, + "nextControl": { + "x": 7.000157485830182, + "y": 6.012548347636063 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.13604056407916, + "y": 5.604960678807947 }, "prevControl": { - "x": 8.131040537138187, - "y": 5.562224078620987 + "x": 6.13604056407916, + "y": 5.604960678807947 }, "nextControl": { - "x": 5.344246688741721, - "y": 5.632185430463576 + "x": 4.821040564079159, + "y": 5.604960678807947 }, "isLocked": false, "linkedName": null @@ -146,7 +162,7 @@ "rotationDegrees": 180.0 }, { - "waypointRelativePos": 6.027598896044192, + "waypointRelativePos": 7.027598896044192, "rotationDegrees": 180.0 } ], @@ -154,7 +170,7 @@ { "name": "Constraints Zone", "minWaypointRelativePos": 2.6742385131646964, - "maxWaypointRelativePos": 5.109963861641695, + "maxWaypointRelativePos": 5.2, "constraints": { "maxVelocity": 3.0, "maxAcceleration": 5.0, diff --git a/src/main/deploy/pathplanner/paths/right 2.5 pass.path b/src/main/deploy/pathplanner/paths/right 2.5 pass.path new file mode 100644 index 00000000..23004a3c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/right 2.5 pass.path @@ -0,0 +1,79 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.970248344370861, + "y": 3.018609271523179 + }, + "prevControl": null, + "nextControl": { + "x": 1.5239857651245556, + "y": 2.0284104389086597 + }, + "isLocked": false, + "linkedName": "DRUM-Right Shot Pos" + }, + { + "anchor": { + "x": 3.535954922894425, + "y": 0.6727520759193351 + }, + "prevControl": { + "x": 1.2550059311981019, + "y": 0.7588256227757985 + }, + "nextControl": { + "x": 5.826436936969747, + "y": 0.5863187923693246 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.193475682087783, + "y": 0.9202135231316719 + }, + "prevControl": { + "x": 5.311221826809016, + "y": 0.6512336892052202 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.8118032786885311, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.3363934426229522, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 6.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 6.0, + "rotation": 50.629803777985515 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -145.40771131248994 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/right shot pos-trench-hairpin-bump-right shot pos.path b/src/main/deploy/pathplanner/paths/right shot pos-trench-hairpin-bump-right shot pos.path index 25a1ba7a..788e4cff 100644 --- a/src/main/deploy/pathplanner/paths/right shot pos-trench-hairpin-bump-right shot pos.path +++ b/src/main/deploy/pathplanner/paths/right shot pos-trench-hairpin-bump-right shot pos.path @@ -64,15 +64,15 @@ }, { "anchor": { - "x": 7.195529801324503, + "x": 7.5, "y": 3.5485844370860926 }, "prevControl": { - "x": 6.497865025321948, + "x": 6.802335223997445, "y": 3.563220761058175 }, "nextControl": { - "x": 8.23370033112582, + "x": 8.538170529801317, "y": 3.5268046357615885 }, "isLocked": false, @@ -80,15 +80,31 @@ }, { "anchor": { - "x": 7.195529801324503, + "x": 7.5, + "y": 1.9 + }, + "prevControl": { + "x": 8.552001026721122, + "y": 1.9 + }, + "nextControl": { + "x": 7.0, + "y": 1.9 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.0, "y": 2.423294701986755 }, "prevControl": { - "x": 8.247530828045626, + "x": 6.314246792138827, "y": 2.423294701986755 }, "nextControl": { - "x": 5.459858942006666, + "x": 5.685753207861173, "y": 2.423294701986755 }, "isLocked": false, @@ -146,7 +162,7 @@ "rotationDegrees": 180.0 }, { - "waypointRelativePos": 6.027598896044192, + "waypointRelativePos": 7.027598896044192, "rotationDegrees": 180.0 } ], diff --git a/src/main/java/com/team2813/RobotContainer.java b/src/main/java/com/team2813/RobotContainer.java index 2b3a4237..706d87ab 100644 --- a/src/main/java/com/team2813/RobotContainer.java +++ b/src/main/java/com/team2813/RobotContainer.java @@ -237,7 +237,8 @@ public RobotContainer( * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - // shooter idle command (runs motors at 1V unless controlled by another subsystem) + // shooter idle command (runs motors at the preference velocity if no other commands use + // shooter). shooter.setDefaultCommand(shooter.idleCommand()); // Operator controls // Operator Intake roller Bindings @@ -266,23 +267,35 @@ private void configureButtonBindings() { drive.stopTowardPoint( HubPositionUtil.getBotToHubAngle(drive.getPose(), currentAlliance)))); - // Feeder controls - operatorController - .leftBumper() - .whileTrue(Commands.parallel(hopper.outtakeCommand(), kicker.outtakeCommand())); - operatorController.povLeft().whileTrue(hopper.intakeCommand()); - - // Operator intake roller bindings. - operatorController.povRight().whileTrue(intakeRoller.intakeCommand()); - + // manual intake for operator + operatorController.povLeft().whileTrue(intakeExtension.retractCommand()); + operatorController.povRight().whileTrue(intakeExtension.extendCommand()); // Spool shooter commands // TODO: add the following controls and hardcoded values - // operator x: spool to hub speed (move hood down) - // operator y: spool to trench speed (move hood down) - // operator b: spool to herd speed (move hood up) - // operator a: spool to tower speed (move hood down) - operatorController.x().whileTrue(shooter.spoolShooterHubSpeedCommand()); - operatorController.y().whileTrue(shooter.spoolShooterHerdSpeedCommand()); + operatorController + .x() + .whileTrue( + shooter + .spoolShooterHubSpeedCommand() + .alongWith(hood.goToAngleCommand(HoodConstants.MINIMUM_SHOOTER_ANGLE))); + operatorController + .y() + .whileTrue( + shooter + .spoolShooterTrenchSpeedCommand() + .alongWith(hood.goToAngleCommand(HoodConstants.MINIMUM_SHOOTER_ANGLE))); + operatorController + .b() + .whileTrue( + shooter + .spoolShooterHerdSpeedCommand() + .alongWith(hood.goToAngleCommand(HoodConstants.MAXIMUM_SHOOTER_ANGLE))); + operatorController + .a() + .whileTrue( + shooter + .spoolShooterTowerSpeedCommand() + .alongWith(hood.goToAngleCommand(HoodConstants.MINIMUM_SHOOTER_ANGLE))); // Hood controls operatorController .povDown() @@ -333,8 +346,9 @@ private void configureButtonBindings() { () -> HubPositionUtil.getBotToHubAngle(drive.getPose(), currentAlliance))) .whileTrue( VariableShooterCommand.shootBasedOnDistanceCommand( - shooter, - () -> HubPositionUtil.getBotToHubDistance(drive.getPose(), currentAlliance))); + shooter, + () -> HubPositionUtil.getBotToHubDistance(drive.getPose(), currentAlliance)) + .alongWith(hood.goToAngleCommand(HoodConstants.MINIMUM_SHOOTER_ANGLE))); // temporary drum test binding // driveController.b().whileTrue(shooter.outakeCommand()); diff --git a/src/main/java/com/team2813/subsystems/shooter/Shooter.java b/src/main/java/com/team2813/subsystems/shooter/Shooter.java index 5fe9e6ff..bccb8803 100644 --- a/src/main/java/com/team2813/subsystems/shooter/Shooter.java +++ b/src/main/java/com/team2813/subsystems/shooter/Shooter.java @@ -54,7 +54,7 @@ public Command spoolCustomVelocityCommand(AngularVelocity velocity) { public Command idleCommand() { return new StartEndCommand( - () -> io.setShooterMotorVoltage(ShooterConstants.getIdleVoltage()), this::stop, this); + () -> io.setShooterMotorVelocity(ShooterConstants.getIdleVelocity()), this::stop, this); } // Instructions taken from https://docs.advantagekit.org/data-flow/sysid-compatibility/ and diff --git a/src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java b/src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java index d00170aa..4630641b 100644 --- a/src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/com/team2813/subsystems/shooter/ShooterConstants.java @@ -28,7 +28,7 @@ public class ShooterConstants { public static final String SHOOTER_OUTTAKE_PREFERENCE_NT = "Shooter/SHOOTER_OUTTAKE_VOLTAGE"; - public static final String SHOOTER_IDLE_PREFERENCE_NT = "Shooter/SHOOTER_IDLING_VOLTAGE"; + public static final String SHOOTER_IDLE_PREFERENCE_NT = "Shooter/SHOOTER_IDLING_VELOCITY"; public static final double SHOOTER_SIM_MOI = 0.00303431; // in kilograms*meters squared. @@ -47,12 +47,12 @@ public class ShooterConstants { static { // Shooter motors. - Preferences.initDouble(SHOOTER_TRENCH_SHOOT_PREFERENCE_NT, 100); - Preferences.initDouble(SHOOTER_HUB_SHOOT_PREFERENCE_NT, 60); + Preferences.initDouble(SHOOTER_TRENCH_SHOOT_PREFERENCE_NT, 50); + Preferences.initDouble(SHOOTER_HUB_SHOOT_PREFERENCE_NT, 36); Preferences.initDouble(SHOOTER_HERD_SHOOT_PREFERENCE_NT, 55); - Preferences.initDouble(SHOOTER_TOWER_SHOOT_PREFERENCE_NT, 40); + Preferences.initDouble(SHOOTER_TOWER_SHOOT_PREFERENCE_NT, 44.5); Preferences.initDouble(SHOOTER_OUTTAKE_PREFERENCE_NT, -5); - Preferences.initDouble(SHOOTER_IDLE_PREFERENCE_NT, 0); + Preferences.initDouble(SHOOTER_IDLE_PREFERENCE_NT, 15); } public static final Current SHOOTER_STATOR_LIMIT = Amps.of(60); @@ -106,11 +106,11 @@ public class ShooterConstants { public static final double SHOOTER_MOTOR_TO_FLYWHEEL_GEARING = 1.0; public static AngularVelocity getShooterTrenchShootVelocity() { - return RotationsPerSecond.of(Preferences.getDouble(SHOOTER_TRENCH_SHOOT_PREFERENCE_NT, 100)); + return RotationsPerSecond.of(Preferences.getDouble(SHOOTER_TRENCH_SHOOT_PREFERENCE_NT, 50)); } public static AngularVelocity getShooterHubShootVelocity() { - return RotationsPerSecond.of(Preferences.getDouble(SHOOTER_HUB_SHOOT_PREFERENCE_NT, 60)); + return RotationsPerSecond.of(Preferences.getDouble(SHOOTER_HUB_SHOOT_PREFERENCE_NT, 36)); } public static AngularVelocity getShooterHerdShootVelocity() { @@ -118,14 +118,14 @@ public static AngularVelocity getShooterHerdShootVelocity() { } public static AngularVelocity getShooterTowerShootVelocity() { - return RotationsPerSecond.of(Preferences.getDouble(SHOOTER_TOWER_SHOOT_PREFERENCE_NT, 40)); + return RotationsPerSecond.of(Preferences.getDouble(SHOOTER_TOWER_SHOOT_PREFERENCE_NT, 44.5)); } public static Voltage getShooterOuttakeVoltage() { return Volts.of(Preferences.getDouble(SHOOTER_OUTTAKE_PREFERENCE_NT, -5)); } - public static Voltage getIdleVoltage() { - return Volts.of(Preferences.getDouble(SHOOTER_IDLE_PREFERENCE_NT, 0)); + public static AngularVelocity getIdleVelocity() { + return RotationsPerSecond.of(Preferences.getDouble(SHOOTER_IDLE_PREFERENCE_NT, 15)); } } diff --git a/src/main/java/com/team2813/subsystems/vision/VisionConstants.java b/src/main/java/com/team2813/subsystems/vision/VisionConstants.java index 6084d0ad..3d4d573f 100644 --- a/src/main/java/com/team2813/subsystems/vision/VisionConstants.java +++ b/src/main/java/com/team2813/subsystems/vision/VisionConstants.java @@ -52,6 +52,12 @@ public class VisionConstants { new Translation3d(Inches.of(-13.263), Centimeters.of(0), Inches.of(14.028)), new Rotation3d(Degrees.of(0), Degrees.of(-15), Degrees.of(180))); + // static { + // Logger.recordOutput("VisTest/Red", RED_BACK_LEFT_CAM_FROM_ROBOT); + // Logger.recordOutput("VisTest/Green", GREEN_BACK_RIGHT_CAM_FROM_ROBOT); + // Logger.recordOutput("VisTest/Blue", BLUE_FRONT_CAM_FROM_ROBOT); + // } + // Basic filtering thresholds public static double maxAmbiguity = 0.3; public static double maxZError = 0.75;