diff --git a/src/main/deploy/pathplanner/autos/greedy left center 2 pass.auto b/src/main/deploy/pathplanner/autos/greedy left center 2 pass.auto new file mode 100644 index 00000000..9a2bc29e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/greedy left center 2 pass.auto @@ -0,0 +1,235 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "greedy left trench - left mid neutral" + } + }, + { + "type": "named", + "data": { + "name": "ExtendIntake" + } + }, + { + "type": "named", + "data": { + "name": "SpinRollerIntake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "greedy netural zone-left midfield" + } + }, + { + "type": "named", + "data": { + "name": "StopRoller" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "VariableShot" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "HalfwayIntake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "RetractIntake" + } + } + ] + } + } + ] + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 9.5 + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "greedy midfield-center pass 2" + } + }, + { + "type": "named", + "data": { + "name": "ExtendIntake" + } + }, + { + "type": "named", + "data": { + "name": "SpinRollerIntake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "greedy center pass 2-bump-midfield" + } + }, + { + "type": "named", + "data": { + "name": "StopRoller" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "VariableShot" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "HalfwayIntake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "RetractIntake" + } + } + ] + } + } + ] + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 10.0 + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ExtendIntake" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/left-mid double trench cycle.auto b/src/main/deploy/pathplanner/autos/left center 2 pass.auto similarity index 64% rename from src/main/deploy/pathplanner/autos/left-mid double trench cycle.auto rename to src/main/deploy/pathplanner/autos/left center 2 pass.auto index dc80b411..fb80ac87 100644 --- a/src/main/deploy/pathplanner/autos/left-mid double trench cycle.auto +++ b/src/main/deploy/pathplanner/autos/left center 2 pass.auto @@ -4,12 +4,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "TrenchShot" - } - }, { "type": "race", "data": { @@ -50,7 +44,7 @@ { "type": "path", "data": { - "pathName": "left mid neutral - left trench" + "pathName": "netural zone-left midfield" } }, { @@ -63,19 +57,44 @@ } }, { - "type": "race", + "type": "parallel", "data": { "commands": [ { "type": "named", "data": { - "name": "TrenchShot" + "name": "VariableShot" } }, { - "type": "named", + "type": "sequential", "data": { - "name": "WalleMode" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "HalfwayIntake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "RetractIntake" + } + } + ] } } ] @@ -87,7 +106,7 @@ { "type": "wait", "data": { - "waitTime": 9.0 + "waitTime": 9.5 } } ] @@ -108,7 +127,7 @@ { "type": "path", "data": { - "pathName": "left trench - left mid neutral" + "pathName": "midfield-center pass 2" } }, { @@ -133,7 +152,7 @@ { "type": "path", "data": { - "pathName": "left mid neutral - left trench" + "pathName": "center pass 2-bump-midfield" } }, { @@ -146,19 +165,44 @@ } }, { - "type": "race", + "type": "parallel", "data": { "commands": [ { "type": "named", "data": { - "name": "TrenchShot" + "name": "VariableShot" } }, { - "type": "named", + "type": "sequential", "data": { - "name": "WalleMode" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "HalfwayIntake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "named", + "data": { + "name": "RetractIntake" + } + } + ] } } ] diff --git a/src/main/deploy/pathplanner/autos/left trench-mid intake-trenchshot.auto b/src/main/deploy/pathplanner/autos/left trench-mid intake-trenchshot.auto deleted file mode 100644 index bb6a88c6..00000000 --- a/src/main/deploy/pathplanner/autos/left trench-mid intake-trenchshot.auto +++ /dev/null @@ -1,107 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "TrenchShot" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "left trench - left mid neutral" - } - }, - { - "type": "named", - "data": { - "name": "ExtendIntake" - } - }, - { - "type": "named", - "data": { - "name": "SpinRollerIntake" - } - } - ] - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "left mid neutral - left trench" - } - }, - { - "type": "named", - "data": { - "name": "StopRoller" - } - } - ] - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "TrenchShot" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 1.0 - } - }, - { - "type": "named", - "data": { - "name": "HalfwayIntake" - } - }, - { - "type": "named", - "data": { - "name": "RetractIntake" - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "ExtendIntake" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/center pass 2-bump-midfield.path b/src/main/deploy/pathplanner/paths/center pass 2-bump-midfield.path new file mode 100644 index 00000000..b573c16d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/center pass 2-bump-midfield.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.810753311258278, + "y": 4.775513245033112 + }, + "prevControl": null, + "nextControl": { + "x": 6.033940397350993, + "y": 6.183940397350994 + }, + "isLocked": false, + "linkedName": "center pass 2" + }, + { + "anchor": { + "x": 2.8228555555555555, + "y": 6.198266666666666 + }, + "prevControl": { + "x": 2.597797608530747, + "y": 4.57204150110484 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "left midfield" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.08935064935064813, + "maxWaypointRelativePos": 0.6316883116883086, + "constraints": { + "maxVelocity": 6.0, + "maxAcceleration": 10.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -54.32359177813802 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/greedy center pass 2-bump-midfield.path b/src/main/deploy/pathplanner/paths/greedy center pass 2-bump-midfield.path new file mode 100644 index 00000000..4f620411 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/greedy center pass 2-bump-midfield.path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.403109843081312, + "y": 2.9287446504992873 + }, + "prevControl": null, + "nextControl": { + "x": 6.872624821682827, + "y": 4.494322396569266 + }, + "isLocked": false, + "linkedName": "greedy center pass 2" + }, + { + "anchor": { + "x": 6.2515691868754075, + "y": 5.904636233944443 + }, + "prevControl": { + "x": 7.196077088227971, + "y": 5.832306433544779 + }, + "nextControl": { + "x": 5.307061285522844, + "y": 5.976966034344108 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.8228555555555555, + "y": 6.198266666666666 + }, + "prevControl": { + "x": 4.090813124107933, + "y": 5.231825962903074 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "left midfield" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 7.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -54.32359177813802 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/greedy left trench - left mid neutral.path b/src/main/deploy/pathplanner/paths/greedy left trench - left mid neutral.path new file mode 100644 index 00000000..658ff293 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/greedy left trench - left mid neutral.path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.478972895863052, + "y": 7.625477888730385 + }, + "prevControl": null, + "nextControl": { + "x": 5.402326158940398, + "y": 7.577847682119204 + }, + "isLocked": true, + "linkedName": "left trench" + }, + { + "anchor": { + "x": 7.157275320970041, + "y": 7.314950071326677 + }, + "prevControl": { + "x": 6.945825829995666, + "y": 7.44832590409513 + }, + "nextControl": { + "x": 7.790664209858929, + "y": 6.915427849104453 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.20530670470756, + "y": 4.2484878744650505 + }, + "prevControl": { + "x": 8.169007035833388, + "y": 5.03256072214717 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "greedy left mid neutral" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 7.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -70.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -90.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/greedy midfield-center pass 2.path b/src/main/deploy/pathplanner/paths/greedy midfield-center pass 2.path new file mode 100644 index 00000000..50cef083 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/greedy midfield-center pass 2.path @@ -0,0 +1,89 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.8228555555555555, + "y": 6.198266666666666 + }, + "prevControl": null, + "nextControl": { + "x": 2.2711005886613735, + "y": 6.917000110379524 + }, + "isLocked": false, + "linkedName": "left midfield" + }, + { + "anchor": { + "x": 4.595420827389443, + "y": 7.521968616262482 + }, + "prevControl": { + "x": 3.1361699403833514, + "y": 7.554738139434656 + }, + "nextControl": { + "x": 7.743205921533032, + "y": 7.451280691723889 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.403109843081312, + "y": 2.9287446504992873 + }, + "prevControl": { + "x": 7.374070107981974, + "y": 4.743728094207896 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "greedy center pass 2" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0366886171213492, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 1.0652801848642368, + "constraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 7.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -54.32359177813802 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/greedy netural zone-left midfield.path b/src/main/deploy/pathplanner/paths/greedy netural zone-left midfield.path new file mode 100644 index 00000000..8567d393 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/greedy netural zone-left midfield.path @@ -0,0 +1,86 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.20530670470756, + "y": 4.2484878744650505 + }, + "prevControl": null, + "nextControl": { + "x": 8.03165852704063, + "y": 5.233295627477259 + }, + "isLocked": false, + "linkedName": "greedy left mid neutral" + }, + { + "anchor": { + "x": 5.082889072852621, + "y": 7.367309602647918 + }, + "prevControl": { + "x": 7.0976264784777765, + "y": 7.353414861919472 + }, + "nextControl": { + "x": 4.030198675501628, + "y": 7.374569536422751 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.8228555555555555, + "y": 6.198266666666666 + }, + "prevControl": { + "x": 1.9952231052244298, + "y": 7.294516666666666 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "left midfield" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.3876040703052817, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [ + { + "fieldPosition": { + "x": 4.75, + "y": 4.0 + }, + "rotationOffset": 0.0, + "minWaypointRelativePos": 1.7974025974026056, + "maxWaypointRelativePos": 2.0, + "name": "Point Towards Zone" + } + ], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -54.32359177813802 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -70.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/left mid neutral - left trench.path b/src/main/deploy/pathplanner/paths/left mid neutral - left trench.path index 46b761b0..e70e54a5 100644 --- a/src/main/deploy/pathplanner/paths/left mid neutral - left trench.path +++ b/src/main/deploy/pathplanner/paths/left mid neutral - left trench.path @@ -64,7 +64,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -57.00000000000001 + "rotation": -70.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/left trench - left mid neutral.path b/src/main/deploy/pathplanner/paths/left trench - left mid neutral.path index c9b99ade..dbbe0544 100644 --- a/src/main/deploy/pathplanner/paths/left trench - left mid neutral.path +++ b/src/main/deploy/pathplanner/paths/left trench - left mid neutral.path @@ -16,16 +16,16 @@ }, { "anchor": { - "x": 7.551266556291392, - "y": 7.236630794701986 + "x": 6.028777777777779, + "y": 7.426066666666667 }, "prevControl": { - "x": 7.404075333981382, - "y": 7.438706879568271 + "x": 5.817328286803404, + "y": 7.559442499435121 }, "nextControl": { - "x": 7.9796026490066225, - "y": 6.648576158940396 + "x": 6.662166666666667, + "y": 7.026544444444443 }, "isLocked": false, "linkedName": null @@ -44,18 +44,13 @@ "linkedName": "left mid neutral" } ], - "rotationTargets": [ - { - "waypointRelativePos": 1.1341350601295181, - "rotationDegrees": -57.00000000000001 - } - ], + "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 6.0, + "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -63,7 +58,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -57.00000000000001 + "rotation": -70.0 }, "reversed": false, "folder": null, @@ -71,5 +66,5 @@ "velocity": 0, "rotation": -90.0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/midfield-center pass 2.path b/src/main/deploy/pathplanner/paths/midfield-center pass 2.path new file mode 100644 index 00000000..7b25eeac --- /dev/null +++ b/src/main/deploy/pathplanner/paths/midfield-center pass 2.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.8228555555555555, + "y": 6.198266666666666 + }, + "prevControl": null, + "nextControl": { + "x": 2.2711005886613735, + "y": 6.917000110379524 + }, + "isLocked": false, + "linkedName": "left midfield" + }, + { + "anchor": { + "x": 4.552913907282945, + "y": 7.476208609274684 + }, + "prevControl": { + "x": 3.093304172211311, + "y": 7.481348080172824 + }, + "nextControl": { + "x": 6.614735099335925, + "y": 7.468948675499848 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.810753311258278, + "y": 4.775513245033112 + }, + "prevControl": { + "x": 6.78171357615894, + "y": 6.590496688741721 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "center pass 2" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.83996299722478, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -54.32359177813802 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/netural zone-left midfield.path b/src/main/deploy/pathplanner/paths/netural zone-left midfield.path index 2b65e1ed..a529cd51 100644 --- a/src/main/deploy/pathplanner/paths/netural zone-left midfield.path +++ b/src/main/deploy/pathplanner/paths/netural zone-left midfield.path @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 3.1009271523228215, - "y": 6.677615894038647 + "x": 2.8228555555555555, + "y": 6.198266666666666 }, "prevControl": { - "x": 2.273294701991696, - "y": 7.7738658940386465 + "x": 1.9952231052244298, + "y": 7.294516666666666 }, "nextControl": null, "isLocked": false, @@ -74,13 +74,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -62.0 + "rotation": -54.32359177813802 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -57.00000000000001 + "rotation": -70.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/neutral zone-left trench-hub.path b/src/main/deploy/pathplanner/paths/neutral zone-left trench-hub.path index 683b01ed..e71ecef4 100644 --- a/src/main/deploy/pathplanner/paths/neutral zone-left trench-hub.path +++ b/src/main/deploy/pathplanner/paths/neutral zone-left trench-hub.path @@ -101,7 +101,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -57.00000000000001 + "rotation": -70.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/java/com/team2813/RobotContainer.java b/src/main/java/com/team2813/RobotContainer.java index 86af2b2e..6cf09c35 100644 --- a/src/main/java/com/team2813/RobotContainer.java +++ b/src/main/java/com/team2813/RobotContainer.java @@ -12,6 +12,7 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.team2813.commands.DriveCommands; +import com.team2813.commands.LockDrivetrainCommand; import com.team2813.commands.VariableShooterCommand; import com.team2813.subsystems.drive.AllTunerConstants; import com.team2813.subsystems.drive.Drive; @@ -249,7 +250,17 @@ private void configureButtonBindings() { // intakeRoller.intakeCommand())); // Defensive Stop. - operatorController.rightBumper().onTrue(new InstantCommand(drive::stopWithX)); + operatorController + .rightBumper() + .whileTrue( + new LockDrivetrainCommand( + drive, + shooter, + () -> -driveController.getLeftY(), + () -> -driveController.getLeftX(), + () -> -driveController.getRightX(), + driveController.a(), + currentAlliance)); // Feeder controls operatorController.leftBumper().whileTrue(hopper.outtakeCommand()); @@ -298,16 +309,17 @@ private void configureButtonBindings() { driveController .a() + .and(operatorController.rightBumper().negate()) // lock drivetrain handles this .whileTrue( - Commands.parallel( - DriveCommands.joystickDriveAtAngle( - drive, - () -> -driveController.getLeftY(), - () -> -driveController.getLeftX(), - () -> HubPositionUtil.getBotToHubAngle(drive.getPose(), currentAlliance)), - VariableShooterCommand.shootBasedOnDistanceCommand( - shooter, - () -> HubPositionUtil.getBotToHubDistance(drive.getPose(), currentAlliance)))); + DriveCommands.joystickDriveAtAngle( + drive, + () -> -driveController.getLeftY(), + () -> -driveController.getLeftX(), + () -> HubPositionUtil.getBotToHubAngle(drive.getPose(), currentAlliance))) + .whileTrue( + VariableShooterCommand.shootBasedOnDistanceCommand( + shooter, + () -> HubPositionUtil.getBotToHubDistance(drive.getPose(), currentAlliance))); } // controller rumble @@ -384,6 +396,18 @@ private void namedCommandsRegistration() { new ParallelCommandGroup(kicker.shootCommand(), hopper.intakeCommand()))), new WaitCommand(6))); + NamedCommands.registerCommand( + "VariableShot", + new ParallelRaceGroup( + new ParallelCommandGroup( + VariableShooterCommand.shootBasedOnDistanceCommand( + shooter, + () -> HubPositionUtil.getBotToHubDistance(drive.getPose(), currentAlliance)), + new SequentialCommandGroup( + new WaitCommand(0.5), + new ParallelCommandGroup(kicker.shootCommand(), hopper.intakeCommand()))), + new WaitCommand(6))); + NamedCommands.registerCommand( "ExtendIntake", intakeExtension diff --git a/src/main/java/com/team2813/commands/LockDrivetrainCommand.java b/src/main/java/com/team2813/commands/LockDrivetrainCommand.java new file mode 100644 index 00000000..898fb3f2 --- /dev/null +++ b/src/main/java/com/team2813/commands/LockDrivetrainCommand.java @@ -0,0 +1,102 @@ +package com.team2813.commands; + +import com.team2813.subsystems.drive.Drive; +import com.team2813.subsystems.shooter.Shooter; +import com.team2813.util.HubPositionUtil; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import java.util.Optional; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + +public class LockDrivetrainCommand extends Command { + private final DoubleSupplier vxSupplier; + private final DoubleSupplier vySupplier; + private final DoubleSupplier omegaSupplier; + private final BooleanSupplier faceHub; + private final Drive drive; + private boolean crossed; + private boolean wasFacingHub = false; + private final Command facingCommand; + private final Command normalCommand; + private final Command prepareShooter; + + public LockDrivetrainCommand( + Drive drive, + Shooter shooter, + DoubleSupplier vxSupplier, + DoubleSupplier vySupplier, + DoubleSupplier omegaSupplier, + BooleanSupplier faceHub, + Optional alliance) { + this.drive = drive; + this.vxSupplier = vxSupplier; + this.vySupplier = vySupplier; + this.omegaSupplier = omegaSupplier; + + this.faceHub = faceHub; + + facingCommand = + DriveCommands.joystickDriveAtAngle( + drive, + vxSupplier, + vySupplier, + () -> HubPositionUtil.getBotToHubAngle(drive.getPose(), alliance)); + + normalCommand = DriveCommands.joystickDrive(drive, vxSupplier, vySupplier, omegaSupplier); + + prepareShooter = + VariableShooterCommand.shootBasedOnDistanceCommand( + shooter, () -> HubPositionUtil.getBotToHubDistance(drive.getPose(), alliance)); + + addRequirements(drive); + // Don't require the shooter, so you can still spool up normally + } + + @Override + public void initialize() { + crossed = false; + wasFacingHub = false; + } + + @Override + public void execute() { + double vx = vxSupplier.getAsDouble(); + double vy = vySupplier.getAsDouble(); + double omega = omegaSupplier.getAsDouble(); + + if (MathUtil.isNear(0, vx, 0.1) + || MathUtil.isNear(0, vy, 0.1) + || MathUtil.isNear(0, omega, 0.1)) { + // we have a requested movement + crossed = false; + if (faceHub.getAsBoolean()) { + facingCommand.execute(); + if (wasFacingHub) { + wasFacingHub = false; + prepareShooter.end(true); + } + } else { + wasFacingHub = true; + normalCommand.execute(); + prepareShooter.execute(); + } + } else if (!crossed) { + drive.stopWithX(); + if (wasFacingHub) { + prepareShooter.end(true); + wasFacingHub = false; + } + crossed = true; + } + } + + @Override + public void end(boolean interrupted) { + if (wasFacingHub) { + prepareShooter.end(interrupted); + } + drive.stop(); + } +} diff --git a/src/main/java/com/team2813/commands/VariableShooterCommand.java b/src/main/java/com/team2813/commands/VariableShooterCommand.java index 9128a3ff..0f35de2b 100644 --- a/src/main/java/com/team2813/commands/VariableShooterCommand.java +++ b/src/main/java/com/team2813/commands/VariableShooterCommand.java @@ -19,7 +19,7 @@ public class VariableShooterCommand { RotationsPerSecond.of(100); // 3.5 meters from hub speed. public static final Distance MIN_DIST = Meters.of(1.5); - public static final Distance MAX_DIST = Meters.of(3.5); + public static final Distance MAX_DIST = Meters.of(3.33); /** * Calculates the speed to shoot at if between the 1.5/3.1 MIN/MAX distance. @@ -41,6 +41,7 @@ private static void shootBasedOnDistance(Shooter shooter, Supplier dis public static Command shootBasedOnDistanceCommand( Shooter shooter, Supplier distanceSupplier) { return new RunCommand(() -> shootBasedOnDistance(shooter, distanceSupplier), shooter) - .finallyDo(shooter::stop); + .finallyDo(shooter::stop) + .withInterruptBehavior(Command.InterruptionBehavior.kCancelIncoming); } } diff --git a/src/main/java/com/team2813/generated/cartacus/TunerConstants.java b/src/main/java/com/team2813/generated/cartacus/TunerConstants.java index 21a879f0..cf47be8d 100644 --- a/src/main/java/com/team2813/generated/cartacus/TunerConstants.java +++ b/src/main/java/com/team2813/generated/cartacus/TunerConstants.java @@ -60,7 +60,8 @@ public class TunerConstants { // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration() - .withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(Amps.of(50))); + .withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(Amps.of(50))) + .withMotorOutput(new MotorOutputConfigs().withNeutralMode(NeutralModeValue.Brake)); private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() .withCurrentLimits( diff --git a/src/main/java/com/team2813/subsystems/intakeroller/IntakeRollerConstants.java b/src/main/java/com/team2813/subsystems/intakeroller/IntakeRollerConstants.java index 7e55afcb..63c8372c 100644 --- a/src/main/java/com/team2813/subsystems/intakeroller/IntakeRollerConstants.java +++ b/src/main/java/com/team2813/subsystems/intakeroller/IntakeRollerConstants.java @@ -29,7 +29,7 @@ public class IntakeRollerConstants { public static final double INTAKE_SIM_MOI = 0.00011331; // In kg*m^2 static { - Preferences.initFloat(INTAKE_PREFERENCE_NT, 8); + Preferences.initFloat(INTAKE_PREFERENCE_NT, 9); Preferences.initFloat(OUTTAKE_PREFERENCE_NT, -6); } diff --git a/src/main/java/com/team2813/subsystems/kicker/Kicker.java b/src/main/java/com/team2813/subsystems/kicker/Kicker.java index 042c7325..a7a2fcee 100644 --- a/src/main/java/com/team2813/subsystems/kicker/Kicker.java +++ b/src/main/java/com/team2813/subsystems/kicker/Kicker.java @@ -3,8 +3,6 @@ import static edu.wpi.first.units.Units.Volts; import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.Preferences; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.StartEndCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -20,49 +18,25 @@ public class Kicker extends SubsystemBase implements AutoCloseable { private final KickerIO io; private final KickerIOInputsAutoLogged replayedInputs; - /** - * @see #shootCommand() - */ - private double shootVoltage = KickerConstants.SHOOT_VOLTAGE; - - /** - * @see #resistFuelCommand() - */ - private double resistFuelVoltage = KickerConstants.RESIST_FUEL_VOLTAGE; - - // note: could put alerts in a kicker-specific location, but it will be easier for seeing alerts - // to put them all in the same place - // also, the default behavior is to not show the alert - private final Alert shootVoltageWarning = - new Alert(createAlertMessage("shoot voltage"), Alert.AlertType.kInfo); - private final Alert resistFuelVoltageWarning = - new Alert(createAlertMessage("resist fuel voltage"), Alert.AlertType.kInfo); - public Kicker(KickerIO io) { - this.io = Objects.requireNonNull(io, "io"); + this.io = Objects.requireNonNull(io, "[Kicker] \"io\" cannot be null!"); this.replayedInputs = new KickerIOInputsAutoLogged(); - - Preferences.initDouble(KickerConstants.SHOOT_PREFERENCE_NT, shootVoltage); - Preferences.initDouble(KickerConstants.RESIST_FUEL_PREFERENCE_NT, resistFuelVoltage); - shootVoltageWarning.set(false); - resistFuelVoltageWarning.set(false); } @Override public void periodic() { // could put somewhere else, but all the other code updates preferences once per cycle anyway - updatePreferences(); io.updateState(replayedInputs); Logger.processInputs("Kicker", replayedInputs); } private void shoot() { - io.setMotorVoltage(Volts.of(shootVoltage)); + io.setMotorVoltage(KickerConstants.getShootVoltage()); } private void resistFuel() { - io.setMotorVoltage(Volts.of(resistFuelVoltage)); + io.setMotorVoltage(KickerConstants.getResistFuelVoltage()); } public void stop() { @@ -102,25 +76,6 @@ public Command customVoltageCommand(Voltage voltageToRun) { return new StartEndCommand(() -> io.setMotorVoltage(voltageToRun), this::stop, this); } - /** - * Refresh all values from preferences. This will also put alerts onto NetworkTables if the value - * from preferences does not match the value in code to encourage keeping the code up-to-date. - */ - // note: if we update preferences somewhere else, we may need to change the visibility of this. - private void updatePreferences() { - shootVoltage = Preferences.getDouble(KickerConstants.SHOOT_PREFERENCE_NT, shootVoltage); - shootVoltageWarning.set(shootVoltage != KickerConstants.SHOOT_VOLTAGE); - resistFuelVoltage = - Preferences.getDouble(KickerConstants.RESIST_FUEL_PREFERENCE_NT, resistFuelVoltage); - resistFuelVoltageWarning.set(resistFuelVoltage != KickerConstants.RESIST_FUEL_VOLTAGE); - } - - private static String createAlertMessage(String preference) { - return String.format( - "[KICKER] The %s was changed in Preferences! Once you are done tuning, please update the code!", - preference); - } - @Override public void close() { io.close(); diff --git a/src/main/java/com/team2813/subsystems/kicker/KickerConstants.java b/src/main/java/com/team2813/subsystems/kicker/KickerConstants.java index 083bbb49..4edc11f4 100644 --- a/src/main/java/com/team2813/subsystems/kicker/KickerConstants.java +++ b/src/main/java/com/team2813/subsystems/kicker/KickerConstants.java @@ -1,13 +1,14 @@ package com.team2813.subsystems.kicker; -import static edu.wpi.first.units.Units.Amps; -import static edu.wpi.first.units.Units.KilogramSquareMeters; +import static edu.wpi.first.units.Units.*; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.InvertedValue; import edu.wpi.first.units.measure.MomentOfInertia; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.Preferences; class KickerConstants { static final MomentOfInertia KICKER_SIM_MOI = KilogramSquareMeters.of(0.0000535531); @@ -21,6 +22,19 @@ class KickerConstants { static final double KICKER_MOTOR_TO_FLYWHEEL_GEARING = 2.0 / 5.0; - static final double SHOOT_VOLTAGE = 7; - static final double RESIST_FUEL_VOLTAGE = -3; + static final double SHOOT_VOLTAGE_DEFAULT = 9; + static final double RESIST_FUEL_VOLTAGE_DEFAULT = -3; + + static { + Preferences.initDouble(SHOOT_PREFERENCE_NT, SHOOT_VOLTAGE_DEFAULT); + Preferences.initDouble(RESIST_FUEL_PREFERENCE_NT, RESIST_FUEL_VOLTAGE_DEFAULT); + } + + public static Voltage getShootVoltage() { + return Volts.of(Preferences.getDouble(SHOOT_PREFERENCE_NT, SHOOT_VOLTAGE_DEFAULT)); + } + + public static Voltage getResistFuelVoltage() { + return Volts.of(Preferences.getDouble(RESIST_FUEL_PREFERENCE_NT, RESIST_FUEL_VOLTAGE_DEFAULT)); + } }