diff --git a/src/main/deploy/pathplanner/autos/A-CC.auto b/src/main/deploy/pathplanner/autos/A-CC.auto deleted file mode 100644 index 436acac1a..000000000 --- a/src/main/deploy/pathplanner/autos/A-CC.auto +++ /dev/null @@ -1,56 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "A-Cycle-Center" - } - }, - { - "type": "named", - "data": { - "name": "startIntake" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "endIntake" - } - }, - { - "type": "named", - "data": { - "name": "driveToHubAuto" - } - }, - { - "type": "named", - "data": { - "name": "shootAuto" - } - }, - { - "type": "named", - "data": { - "name": "bringInIntake" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} diff --git a/src/main/deploy/pathplanner/autos/A-LSC.auto b/src/main/deploy/pathplanner/autos/A-LSC.auto deleted file mode 100644 index 48a36d011..000000000 --- a/src/main/deploy/pathplanner/autos/A-LSC.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "A-Cycle-LeftSweep" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/A-Side 2 Cycle.auto similarity index 81% rename from src/main/deploy/pathplanner/autos/New Auto.auto rename to src/main/deploy/pathplanner/autos/A-Side 2 Cycle.auto index f1f316df0..ce72f9f46 100644 --- a/src/main/deploy/pathplanner/autos/New Auto.auto +++ b/src/main/deploy/pathplanner/autos/A-Side 2 Cycle.auto @@ -7,13 +7,13 @@ { "type": "path", "data": { - "pathName": "Test Circle" + "pathName": "A-Cycle1" } }, { "type": "path", "data": { - "pathName": "Test Straight" + "pathName": "A-Cycle2" } } ] diff --git a/src/main/deploy/pathplanner/autos/A-LC-CC.auto b/src/main/deploy/pathplanner/autos/B-Side 2 Cycle.auto similarity index 80% rename from src/main/deploy/pathplanner/autos/A-LC-CC.auto rename to src/main/deploy/pathplanner/autos/B-Side 2 Cycle.auto index b7fef3438..6961e215d 100644 --- a/src/main/deploy/pathplanner/autos/A-LC-CC.auto +++ b/src/main/deploy/pathplanner/autos/B-Side 2 Cycle.auto @@ -7,13 +7,13 @@ { "type": "path", "data": { - "pathName": "A-Cycle-Left" + "pathName": "B-Cycle1" } }, { "type": "path", "data": { - "pathName": "A-Cycle-Right#" + "pathName": "B-Cycle2" } } ] diff --git a/src/main/deploy/pathplanner/autos/Bummmmpar.auto b/src/main/deploy/pathplanner/autos/Bummmmpar.auto deleted file mode 100644 index ccf8616c2..000000000 --- a/src/main/deploy/pathplanner/autos/Bummmmpar.auto +++ /dev/null @@ -1,31 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drive Back Left" - } - }, - { - "type": "named", - "data": { - "name": "Drive Over Left" - } - }, - { - "type": "named", - "data": { - "name": "Drive Back Left" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A-Cycle-Center#.path b/src/main/deploy/pathplanner/paths/A-Cycle-Center#.path deleted file mode 100644 index 60cf9b1fa..000000000 --- a/src/main/deploy/pathplanner/paths/A-Cycle-Center#.path +++ /dev/null @@ -1,116 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.493756708407871, - "y": 5.519624329159212 - }, - "prevControl": null, - "nextControl": { - "x": 3.3709768232979314, - "y": 5.466689313590882 - }, - "isLocked": false, - "linkedName": "A-end" - }, - { - "anchor": { - "x": 7.604758497316638, - "y": 5.519624329159212 - }, - "prevControl": { - "x": 7.855269447439065, - "y": 5.5207065008482745 - }, - "nextControl": { - "x": 6.419272864594822, - "y": 5.514503199767883 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.493756708407871, - "y": 5.519624329159212 - }, - "prevControl": { - "x": 3.0725592844223644, - "y": 5.518868335131507 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "A-end" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.09090909090909055, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 1.8288770053475967, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 1.8877005347593623, - "rotationDegrees": 0.0 - } - ], - "constraintZones": [ - { - "name": "Intake", - "minWaypointRelativePos": 0.6508474576271186, - "maxWaypointRelativePos": 0.9627118644067798, - "constraints": { - "maxVelocity": 1.2, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "startIntake", - "waypointRelativePos": 0.46779661016949126, - "endWaypointRelativePos": null, - "command": null - }, - { - "name": "endIntake", - "waypointRelativePos": 1.3966101694915225, - "endWaypointRelativePos": null, - "command": null - }, - { - "name": "shootDistance", - "waypointRelativePos": 2.0, - "endWaypointRelativePos": null, - "command": null - } - ], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -49.531177178735966 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": -49.531177178735966 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A-Cycle-Center.path b/src/main/deploy/pathplanner/paths/A-Cycle-Center.path deleted file mode 100644 index 04862b0b1..000000000 --- a/src/main/deploy/pathplanner/paths/A-Cycle-Center.path +++ /dev/null @@ -1,115 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6446013289036543, - "y": 5.519624329159212 - }, - "prevControl": null, - "nextControl": { - "x": 4.521821443793716, - "y": 5.466689313590882 - }, - "isLocked": false, - "linkedName": "A-initial" - }, - { - "anchor": { - "x": 7.767012522361359, - "y": 5.519624329159212 - }, - "prevControl": { - "x": 8.017523472483786, - "y": 5.5207065008482745 - }, - "nextControl": { - "x": 6.581526889639543, - "y": 5.514503199767883 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.493756708407871, - "y": 5.519624329159212 - }, - "prevControl": { - "x": 3.0725592844223644, - "y": 5.518868335131507 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "A-end" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.8288770053475967, - "rotationDegrees": 0.0 - } - ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.6, - "maxWaypointRelativePos": 0.66, - "constraints": { - "maxVelocity": 1.35, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - }, - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.66, - "maxWaypointRelativePos": 1.08, - "constraints": { - "maxVelocity": 0.9, - "maxAcceleration": 2.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - }, - { - "name": "Constraints Zone", - "minWaypointRelativePos": 1.08, - "maxWaypointRelativePos": 1.75, - "constraints": { - "maxVelocity": 2.5, - "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -49.531177178735966 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} diff --git a/src/main/deploy/pathplanner/paths/A-Cycle-Left#.path b/src/main/deploy/pathplanner/paths/A-Cycle-Left#.path deleted file mode 100644 index 187d23772..000000000 --- a/src/main/deploy/pathplanner/paths/A-Cycle-Left#.path +++ /dev/null @@ -1,156 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6446013289036543, - "y": 5.519624329159212 - }, - "prevControl": null, - "nextControl": { - "x": 4.8770912764137435, - "y": 5.519624329159212 - }, - "isLocked": false, - "linkedName": "A-initial" - }, - { - "anchor": { - "x": 5.738837209302326, - "y": 5.519624329159212 - }, - "prevControl": { - "x": 4.73288905179687, - "y": 5.51228164187815 - }, - "nextControl": { - "x": 7.961717352415029, - "y": 5.535849731663684 - }, - "isLocked": false, - "linkedName": "overBump" - }, - { - "anchor": { - "x": 7.750787119856888, - "y": 6.704078711985688 - }, - "prevControl": { - "x": 7.767012522361359, - "y": 5.7143291592128795 - }, - "nextControl": { - "x": 7.735900794997783, - "y": 7.612144528391123 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.738837209302326, - "y": 5.519624329159212 - }, - "prevControl": { - "x": 6.210705938954184, - "y": 5.533502821207796 - }, - "nextControl": { - "x": 4.635509838998211, - "y": 5.4871735241502675 - }, - "isLocked": false, - "linkedName": "overBump" - }, - { - "anchor": { - "x": 2.493756708407871, - "y": 5.519624329159212 - }, - "prevControl": { - "x": 2.243756708407871, - "y": 5.519624329159212 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "A-end" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.06417112299465277, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 1.0106951871657757, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 2.213903743315511, - "rotationDegrees": 90.0 - }, - { - "waypointRelativePos": 2.9946524064171127, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 3.689839572192517, - "rotationDegrees": 0.0 - } - ], - "constraintZones": [ - { - "name": "Intake", - "minWaypointRelativePos": 1.1796610169491524, - "maxWaypointRelativePos": 2.115254237288133, - "constraints": { - "maxVelocity": 1.2, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "startIntake", - "waypointRelativePos": 1.1254237288135593, - "endWaypointRelativePos": null, - "command": null - }, - { - "name": "endIntake", - "waypointRelativePos": 2.250847457627117, - "endWaypointRelativePos": null, - "command": null - }, - { - "name": "shootDistance", - "waypointRelativePos": 4.0, - "endWaypointRelativePos": null, - "command": null - } - ], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -49.531177178735966 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A-Cycle-Left.path b/src/main/deploy/pathplanner/paths/A-Cycle-Left.path deleted file mode 100644 index 420c9724a..000000000 --- a/src/main/deploy/pathplanner/paths/A-Cycle-Left.path +++ /dev/null @@ -1,158 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6446013289036543, - "y": 5.519624329159212 - }, - "prevControl": null, - "nextControl": { - "x": 4.8770912764137435, - "y": 5.519624329159212 - }, - "isLocked": false, - "linkedName": "A-initial" - }, - { - "anchor": { - "x": 5.738837209302326, - "y": 5.519624329159212 - }, - "prevControl": { - "x": 4.73288905179687, - "y": 5.51228164187815 - }, - "nextControl": { - "x": 7.961717352415029, - "y": 5.535849731663684 - }, - "isLocked": false, - "linkedName": "overBump" - }, - { - "anchor": { - "x": 7.750787119856888, - "y": 6.704078711985688 - }, - "prevControl": { - "x": 7.767012522361359, - "y": 5.7143291592128795 - }, - "nextControl": { - "x": 7.735900794997783, - "y": 7.612144528391123 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.738837209302326, - "y": 5.519624329159212 - }, - "prevControl": { - "x": 6.210705938954184, - "y": 5.533502821207796 - }, - "nextControl": { - "x": 4.635509838998211, - "y": 5.4871735241502675 - }, - "isLocked": false, - "linkedName": "overBump" - }, - { - "anchor": { - "x": 2.493756708407871, - "y": 5.519624329159212 - }, - "prevControl": { - "x": 2.243756708407871, - "y": 5.519624329159212 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "A-end" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.0106951871657757, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 2.213903743315511, - "rotationDegrees": 90.0 - }, - { - "waypointRelativePos": 2.9946524064171127, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 3.689839572192517, - "rotationDegrees": 0.0 - } - ], - "constraintZones": [ - { - "name": "Drive", - "minWaypointRelativePos": 1.1254237288135591, - "maxWaypointRelativePos": 2.2644067796610168, - "constraints": { - "maxVelocity": 1.2, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "startIntake", - "waypointRelativePos": 1.1254237288135593, - "endWaypointRelativePos": null, - "command": null - }, - { - "name": "endIntake", - "waypointRelativePos": 2.250847457627117, - "endWaypointRelativePos": null, - "command": null - }, - { - "name": "preloadBalls", - "waypointRelativePos": 2.7254237288135585, - "endWaypointRelativePos": null, - "command": null - }, - { - "name": "shootDistance", - "waypointRelativePos": 4.0, - "endWaypointRelativePos": null, - "command": null - } - ], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -49.531177178735966 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A-Cycle-LeftSweep.path b/src/main/deploy/pathplanner/paths/A-Cycle-LeftSweep.path deleted file mode 100644 index b4b732b20..000000000 --- a/src/main/deploy/pathplanner/paths/A-Cycle-LeftSweep.path +++ /dev/null @@ -1,158 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6446013289036543, - "y": 5.519624329159212 - }, - "prevControl": null, - "nextControl": { - "x": 4.644601328903654, - "y": 5.519624329159212 - }, - "isLocked": false, - "linkedName": "A-initial" - }, - { - "anchor": { - "x": 5.738837209302326, - "y": 5.519624329159212 - }, - "prevControl": { - "x": 4.738967256770203, - "y": 5.503497394440951 - }, - "nextControl": { - "x": 6.744812164579606, - "y": 5.535849731663685 - }, - "isLocked": false, - "linkedName": "overBump" - }, - { - "anchor": { - "x": 7.507406082289803, - "y": 7.093488372093023 - }, - "prevControl": { - "x": 7.00742058048248, - "y": 7.076822188699445 - }, - "nextControl": { - "x": 7.994168157423971, - "y": 7.109713774597496 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.718336314847942, - "y": 4.57855098389982 - }, - "prevControl": { - "x": 7.443998211091234, - "y": 4.570438282647584 - }, - "nextControl": { - "x": 7.9926744186046506, - "y": 4.586663685152057 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.738837209302326, - "y": 5.519624329159212 - }, - "prevControl": { - "x": 5.988346608309651, - "y": 5.50396994296864 - }, - "nextControl": { - "x": 5.489327810295001, - "y": 5.535278715349785 - }, - "isLocked": false, - "linkedName": "overBump" - }, - { - "anchor": { - "x": 2.493756708407871, - "y": 5.519624329159212 - }, - "prevControl": { - "x": 2.244321250822382, - "y": 5.502832887913206 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "A-end" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.0000000000000002, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 2.0454545454545476, - "rotationDegrees": -90.0 - }, - { - "waypointRelativePos": 4.6791443850267385, - "rotationDegrees": -90.0 - } - ], - "constraintZones": [ - { - "name": "intake", - "minWaypointRelativePos": 1.6271186440677965, - "maxWaypointRelativePos": 2.796610169491525, - "constraints": { - "maxVelocity": 1.5, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - }, - { - "name": "bump", - "minWaypointRelativePos": 3.796610169491523, - "maxWaypointRelativePos": 4.389830508474569, - "constraints": { - "maxVelocity": 2.4, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -49.531177178735966 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A-Cycle-Right#.path b/src/main/deploy/pathplanner/paths/A-Cycle-Right#.path deleted file mode 100644 index 14c74d019..000000000 --- a/src/main/deploy/pathplanner/paths/A-Cycle-Right#.path +++ /dev/null @@ -1,156 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.493756708407871, - "y": 5.519624329159212 - }, - "prevControl": null, - "nextControl": { - "x": 4.6030590339892665, - "y": 5.535849731663685 - }, - "isLocked": false, - "linkedName": "A-end" - }, - { - "anchor": { - "x": 5.738837209302326, - "y": 5.519624329159212 - }, - "prevControl": { - "x": 4.820002561980206, - "y": 5.514850828351657 - }, - "nextControl": { - "x": 7.704960966476037, - "y": 5.529838673350493 - }, - "isLocked": false, - "linkedName": "overBump" - }, - { - "anchor": { - "x": 7.783237924865832, - "y": 4.57855098389982 - }, - "prevControl": { - "x": 7.864364937388192, - "y": 4.335169946332736 - }, - "nextControl": { - "x": 7.505477705640591, - "y": 5.411831641575549 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.738837209302326, - "y": 5.519624329159212 - }, - "prevControl": { - "x": 6.435774438518339, - "y": 5.519624329159212 - }, - "nextControl": { - "x": 3.2076744186046513, - "y": 5.519624329159212 - }, - "isLocked": false, - "linkedName": "overBump" - }, - { - "anchor": { - "x": 2.493756708407871, - "y": 5.519624329159212 - }, - "prevControl": { - "x": 4.992468694096601, - "y": 5.519624329159212 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "A-end" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.042780748663101734, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 1.0026737967914436, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 1.9518716577540105, - "rotationDegrees": -90.0 - }, - { - "waypointRelativePos": 2.9812834224599016, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 3.9572192513369, - "rotationDegrees": 0.0 - } - ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 1.193220338983051, - "maxWaypointRelativePos": 1.8169491525423729, - "constraints": { - "maxVelocity": 1.5, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "startIntake", - "waypointRelativePos": 1.1254237288135593, - "endWaypointRelativePos": null, - "command": null - }, - { - "name": "endIntake", - "waypointRelativePos": 2.250847457627117, - "endWaypointRelativePos": null, - "command": null - }, - { - "name": "shootDistance", - "waypointRelativePos": 3.5, - "endWaypointRelativePos": null, - "command": null - } - ], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -49.531177178735966 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": -49.531177178735966 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A-Cycle-Right.path b/src/main/deploy/pathplanner/paths/A-Cycle-Right.path deleted file mode 100644 index 373bdb620..000000000 --- a/src/main/deploy/pathplanner/paths/A-Cycle-Right.path +++ /dev/null @@ -1,138 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6446013289036543, - "y": 5.519624329159212 - }, - "prevControl": null, - "nextControl": { - "x": 5.75390365448505, - "y": 5.535849731663685 - }, - "isLocked": false, - "linkedName": "A-initial" - }, - { - "anchor": { - "x": 5.738837209302326, - "y": 5.519624329159212 - }, - "prevControl": { - "x": 4.820002561980206, - "y": 5.514850828351657 - }, - "nextControl": { - "x": 7.704960966476037, - "y": 5.529838673350493 - }, - "isLocked": false, - "linkedName": "overBump" - }, - { - "anchor": { - "x": 7.783237924865832, - "y": 4.57855098389982 - }, - "prevControl": { - "x": 7.864364937388192, - "y": 4.335169946332736 - }, - "nextControl": { - "x": 7.505477705640591, - "y": 5.411831641575549 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.738837209302326, - "y": 5.519624329159212 - }, - "prevControl": { - "x": 6.435774438518339, - "y": 5.519624329159212 - }, - "nextControl": { - "x": 3.2076744186046513, - "y": 5.519624329159212 - }, - "isLocked": false, - "linkedName": "overBump" - }, - { - "anchor": { - "x": 2.493756708407871, - "y": 5.519624329159212 - }, - "prevControl": { - "x": 4.992468694096601, - "y": 5.519624329159212 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "A-end" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 1.0026737967914436, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 1.9518716577540105, - "rotationDegrees": -90.0 - }, - { - "waypointRelativePos": 2.9812834224599016, - "rotationDegrees": 0.0 - }, - { - "waypointRelativePos": 3.946524064171123, - "rotationDegrees": 0.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "startIntake", - "waypointRelativePos": 1.1254237288135593, - "endWaypointRelativePos": null, - "command": null - }, - { - "name": "endIntake", - "waypointRelativePos": 2.250847457627117, - "endWaypointRelativePos": null, - "command": null - }, - { - "name": "shootDistance", - "waypointRelativePos": 3.5, - "endWaypointRelativePos": null, - "command": null - } - ], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -49.531177178735966 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A-Cycle1.path b/src/main/deploy/pathplanner/paths/A-Cycle1.path new file mode 100644 index 000000000..6c49fd076 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/A-Cycle1.path @@ -0,0 +1,154 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.424579606440071, + "y": 7.369320214669052 + }, + "prevControl": null, + "nextControl": { + "x": 5.438196256563948, + "y": 7.412915747707522 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.783237924865832, + "y": 7.028586762075134 + }, + "prevControl": { + "x": 7.767012522361359, + "y": 7.531574239713775 + }, + "nextControl": { + "x": 7.817452345049281, + "y": 5.967939736388225 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.783237924865832, + "y": 4.805706618962433 + }, + "prevControl": { + "x": 7.77903133903134, + "y": 5.171980056980058 + }, + "nextControl": { + "x": 7.786250707881794, + "y": 4.543379297786823 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.797094017094016, + "y": 5.611267806267806 + }, + "prevControl": { + "x": 7.802016105528991, + "y": 5.5605141654377555 + }, + "nextControl": { + "x": 5.517991452991453, + "y": 5.675868945868947 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.533418803418803, + "y": 5.611267806267806 + }, + "prevControl": { + "x": 3.334472934472934, + "y": 5.675868945868947 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "A-Cycle1-End" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.29946524064171126, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.171122994652407, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 1.9411764705882377, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 3.0, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 3.45, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 3.7526652452025653, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.0508474576271185, + "maxWaypointRelativePos": 2.1694915254237293, + "constraints": { + "maxVelocity": 1.7, + "maxAcceleration": 6, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 3.14382174206618, + "maxWaypointRelativePos": 3.792032410533432, + "constraints": { + "maxVelocity": 2.3, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 8.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -37.18470645323313 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A-Cycle2.path b/src/main/deploy/pathplanner/paths/A-Cycle2.path new file mode 100644 index 000000000..811891d20 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/A-Cycle2.path @@ -0,0 +1,219 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.533418803418803, + "y": 5.611267806267806 + }, + "prevControl": null, + "nextControl": { + "x": 2.267699809926946, + "y": 6.160705710211645 + }, + "isLocked": false, + "linkedName": "A-Cycle1-End" + }, + { + "anchor": { + "x": 2.5075783475783475, + "y": 7.447760964912282 + }, + "prevControl": { + "x": 2.0694924799575967, + "y": 7.351408549885447 + }, + "nextControl": { + "x": 2.7903904157463497, + "y": 7.509962528077773 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.634273504273503, + "y": 7.447760964912282 + }, + "prevControl": { + "x": 4.144916933849193, + "y": 7.530332248967907 + }, + "nextControl": { + "x": 6.494219837010534, + "y": 7.400084757398864 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.202763532763532, + "y": 4.060840455840457 + }, + "prevControl": { + "x": 6.215683760683759, + "y": 3.569871794871795 + }, + "nextControl": { + "x": 6.1825608953181, + "y": 4.828540678766911 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.866837606837606, + "y": 5.611 + }, + "prevControl": { + "x": 6.590370370370371, + "y": 5.624188034188035 + }, + "nextControl": { + "x": 5.351084085527113, + "y": 5.601599216214575 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.755698005698005, + "y": 5.611 + }, + "prevControl": { + "x": 5.905598290598291, + "y": 5.598079772079772 + }, + "nextControl": { + "x": 4.148936797920834, + "y": 5.6178175416604175 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.760840455840455, + "y": 5.611 + }, + "prevControl": { + "x": 4.6006552706552695, + "y": 5.598347578347578 + }, + "nextControl": { + "x": 3.34446165048371, + "y": 5.61727304986715 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.533418803418803, + "y": 5.611267806267806 + }, + "prevControl": { + "x": 3.476099715099715, + "y": 5.624455840455841 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.1886792452830188, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.7264150943396226, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 2.0754716981132075, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 2.9433962264150932, + "rotationDegrees": -90.0 + }, + { + "waypointRelativePos": 3.9622641509434033, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 4.688679245283029, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 5.734276729559759, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 2.0644067796610166, + "maxWaypointRelativePos": 3.22711864406781, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 6, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.529372045914936, + "maxWaypointRelativePos": 1.380148548278184, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 4.442943956785955, + "maxWaypointRelativePos": 6.4280891289669135, + "constraints": { + "maxVelocity": 2.3, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 8.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -37.195226690262515 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -37.18470645323313 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/A-backup.path b/src/main/deploy/pathplanner/paths/A-backup.path deleted file mode 100644 index 8b2593371..000000000 --- a/src/main/deploy/pathplanner/paths/A-backup.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.6446013289036543, - "y": 5.519624329159212 - }, - "prevControl": null, - "nextControl": { - "x": 3.2053024802284984, - "y": 5.526804970948405 - }, - "isLocked": false, - "linkedName": "A-initial" - }, - { - "anchor": { - "x": 3.1624750830564783, - "y": 5.519624329159212 - }, - "prevControl": { - "x": 4.3413732420324855, - "y": 5.5414336122651005 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "A-start" - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/B-Cycle1.path b/src/main/deploy/pathplanner/paths/B-Cycle1.path new file mode 100644 index 000000000..632dc384e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/B-Cycle1.path @@ -0,0 +1,154 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.424579606440071, + "y": 0.6996797853309484 + }, + "prevControl": null, + "nextControl": { + "x": 5.438196256563948, + "y": 0.6560842522924792 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.783237924865832, + "y": 1.0404132379248665 + }, + "prevControl": { + "x": 7.767012522361359, + "y": 0.5374257602862258 + }, + "nextControl": { + "x": 7.817452345049281, + "y": 2.1010602636117754 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.783237924865832, + "y": 3.2632933810375677 + }, + "prevControl": { + "x": 7.77903133903134, + "y": 2.8970199430199433 + }, + "nextControl": { + "x": 7.786250707881794, + "y": 3.5256207022131782 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.797094017094016, + "y": 2.4577321937321948 + }, + "prevControl": { + "x": 7.802016105528991, + "y": 2.5084858345622454 + }, + "nextControl": { + "x": 5.517991452991453, + "y": 2.3931310541310538 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.533418803418803, + "y": 2.4577321937321948 + }, + "prevControl": { + "x": 3.334472934472934, + "y": 2.3931310541310538 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "A-Cycle1-End" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.29946524064171126, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.171122994652407, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 1.9411764705882377, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 3.45, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 3.7526652452025653, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.0508474576271185, + "maxWaypointRelativePos": 2.1694915254237293, + "constraints": { + "maxVelocity": 1.7, + "maxAcceleration": 6, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 3.14382174206618, + "maxWaypointRelativePos": 3.792032410533432, + "constraints": { + "maxVelocity": 2.3, + "maxAcceleration": 6, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 8, + "maxAcceleration": 6, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -37.18470645323313 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/B-Cycle2.path b/src/main/deploy/pathplanner/paths/B-Cycle2.path new file mode 100644 index 000000000..a28ef683d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/B-Cycle2.path @@ -0,0 +1,219 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.533418803418803, + "y": 2.4577321937321948 + }, + "prevControl": null, + "nextControl": { + "x": 2.267699809926946, + "y": 1.9082942897883557 + }, + "isLocked": false, + "linkedName": "A-Cycle1-End" + }, + { + "anchor": { + "x": 2.5075783475783475, + "y": 0.6212390350877186 + }, + "prevControl": { + "x": 2.0694924799575967, + "y": 0.717591450114554 + }, + "nextControl": { + "x": 2.7903904157463497, + "y": 0.5590374719222275 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.634273504273503, + "y": 0.6212390350877186 + }, + "prevControl": { + "x": 4.144916933849193, + "y": 0.5386677510320939 + }, + "nextControl": { + "x": 6.494219837010534, + "y": 0.6689152426011367 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.202763532763532, + "y": 4.008159544159544 + }, + "prevControl": { + "x": 6.215683760683759, + "y": 4.499128205128206 + }, + "nextControl": { + "x": 6.1825608953181, + "y": 3.2404593212330894 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.866837606837606, + "y": 2.458000000000001 + }, + "prevControl": { + "x": 6.590370370370371, + "y": 2.444811965811966 + }, + "nextControl": { + "x": 5.351084085527113, + "y": 2.4674007837854255 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.755698005698005, + "y": 2.458000000000001 + }, + "prevControl": { + "x": 5.905598290598291, + "y": 2.470920227920229 + }, + "nextControl": { + "x": 4.148936797920834, + "y": 2.4511824583395834 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.760840455840455, + "y": 2.458000000000001 + }, + "prevControl": { + "x": 4.6006552706552695, + "y": 2.4706524216524226 + }, + "nextControl": { + "x": 3.34446165048371, + "y": 2.4517269501328505 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.533418803418803, + "y": 2.4577321937321948 + }, + "prevControl": { + "x": 3.476099715099715, + "y": 2.4445441595441597 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.1886792452830188, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 1.7264150943396226, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 2.0754716981132075, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2.9433962264150932, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.9622641509434033, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 4.688679245283029, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 5.734276729559759, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 2.0644067796610166, + "maxWaypointRelativePos": 3.22711864406781, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 6, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.529372045914936, + "maxWaypointRelativePos": 1.380148548278184, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 6, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 4.442943956785955, + "maxWaypointRelativePos": 6.4280891289669135, + "constraints": { + "maxVelocity": 2.3, + "maxAcceleration": 6, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 8, + "maxAcceleration": 6, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 52.804773309737485 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -37.18470645323313 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Depot.path b/src/main/deploy/pathplanner/paths/Depot.path new file mode 100644 index 000000000..90ed3dfca --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Depot.path @@ -0,0 +1,113 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.63, + "y": 5.844 + }, + "prevControl": null, + "nextControl": { + "x": 2.9647584973166365, + "y": 5.90890161001789 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.282826475849731, + "y": 5.9577101967799635 + }, + "prevControl": { + "x": 3.2798809613513087, + "y": 5.881013697895228 + }, + "nextControl": { + "x": 1.8609660107334522, + "y": 5.990161001788907 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.685826210826211, + "y": 5.9577101967799635 + }, + "prevControl": { + "x": 1.7065242165242167, + "y": 5.947193732193732 + }, + "nextControl": { + "x": 0.3447247623997972, + "y": 5.9612246361956025 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9480679785330945, + "y": 4.611001788908765 + }, + "prevControl": { + "x": 2.365455840455841, + "y": 6.218518518518518 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.0160427807486627, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 1.9358288770053491, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 2.660980810234532, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 1.799999999999997, + "maxWaypointRelativePos": 2.450847457627116, + "constraints": { + "maxVelocity": 3.58, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 8.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -27.897271030947596 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Test Circle.path b/src/main/deploy/pathplanner/paths/Test Circle.path deleted file mode 100644 index 1d934df2b..000000000 --- a/src/main/deploy/pathplanner/paths/Test Circle.path +++ /dev/null @@ -1,86 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.659667774086378, - "y": 5.55671096345515 - }, - "prevControl": null, - "nextControl": { - "x": 3.072076411960133, - "y": 4.697923588039868 - }, - "isLocked": false, - "linkedName": "test" - }, - { - "anchor": { - "x": 1.9119601328903655, - "y": 5.55671096345515 - }, - "prevControl": { - "x": 2.0025193701365023, - "y": 5.058635158601398 - }, - "nextControl": { - "x": 1.7914285714285714, - "y": 6.219634551495017 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 2.7556810631229234, - "y": 7.409883720930233 - }, - "prevControl": { - "x": 2.532074265372944, - "y": 7.2980803220552435 - }, - "nextControl": { - "x": 3.5090033222591357, - "y": 7.786544850498338 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.659667774086378, - "y": 5.55671096345515 - }, - "prevControl": { - "x": 3.4790073097994525, - "y": 5.38390530196331 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Test Straight.path b/src/main/deploy/pathplanner/paths/Test Straight.path deleted file mode 100644 index 2a18876c5..000000000 --- a/src/main/deploy/pathplanner/paths/Test Straight.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.659667774086378, - "y": 5.55671096345515 - }, - "prevControl": null, - "nextControl": { - "x": 4.052190889030067, - "y": 5.547177274221312 - }, - "isLocked": false, - "linkedName": "test" - }, - { - "anchor": { - "x": 1.4900996677740863, - "y": 5.55671096345515 - }, - "prevControl": { - "x": 2.321660995998263, - "y": 5.55732099793068 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/bummmparr.path b/src/main/deploy/pathplanner/paths/bummmparr.path deleted file mode 100644 index 1fdc53902..000000000 --- a/src/main/deploy/pathplanner/paths/bummmparr.path +++ /dev/null @@ -1,99 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 6.714, - "y": 5.44 - }, - "prevControl": null, - "nextControl": { - "x": 6.630224274935788, - "y": 4.48569717198202 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.770951011344179, - "y": 5.034189988227739 - }, - "prevControl": { - "x": 7.762339859172713, - "y": 3.075686618320331 - }, - "nextControl": { - "x": 7.778291751391267, - "y": 6.703753344392124 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 6.714, - "y": 6.520234843214897 - }, - "prevControl": { - "x": 7.686994659191226, - "y": 7.3938356083891374 - }, - "nextControl": { - "x": 5.7410053408087744, - "y": 5.646634078040657 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.384828499571918, - "y": 5.663552279537671 - }, - "prevControl": { - "x": 4.233927654109589, - "y": 5.673683714148116 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.9965843900966185, - "rotationDegrees": 90.0 - }, - { - "waypointRelativePos": 1.95, - "rotationDegrees": -179.9724768467094 - }, - { - "waypointRelativePos": 2.35, - "rotationDegrees": 180.0 - } - ], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -179.60897423494995 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 9134dcf0b..2e5cfd4e0 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,10 +1,10 @@ { - "robotWidth": 0.702, - "robotLength": 0.702, + "robotWidth": 0.9017, + "robotLength": 0.8128, "holonomicMode": true, "pathFolders": [], "autoFolders": [], - "defaultMaxVel": 2.0, + "defaultMaxVel": 8.0, "defaultMaxAccel": 6.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, @@ -19,13 +19,13 @@ "driveCurrentLimit": 46.0, "wheelCOF": 1.2, "flModuleX": 0.273, - "flModuleY": 0.244, + "flModuleY": 0.299, "frModuleX": 0.273, - "frModuleY": -0.244, + "frModuleY": -0.299, "blModuleX": -0.273, - "blModuleY": 0.244, + "blModuleY": 0.299, "brModuleX": -0.273, - "brModuleY": -0.244, + "brModuleY": -0.299, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, "robotFeatures": [] diff --git a/src/main/java/frc/lib/utils/AllianceFlipUtil.java b/src/main/java/frc/lib/utils/AllianceFlipUtil.java index c5b5cf2dc..c5d1063c0 100644 --- a/src/main/java/frc/lib/utils/AllianceFlipUtil.java +++ b/src/main/java/frc/lib/utils/AllianceFlipUtil.java @@ -27,6 +27,13 @@ public static double applyY(double y) { return shouldFlip() ? FieldConstants.fieldWidth - y : y; } + public static Pose2d reflectY(Pose2d pose) { + return new Pose2d( + pose.getX(), + FieldConstants.fieldWidth - pose.getY(), + Rotation2d.kZero.minus(pose.getRotation())); + } + /** Flips a translation to the correct side of the field based on the current alliance color. */ public static Translation2d apply(Translation2d translation) { return new Translation2d(applyX(translation.getX()), applyY(translation.getY())); diff --git a/src/main/java/frc/robot/Orchestrator.java b/src/main/java/frc/robot/Orchestrator.java index 2e5ac8764..37135b86b 100644 --- a/src/main/java/frc/robot/Orchestrator.java +++ b/src/main/java/frc/robot/Orchestrator.java @@ -1,15 +1,23 @@ package frc.robot; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Minute; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Rotations; import static frc.robot.subsystems.shooter.ShooterConstants.CLOSE_HUB_SHOOTER_RPM; import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Distance; 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.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.SelectCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.lib.utils.AllianceFlipUtil; import frc.robot.FieldConstants.Hub; @@ -19,20 +27,40 @@ import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.intake.IntakeConstants; +import frc.robot.subsystems.intake.rollers.IntakeRollers; import frc.robot.subsystems.magicCarpet.MagicCarpet; import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.util.ShooterLeadCompensator; +import frc.robot.util.Zone; +import frc.robot.util.Zone.Tuple2d; +import java.util.Map; import java.util.function.DoubleSupplier; +import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; public class Orchestrator { - private final double FRONT_HUB_OFFSET = Units.inchesToMeters(40.0); + private final Pose2d BBumpClosePose = + new Pose2d(4.415811061859131, 5.599213600158691, new Rotation2d()); + private final Pose2d BBumpFarPose = + new Pose2d(11.334761619567871, 5.599213600158691, new Rotation2d()); + + public enum ZoneId { + NONE, + ZONE_1, + ZONE_2, + ZONE_3, + ZONE_4 + } + + private final double FRONT_HUB_OFFSET = Units.inchesToMeters(58.0); private final Drive drive; private final Shooter shooter; private final MagicCarpet magicCarpet; private final Indexer indexer; private final Intake intake; - private final RobotState robotState = RobotState.getInstance(); + private final IntakeRollers rollers; private final ShooterLeadCompensator shooterLeadCompensator; private final CommandXboxController driverController; @@ -46,20 +74,137 @@ public class Orchestrator { private final LinearFilter targetYFilter = LinearFilter.singlePoleIIR(0.06, 0.02); private final LinearFilter distanceFilter = LinearFilter.singlePoleIIR(0.06, 0.02); + private final Zone zone1; + private final Zone zone2; + private final Zone zone3; + private final Zone zone4; + public Orchestrator( Drive drive, MagicCarpet hopperBelt, Shooter shooter, Indexer indexer, Intake intake, + IntakeRollers rollers, CommandXboxController driverController) { this.drive = drive; this.magicCarpet = hopperBelt; this.shooter = shooter; this.indexer = indexer; this.intake = intake; + this.rollers = rollers; this.shooterLeadCompensator = new ShooterLeadCompensator(drive, shooter); this.driverController = driverController; + + this.zone1 = new Zone(drive::getPose); + this.zone2 = new Zone(drive::getPose); + this.zone3 = new Zone(drive::getPose); + this.zone4 = new Zone(drive::getPose); + zone1.initializeZone(new Tuple2d(0.0, 0.0), new Tuple2d(3.993527889251709, 8.100430488586426)); + zone2.initializeZone( + new Tuple2d(4.307533264160156, 8.04629135131836), new Tuple2d(11.9086332321167, 0)); + zone3.initializeZone( + new Tuple2d(13.69521713256836, 4.061668872833252), + new Tuple2d(16.51043891906738, 8.078774452209473)); + zone4.initializeZone( + new Tuple2d(13.69521713256836, 4.061668872833252), new Tuple2d(16.564579010009766, 0)); + } + + public Zone getZone1() { + return zone1; + } + + public Zone getZone2() { + return zone2; + } + + public Zone getZone3() { + return zone3; + } + + public Zone getZone4() { + return zone4; + } + + public ZoneId getCurrentZone() { + if (zone1.isInZoneForAlliance()) { + return ZoneId.ZONE_1; + } + if (zone2.isInZoneForAlliance()) { + return ZoneId.ZONE_2; + } + if (zone3.isInZoneForAlliance()) { + return ZoneId.ZONE_3; + } + if (zone4.isInZoneForAlliance()) { + return ZoneId.ZONE_4; + } + return ZoneId.NONE; + } + + public Command zoneBasedShooter() { + DoubleSupplier allianceY = () -> AllianceFlipUtil.applyY(drive.getPose().getY()); + + return new SelectCommand<>( + Map.ofEntries( + Map.entry(ZoneId.ZONE_1, spinUpShooterDistance(getHubDistance())), + Map.entry( + ZoneId.ZONE_2, + new ConditionalCommand( + spinUpShooterDistance( + () -> + Meters.of( + AllianceFlipUtil.apply(BBumpClosePose) + .getTranslation() + .getDistance((drive.getPose().getTranslation())))), + spinUpShooterDistance( + () -> + Meters.of( + AllianceFlipUtil.apply(AllianceFlipUtil.reflectY(BBumpClosePose)) + .getTranslation() + .getDistance(drive.getPose().getTranslation()))), + () -> allianceY.getAsDouble() > FieldConstants.fieldWidth / 2)), + Map.entry( + ZoneId.ZONE_3, + spinUpShooterDistance( + () -> + Meters.of( + AllianceFlipUtil.apply(BBumpFarPose) + .getTranslation() + .getDistance(drive.getPose().getTranslation())))), + Map.entry( + ZoneId.ZONE_4, + spinUpShooterDistance( + () -> + Meters.of( + AllianceFlipUtil.apply(AllianceFlipUtil.reflectY(BBumpFarPose)) + .getTranslation() + .getDistance(drive.getPose().getTranslation()))))), + this::getCurrentZone); + } + + public Command zoneBasedAim() { + DoubleSupplier allianceY = () -> AllianceFlipUtil.applyY(drive.getPose().getY()); + + return new SelectCommand<>( + Map.ofEntries( + Map.entry(ZoneId.ZONE_1, aimToHub()), + Map.entry( + ZoneId.ZONE_2, + new ConditionalCommand( + new RotateToOrientation(drive, () -> AllianceFlipUtil.apply(BBumpClosePose)), + new RotateToOrientation( + drive, + () -> AllianceFlipUtil.apply(AllianceFlipUtil.reflectY(BBumpClosePose))), + () -> allianceY.getAsDouble() > FieldConstants.fieldWidth / 2)), + Map.entry( + ZoneId.ZONE_3, + new RotateToOrientation(drive, () -> AllianceFlipUtil.apply(BBumpFarPose))), + Map.entry( + ZoneId.ZONE_4, + new RotateToOrientation( + drive, () -> AllianceFlipUtil.apply(AllianceFlipUtil.reflectY(BBumpFarPose))))), + this::getCurrentZone); } public Pose2d getShootWhileDrivingResultPose() { @@ -72,19 +217,24 @@ public Pose2d getShootWhileDrivingResultPose() { Rotation2d.fromDegrees(0)); } - public DoubleSupplier getShootWhileDrivingResultDistance() { + public Supplier getShootWhileDrivingResultDistance() { return () -> { var shootWhileDrivingResult = shooterLeadCompensator.shootWhileDriving( AllianceFlipUtil.apply(Hub.innerCenterPoint.toTranslation2d())); - return shootWhileDrivingResult.distance(); + return Meters.of(shootWhileDrivingResult.distance()); }; } - public DoubleSupplier getHubDistance() { + public Supplier getHubDistance() { return () -> - AllianceFlipUtil.apply(Hub.innerCenterPoint.toTranslation2d()) - .getDistance(drive.getPose().getTranslation()); + Meters.of( + AllianceFlipUtil.apply(Hub.innerCenterPoint.toTranslation2d()) + .getDistance( + drive + .getPose() + .transformBy(ShooterConstants.kShooterOffsetFromRobotCenter) + .getTranslation())); } private Rotation2d filteredHubAngle(Rotation2d raw) { @@ -98,6 +248,9 @@ private Rotation2d filteredHubAngle(Rotation2d raw) { } public void orchestratorPeriodic() { + Logger.recordOutput("Orchestrator/Pose", drive.getPose()); + Logger.recordOutput("Orchestrator/CurrentZone", getCurrentZone().name()); + var shootWhileDrivingResult = shooterLeadCompensator.shootWhileDriving( AllianceFlipUtil.apply(Hub.innerCenterPoint.toTranslation2d())); @@ -127,60 +280,47 @@ public Command driveToHub() { }); } - public Command alignToHub() { - return new RotateToOrientation( - drive, - () -> { - Pose2d hubApproachPose = AllianceFlipUtil.apply(Hub.nearFace); - Rotation2d angle = - hubApproachPose.getTranslation().minus(drive.getPose().getTranslation()).getAngle(); - return angle; - }); - } - public Command feedUp() { return Commands.repeatingSequence( - preloadBalls().until(() -> RobotState.getInstance().shooterAtSpeed), - Commands.parallel(magicCarpet.run(), indexer.run()) - .until(() -> !RobotState.getInstance().shooterAtSpeed)) - .onlyIf(() -> shooter.getSetpoint() > 0) - .onlyWhile(() -> shooter.getSetpoint() > 0) + Commands.parallel(indexer.run()).until(() -> !RobotState.getInstance().shooterAtSpeed)) + .onlyIf(() -> shooter.getSetpoint().gt(RadiansPerSecond.of(0))) + .onlyIf(() -> RobotState.getInstance().shooterAtSpeed) + .onlyWhile(() -> shooter.getSetpoint().gt(RadiansPerSecond.of(0))) .withName("feedUp"); } public Command spinUpShooterTest() { SmartDashboard.putNumber("Shooter/TestRPM", CLOSE_HUB_SHOOTER_RPM); return shooter - .setTargetVelocityRadians( - () -> - Units.rotationsPerMinuteToRadiansPerSecond( - SmartDashboard.getNumber("Shooter/TestRPM", 1000.0))) + .setTargetVelocity( + () -> Rotations.per(Minute).of(SmartDashboard.getNumber("Shooter/TestRPM", 1000.0))) .withName("spinUpShooterTest"); } - public Command spinUpShooterDistance(DoubleSupplier targetDistance) { - return shooter.setTargetVelocityRadians( - () -> - Units.rotationsPerMinuteToRadiansPerSecond( - shooter.calculateSetpoint(targetDistance).getAsDouble())); + public Command spinUpShooterDistance(Supplier targetDistance) { + return shooter.setTargetVelocity(shooter.calculateSetpoint(targetDistance)); } public Command spinUpShooterHub() { - return shooter.setTargetVelocityRadians( - Units.rotationsPerMinuteToRadiansPerSecond(CLOSE_HUB_SHOOTER_RPM)); + return shooter.setTargetVelocity(Rotations.per(Minute).of(CLOSE_HUB_SHOOTER_RPM)); } public Command spinUpShooter(double velocityRPM) { - return shooter.setTargetVelocityRadians( - Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM)); + return shooter.setTargetVelocity(Rotations.per(Minute).of(velocityRPM)); + } + + public Command shootWhileRetractingIntake(Command shooterCommand) { + return Commands.parallel( + intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS), shooterCommand, feedUp()) + .withName("shootWhileRetractingIntake"); } - public Command preloadBalls() { - return Commands.parallel(magicCarpet.run(), indexer.runPreloadSpeeds()) - .onlyWhile(() -> !RobotState.getInstance().indexerHasFuel) - .onlyIf(() -> !RobotState.getInstance().indexerHasFuel) - .until(() -> RobotState.getInstance().indexerHasFuel) - .andThen(indexer.stop().withTimeout(0.01)); + public Command shootWhileRetractingIntakeHub() { + return shootWhileRetractingIntake(spinUpShooterHub()); + } + + public Command shootWhileRetractingIntakeDistance(Supplier targetDistance) { + return shootWhileRetractingIntake(spinUpShooterDistance(targetDistance)); } public Command driveShootAtAngle() { @@ -201,8 +341,8 @@ public Command driveShootAtAngle() { } public Command spinUpShooterWhileDriving() { - return shooter.setTargetVelocityRadians( - () -> shooter.calculateSetpoint(this.getShootWhileDrivingResultDistance()).getAsDouble()); + return shooter.setTargetVelocity( + shooter.calculateSetpoint(this.getShootWhileDrivingResultDistance())); } public Command aimToHub() { @@ -213,7 +353,16 @@ public Command aimToHub() { drive.getPose().getX(), drive.getPose().getY(), AllianceFlipUtil.apply(Hub.blueCenter) - .minus(drive.getPose().getTranslation()) + .plus(new Translation2d(-0.4, 0)) + .minus( + drive + .getPose() + .transformBy(ShooterConstants.kShooterOffsetFromRobotCenter) + .getTranslation()) .getAngle())); } + + public Command stopShootingAuto() { + return Commands.parallel(indexer.stop(), shooter.stop()); + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index be8ea5a51..a205ec02e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,21 +4,20 @@ package frc.robot; +import static edu.wpi.first.units.Units.*; import static frc.robot.subsystems.drive.DriveConstants.ppConfig; -import static frc.robot.subsystems.shooter.ShooterConstants.CLOSE_HUB_SHOOTER_RPM; import static frc.robot.subsystems.vision.VisionConstants.*; import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.pathfinding.Pathfinding; import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.math.geometry.*; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.GenericHID.RumbleType; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -29,19 +28,20 @@ import frc.lib.utils.LocalADStarAK; import frc.robot.RobotState.IntakePosition; import frc.robot.commands.auto.Autos; -import frc.robot.commands.auto.DriveToPose; import frc.robot.commands.drive.DriveCommands; import frc.robot.commands.drive.DriveWithDpad; -import frc.robot.subsystems.climber.Climber; -import frc.robot.subsystems.climber.ClimberIO; import frc.robot.subsystems.drive.*; import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.indexer.IndexerIO; import frc.robot.subsystems.indexer.IndexerIOSparkMax; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeConstants; -import frc.robot.subsystems.intake.IntakeIO; -import frc.robot.subsystems.intake.IntakeIOKraken; +import frc.robot.subsystems.intake.extend.IntakeExtend; +import frc.robot.subsystems.intake.extend.IntakeExtendIO; +import frc.robot.subsystems.intake.extend.IntakeExtendIOSparkMax; +import frc.robot.subsystems.intake.rollers.IntakeRollers; +import frc.robot.subsystems.intake.rollers.IntakeRollersIO; +import frc.robot.subsystems.intake.rollers.IntakeRollersIOKraken; import frc.robot.subsystems.leds.Leds; import frc.robot.subsystems.magicCarpet.MagicCarpet; import frc.robot.subsystems.magicCarpet.MagicCarpetIO; @@ -71,8 +71,9 @@ public class RobotContainer { private Indexer indexer; private final Orchestrator orchestrator; private Shooter shooter; - private Climber climber; private Intake intake; + private IntakeRollers intakeRollers; + private IntakeExtend intakeExtend; private RobotState robotState = RobotState.getInstance(); private boolean isRobotOriented = true; // Workaround, change if needed @@ -101,8 +102,7 @@ public RobotContainer() { drive::addVisionMeasurement, new VisionIOPhotonVision(camera0Name, robotToCamera0), new VisionIOPhotonVision(camera1Name, robotToCamera1), - new VisionIOPhotonVision(camera2Name, robotToCamera2), - new VisionIOPhotonVision(camera3Name, robotToCamera3)); + new VisionIOPhotonVision(camera2Name, robotToCamera2)); leds = new Leds(); } case ROBOT_2026_COMP -> { @@ -118,18 +118,14 @@ public RobotContainer() { drive::addVisionMeasurement, new VisionIOPhotonVision(camera0Name, robotToCamera0), new VisionIOPhotonVision(camera1Name, robotToCamera1), - new VisionIOPhotonVision(camera2Name, robotToCamera2), - new VisionIOPhotonVision(camera3Name, robotToCamera3)); + new VisionIOPhotonVision(camera2Name, robotToCamera2)); leds = new Leds(); shooter = new Shooter(new ShooterIOSparkMax()); magicCarpet = new MagicCarpet(new MagicCarpetSparkMax()); indexer = new Indexer(new IndexerIOSparkMax()); - // TODO: GET THE ACTUAL BUTTON BINDINGS FOR THE OP SWITCHES - intake = - new Intake( - new IntakeIOKraken(), - operatorController.rightTrigger(), - operatorController.leftBumper()); + intakeRollers = new IntakeRollers(new IntakeRollersIOKraken()); + intakeExtend = + new IntakeExtend(new IntakeExtendIOSparkMax(), operatorController.rightTrigger()); // climber = new Climber(new ClimberIOPhysical()); } @@ -149,6 +145,7 @@ public RobotContainer() { // leds = new Leds(); // hopperBelt = new HopperBelt(new HopperBeltSparkMax()); leds = new Leds(); + shooter = new Shooter(new ShooterIOSparkMax(false)); } } } @@ -163,9 +160,13 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}); } - if (intake == null) { - intake = new Intake(new IntakeIO() {}, () -> false, () -> false); + if (intakeRollers == null) { + intakeRollers = new IntakeRollers(new IntakeRollersIO() {}); } + if (intakeExtend == null) { + intakeExtend = new IntakeExtend(new IntakeExtendIO() {}, () -> false); + } + intake = new Intake(intakeRollers, intakeExtend); if (magicCarpet == null) { magicCarpet = new MagicCarpet(new MagicCarpetIO() {}); } @@ -175,58 +176,17 @@ public RobotContainer() { if (indexer == null) { indexer = new Indexer(new IndexerIO() {}); } - if (climber == null) { - climber = new Climber(new ClimberIO() {}); - } - orchestrator = new Orchestrator(drive, magicCarpet, shooter, indexer, intake, driverController); - Autos autos = new Autos(drive, orchestrator, intake, shooter); - NamedCommands.registerCommand( - "startIntake", - Commands.parallel( - intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS), orchestrator.preloadBalls()) - .withTimeout(10.0)); - NamedCommands.registerCommand( - "endIntake", - Commands.parallel( - intake.stopIntakeCommand().withTimeout(0.05), - magicCarpet.stop().withTimeout(0.05), - indexer.stop().withTimeout(0.05)) - .withTimeout(0.05)); - NamedCommands.registerCommand( - "spinUp", - shooter.setTargetVelocityRadiansRepeatedly( - Units.rotationsPerMinuteToRadiansPerSecond(CLOSE_HUB_SHOOTER_RPM))); - NamedCommands.registerCommand("feedShooter", orchestrator.feedUp()); - NamedCommands.registerCommand("bringInIntake", intake.extendToAngleAndIntake(0)); - NamedCommands.registerCommand("driveToHub", orchestrator.driveToHub()); - NamedCommands.registerCommand("driveToHubAuto", orchestrator.driveToHub().withTimeout(3.0)); - NamedCommands.registerCommand( - "shootAuto", - Commands.sequence( - Commands.parallel( - shooter - .setTargetVelocityRadiansRepeatedly( - Units.rotationsPerMinuteToRadiansPerSecond(CLOSE_HUB_SHOOTER_RPM)) - .withTimeout(0.8), - intake.stopIntakeCommand(), - magicCarpet.stop(), - indexer.stop()) - .withTimeout(0.8), - Commands.deadline( - Commands.waitSeconds(3.2), - shooter.setTargetVelocityRadiansRepeatedly( - Units.rotationsPerMinuteToRadiansPerSecond(CLOSE_HUB_SHOOTER_RPM)), - orchestrator.feedUp()), - Commands.parallel( - magicCarpet.stop().withTimeout(0.05), indexer.stop().withTimeout(0.05)) - .withTimeout(0.05))); + orchestrator = + new Orchestrator( + drive, magicCarpet, shooter, indexer, intake, intakeRollers, driverController); + Autos autos = new Autos(drive, orchestrator, intake, intakeRollers, shooter); AutoBuilder.configure( drive::getPose, drive::setPose, drive::getChassisSpeeds, drive::runVelocity, - new PPHolonomicDriveController(new PIDConstants(12, 0, 0), new PIDConstants(9, 0, 0)), + new PPHolonomicDriveController(new PIDConstants(14.0, 0, 0), new PIDConstants(7, 0, 0)), ppConfig, () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, drive); @@ -241,12 +201,12 @@ public RobotContainer() { Logger.recordOutput("Odometry/TrajectorySetpoint", targetPose); }); - NamedCommands.registerCommand( - "Drive Back Left", - new DriveToPose(drive, () -> new Pose2d(2.798, 5.440, Rotation2d.fromDegrees(180)))); - NamedCommands.registerCommand( - "Drive Over Left", - new DriveToPose(drive, () -> new Pose2d(6.714, 5.440, Rotation2d.fromDegrees(180)))); + // NamedCommands.registerCommand( + // "Drive Back Left", + // new DriveToPose(drive, () -> new Pose2d(2.798, 5.440, Rotation2d.fromDegrees(180)))); + // NamedCommands.registerCommand( + // "Drive Over Left", + // new DriveToPose(drive, () -> new Pose2d(6.714, 5.440, Rotation2d.fromDegrees(180)))); // // NamedCommands.registerCommand("startShooter",Commands.parallel(orchestrator.preloadBalls(),orchestrator.prepShooter())); @@ -262,18 +222,15 @@ public RobotContainer() { autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); autoChooser.addDefaultOption("Do Nothing", Commands.none()); - - autoChooser.addOption("test path", autos.testPath()); - autoChooser.addOption("Bummmmpar", autos.Bummmmpar()); - autoChooser.addOption("Manual A-CC", autos.ACCManuelAuto()); + autoChooser.addOption( + "RunIntakeOut", intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)); + autoChooser.addOption("Manual A-CC", autos.ACCManualAuto()); autoChooser.addOption("Manual A-CC-Improved", autos.ACCManuelAutoAlt()); - autoChooser.addOption("Manual A-CC-Over-Bump", autos.ACCManuelAutoOverBump()); - autoChooser.addOption("Manual ADepot", autos.ADepot()); - autoChooser.addOption("Manual B-CC", autos.BCCManuelAuto()); + autoChooser.addOption("Manual A-CC-Over-Bump", autos.ACCManualAutoOverBump()); + autoChooser.addOption("Manual B-CC", autos.BCCManualAuto()); autoChooser.addOption("Manual B-CC-Improved", autos.BCCManuelAutoAlt()); - autoChooser.addOption("Manual B-CC-Over-Bump", autos.BCCManuelAutoOverBump()); + autoChooser.addOption("Manual B-CC-Over-Bump", autos.BCCManualAutoOverBump()); autoChooser.addOption("test path 2", drive.getAutonomousCommand("test path 2")); - autoChooser.addOption("CL auto", autos.CenterA()); autoChooser.addOption("8 Ball Auto", autos.EightBalls()); autoChooser.addOption( @@ -300,6 +257,11 @@ public RobotContainer() { "Drive SysId (Dynamic Forward)", drive.sysIdDynamic(SysIdRoutine.Direction.kForward)); autoChooser.addOption( "Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + + // pp + + autoChooser.addOption("A-side", autos.ppA2CycleRightRegression()); + autoChooser.addOption("B-side", autos.ppB2CycleRightRegression()); // Configure the button bindings configureButtonBindings(); } @@ -311,10 +273,8 @@ public RobotContainer() { * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - magicCarpet.setDefaultCommand(magicCarpet.stop()); indexer.setDefaultCommand(indexer.stop()); shooter.setDefaultCommand(shooter.stop()); - // shooter.setDefaultCommand(shooter.stop()); drive.setDefaultCommand( DriveCommands.joystickDrive( @@ -324,7 +284,11 @@ private void configureButtonBindings() { () -> -driverController.getRightX())); // Lock to 0 degrees when A button is held - + Trigger intaking = new Trigger(() -> RobotState.getInstance().intaking); + intaking.onTrue( + Commands.runOnce(() -> driverController.setRumble(RumbleType.kBothRumble, 0.2))); + intaking.onFalse( + Commands.runOnce(() -> driverController.setRumble(RumbleType.kBothRumble, 0.0))); driverController .start() .onTrue( @@ -336,28 +300,34 @@ private void configureButtonBindings() { .ignoringDisable(true)); new Trigger(() -> driverController.getHID().getPOV() != -1) .whileTrue(new DriveWithDpad(drive, () -> driverController.getHID().getPOV())); - driverController.x().toggleOnTrue(orchestrator.aimToHub()); + driverController.x().toggleOnTrue(orchestrator.zoneBasedAim()); driverController.y().toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS)); driverController .leftBumper() .and(operatorController.pov(180)) - .whileTrue(intake.runIntakeExtendVolts(-4)) - .onFalse(intake.stopExtendingCommand()); + .whileTrue(intakeExtend.runIntakeExtendVolts(-4)) + .onFalse(intakeExtend.stopExtendingCommand()); CustomTriggers.toggleIntakeUp( driverController.leftBumper(), () -> RobotState.getInstance().intakePosition == IntakePosition.DEPLOYED) .and(() -> !operatorController.pov(180).getAsBoolean()) - .toggleOnTrue(intake.extendToAngle(IntakeConstants.COLLAPSE_POS)); + .toggleOnTrue(intakeExtend.extendToAngle(IntakeConstants.COLLAPSE_POS)); CustomTriggers.toggleIntakeDown( driverController.leftBumper(), () -> RobotState.getInstance().intakePosition == IntakePosition.STOWED) .and(() -> !operatorController.pov(180).getAsBoolean()) - .toggleOnTrue(intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)); + .toggleOnTrue(intake.extendToAngle(IntakeConstants.EXTEND_POS)); + + // VERY IMPORTANT BECAUSE COMMAND GROUP DOESN'T MESH WITH SHOOTING DON'T COMBINE + // driverController.leftTrigger(0.2).toggleOnTrue(intake.runIntakeMotor()); - // VERY IMPORTANT BECAUSE COMMAND GROUP DOESN'T MESH WITH SHOOTING DON'T COMBINE - driverController.leftTrigger(0.2).toggleOnTrue(intake.intake()); driverController.rightTrigger(0.1).toggleOnTrue(orchestrator.feedUp()); - driverController.a().and(operatorController.pov(180)).onTrue(intake.resetExtendPosition()); + driverController.leftTrigger(0.2).toggleOnTrue(intake.runIntakeMotor()); + + driverController + .a() + .and(operatorController.pov(180)) + .onTrue(intakeExtend.resetExtendPosition()); driverController .rightBumper() .and(() -> !operatorController.pov(180).getAsBoolean()) @@ -365,29 +335,19 @@ private void configureButtonBindings() { driverController .rightBumper() .and(operatorController.pov(180)) - .whileTrue(intake.runIntakeExtendVolts(4)) - .onFalse(intake.stopExtendingCommand()); - // operatorController.rightTrigger(0.1).toggleOnTrue(orchestrator.spinUpShooterTest()); + .whileTrue(intakeExtend.runIntakeExtendVolts(4)) + .onFalse(intakeExtend.stopExtendingCommand()); + operatorController.leftTrigger(0.1).toggleOnTrue(orchestrator.spinUpShooterTest()); operatorController .rightTrigger(0.1) .and(() -> !operatorController.pov(0).getAsBoolean()) - .toggleOnTrue( - orchestrator.spinUpShooterDistance(orchestrator.getShootWhileDrivingResultDistance())); + .toggleOnTrue(orchestrator.zoneBasedShooter()); operatorController .rightTrigger(0.1) .and(operatorController.pov(0)) .toggleOnTrue(orchestrator.spinUpShooterHub()); - operatorController.leftTrigger(0.1).toggleOnTrue(orchestrator.spinUpShooterTest()); operatorController.y().whileTrue(indexer.reverse()); - operatorController.x().whileTrue(intake.outtake()); - - operatorController - .leftTrigger() - .whileTrue( - shooter.setTargetVelocityRadians( - () -> - operatorController.getLeftY() - * Units.rotationsPerMinuteToRadiansPerSecond(5600))); + operatorController.x().whileTrue(intakeRollers.outtake()); } /** @@ -403,4 +363,12 @@ public void robotPeriodic() { RobotState.getInstance().updateLEDState(); orchestrator.orchestratorPeriodic(); } + + private Command rumblePulse(double intensity, double seconds) { + return Commands.sequence( + Commands.runOnce( + () -> driverController.getHID().setRumble(RumbleType.kBothRumble, intensity)), + Commands.waitSeconds(seconds), + Commands.runOnce(() -> driverController.getHID().setRumble(RumbleType.kBothRumble, 0.0))); + } } diff --git a/src/main/java/frc/robot/Schematic.java b/src/main/java/frc/robot/Schematic.java index 9f4449875..0707243cf 100644 --- a/src/main/java/frc/robot/Schematic.java +++ b/src/main/java/frc/robot/Schematic.java @@ -20,16 +20,15 @@ public class Schematic { public static final int backRightAbsoluteEncoderCanId; // shooter subsystem - public static final int shooterBottomMotorCanId; - public static final int shooterTopMotorCanId; - public static final int shooterMiddleMotorCanId; + public static final int shooterTopLeftMotorCanId; + public static final int shooterTopRightMotorCanId; + public static final int shooterBottomLeftMotorCanId; + public static final int shooterBottomRightMotorCanId; // magic carpet sub-system public static final int magicCarpetCanId; // indexer subsystem public static final int indexerFeedupCanId; - public static final int indexerLeftLimitSwitchDIO; - public static final int indexerRightLimitSwitchDIO; // intake subsystem public static final int intakeMotorCanId; @@ -62,17 +61,16 @@ public class Schematic { backLeftAbsoluteEncoderCanId = 20; // Shooter (CAN Ids) - shooterBottomMotorCanId = 0; - shooterTopMotorCanId = 0; - shooterMiddleMotorCanId = 0; + shooterTopLeftMotorCanId = 0; + shooterTopRightMotorCanId = 0; + shooterBottomLeftMotorCanId = 0; + shooterBottomRightMotorCanId = 0; // Magic Carpet (CAN Ids) magicCarpetCanId = 0; // Indexer (CAN IDs and DIOs) indexerFeedupCanId = 0; - indexerLeftLimitSwitchDIO = 0; - indexerRightLimitSwitchDIO = 0; // Intake (CAN IDs) intakeMotorCanId = 0; @@ -87,33 +85,32 @@ public class Schematic { pigeonCanId = 17; // Drive (CAN Ids) - frontLeftDriveCanId = 3; - frontLeftTurnCanId = 4; - frontRightDriveCanId = 1; - frontRightTurnCanId = 2; + frontLeftDriveCanId = 5; + frontLeftTurnCanId = 6; + frontRightDriveCanId = 3; + frontRightTurnCanId = 4; - backRightDriveCanId = 7; - backRightTurnCanId = 8; - backLeftDriveCanId = 5; - backLeftTurnCanId = 6; + backRightDriveCanId = 1; + backRightTurnCanId = 2; + backLeftDriveCanId = 7; + backLeftTurnCanId = 8; - frontLeftAbsoluteEncoderCanId = 19; - frontRightAbsoluteEncoderCanId = 18; - backRightAbsoluteEncoderCanId = 20; + frontLeftAbsoluteEncoderCanId = 20; + frontRightAbsoluteEncoderCanId = 19; + backRightAbsoluteEncoderCanId = 18; backLeftAbsoluteEncoderCanId = 21; // Shooter (CAN Ids) - shooterTopMotorCanId = 11; - shooterMiddleMotorCanId = 12; - shooterBottomMotorCanId = 13; + shooterTopLeftMotorCanId = 33; + shooterTopRightMotorCanId = 35; + shooterBottomLeftMotorCanId = 34; + shooterBottomRightMotorCanId = 36; // Hopper (CAN Ids) magicCarpetCanId = 15; // Indexer (CAN Ids) indexerFeedupCanId = 9; - indexerLeftLimitSwitchDIO = 0; - indexerRightLimitSwitchDIO = 2; // Intake (CAN IDs) intakeMotorCanId = 23; @@ -143,17 +140,16 @@ public class Schematic { backRightAbsoluteEncoderCanId = 0; // Shooter (CAN Ids) - shooterBottomMotorCanId = 0; - shooterTopMotorCanId = 0; - shooterMiddleMotorCanId = 0; + shooterTopLeftMotorCanId = 8; + shooterTopRightMotorCanId = 1; + shooterBottomLeftMotorCanId = 7; + shooterBottomRightMotorCanId = 2; // Magic Carpet (CAN Ids) magicCarpetCanId = 0; // Indexer (CAN IDs and DIOs) indexerFeedupCanId = 0; - indexerLeftLimitSwitchDIO = 0; - indexerRightLimitSwitchDIO = 2; // Intake (CAN IDs) intakeMotorCanId = 0; @@ -183,17 +179,16 @@ public class Schematic { backRightAbsoluteEncoderCanId = 0; // Shooter (CAN Ids) - shooterBottomMotorCanId = 0; - shooterTopMotorCanId = 0; - shooterMiddleMotorCanId = 0; + shooterTopLeftMotorCanId = 0; + shooterTopRightMotorCanId = 0; + shooterBottomLeftMotorCanId = 0; + shooterBottomRightMotorCanId = 0; // Magic Carpet (CAN Ids) magicCarpetCanId = 0; // Indexer (CAN IDs and DIOs) indexerFeedupCanId = 0; - indexerLeftLimitSwitchDIO = 0; - indexerRightLimitSwitchDIO = 0; // Intake (CAN IDs) intakeMotorCanId = 0; diff --git a/src/main/java/frc/robot/commands/auto/Autos.java b/src/main/java/frc/robot/commands/auto/Autos.java index 0ff07b388..06eb4f0a5 100644 --- a/src/main/java/frc/robot/commands/auto/Autos.java +++ b/src/main/java/frc/robot/commands/auto/Autos.java @@ -10,198 +10,311 @@ import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeConstants; +import frc.robot.subsystems.intake.rollers.IntakeRollers; import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.ShooterConstants; import java.util.function.Supplier; +/** Contains all autos */ public class Autos { + + /** + * A helper class that tracks poses needed for manual autos. It simplifies switching between left + * and right sides. + * + * @param intakeSimple The position to intake at + * @param overBumpAllianceAlt The position to go over the bump + * @param shootFromCorner The position to shoot at the hub from + */ + public record AutoPositions( + Pose2d center, + Pose2d intakeSimple, + Pose2d overBumpNeutral, + Pose2d overBumpAlliance, + Pose2d overBumpAllianceAlt, + Pose2d shootFromCorner) {} + + // The necessary poses for autos on left side + private final AutoPositions poseA = + new AutoPositions( + /* center */ new Pose2d(3.457, 4.941, new Rotation2d(Units.degreesToRadians(-55.305))), + /* intakeSimple */ new Pose2d( + 7.7052903175354, 5.8276801109313965, Rotation2d.fromDegrees(0.0)), + /* overBumpNeutral */ new Pose2d(6.1, 5.574310302734375, Rotation2d.fromDegrees(0)), + /* overBumpAlliance */ new Pose2d( + 3.0666706562042236, 5.574310302734375, Rotation2d.fromDegrees(0.0)), + /* overBumpAllianceAlt */ new Pose2d( + 3.086160182952881, 5.437880039215088, Rotation2d.fromDegrees(0)), + /* shootFromCorner */ new Pose2d( + 3.086160182952881, 5.437880039215088, Rotation2d.fromRadians(-0.7553977556351216))); + + // The necessary poses for autos on right side + private final AutoPositions poseB = + new AutoPositions( + AllianceFlipUtil.reflectY(poseA.center), + AllianceFlipUtil.reflectY(poseA.intakeSimple), + AllianceFlipUtil.reflectY(poseA.overBumpNeutral), + AllianceFlipUtil.reflectY(poseA.overBumpAlliance), + AllianceFlipUtil.reflectY(poseA.overBumpAllianceAlt), + AllianceFlipUtil.reflectY(poseA.shootFromCorner)); + + // Relevant subsystems for the autos private final Drive drive; private final Orchestrator orchestrator; private final Intake intake; + private final IntakeRollers rollers; private final Shooter shooter; - public Autos(Drive drive, Orchestrator orchestrator, Intake intake, Shooter shooter) { + /** + * Basic constructor, takes in all initialized subsystems and stores them + * + * @param drive Drive subsystem + * @param orchestrator Orchestrator subsystem + * @param intake Intake subsystem + * @param rollers Intake rollers subsystem + * @param shooter Shooter subsystem + */ + public Autos( + Drive drive, + Orchestrator orchestrator, + Intake intake, + IntakeRollers rollers, + Shooter shooter) { this.drive = drive; this.orchestrator = orchestrator; this.intake = intake; + this.rollers = rollers; this.shooter = shooter; } - private static final Supplier climb = - () -> new Pose2d(1.583, 3.750, new Rotation2d(Units.degreesToRadians(180.000))); private static final Supplier center = () -> new Pose2d(3.504, 4.019, new Rotation2d(0)); - private static final Supplier CenterA = - () -> new Pose2d(3.457, 4.941, new Rotation2d(Units.degreesToRadians(-55.305))); - private static final Supplier firstPoseA = - () -> new Pose2d(7.7052903175354, 5.8276801109313965, Rotation2d.fromDegrees(0.0)); - private static final Supplier secondPoseA = - () -> new Pose2d(3.0666706562042236, 5.574310302734375, Rotation2d.fromDegrees(0.0)); - private static final Supplier secondPoseAAlt1 = - () -> new Pose2d(3.086160182952881, 5.437880039215088, Rotation2d.fromDegrees(0)); - private static final Supplier secondPoseAAlt2 = - () -> - new Pose2d( - 3.086160182952881, 5.437880039215088, Rotation2d.fromRadians(-0.7553977556351216)); - private static final Supplier firstPoseB = - () -> new Pose2d(7.724780082702637, 2.436420202255249, Rotation2d.fromDegrees(0.0)); - private static final Supplier secondPoseB = - () -> new Pose2d(3.1251401901245117, 2.397440195083618, Rotation2d.fromDegrees(0.0)); - private static final Supplier secondPoseBAlt1 = - () -> new Pose2d(3.1251401901245117, 2.397440195083618, Rotation2d.fromDegrees(0)); - private static final Supplier secondPoseBAlt2 = - () -> - new Pose2d( - 3.1251401901245117, 2.397440195083618, Rotation2d.fromRadians(0.7358299996216245)); private static final Supplier depotPoseStopPoint = () -> new Pose2d(1.7998201847076416, 5.983600616455078, Rotation2d.fromDegrees(180)); private static final Supplier depotPose = () -> new Pose2d(0.45501017570495605, 5.983600616455078, Rotation2d.fromDegrees(180)); - public Command CenterA() { - return Commands.sequence( - Commands.runOnce(() -> drive.setPose(AllianceFlipUtil.apply(CenterA.get()))), - drive.getAutonomousCommand("CA-out-intake"), - new StraightDriveToPose(climb.get(), drive).withTimeout(2.0)); - } - - public Command testPath() { - return Commands.sequence( - Commands.runOnce( - () -> - drive.setPose( - AllianceFlipUtil.apply( - new Pose2d(3.213, 5.600, new Rotation2d(Units.degreesToRadians(0)))))), - drive.getAutonomousCommand("test path")); - } + // pp - public Command Bummmmpar() { - return Commands.sequence( - Commands.runOnce( - () -> - drive.setPose( - AllianceFlipUtil.apply( - new Pose2d(3.213, 5.600, new Rotation2d(Units.degreesToRadians(0)))))), - new DriveToPose( - drive, - () -> AllianceFlipUtil.apply(new Pose2d(2.798, 5.440, Rotation2d.fromDegrees(0)))), - new DriveToPose( - drive, - () -> AllianceFlipUtil.apply(new Pose2d(6.714, 5.440, Rotation2d.fromDegrees(0)))), - new DriveToPose( - drive, - () -> AllianceFlipUtil.apply(new Pose2d(2.798, 5.440, Rotation2d.fromDegrees(0))))); - } + private static final Supplier startAside = + () -> AllianceFlipUtil.apply(new Pose2d(4.295, 7.531, new Rotation2d(0.000))); + private static final Supplier startBside = + () -> AllianceFlipUtil.apply(new Pose2d(4.295, 0.538, new Rotation2d(0.000))); + private static final Supplier startDepot = + () -> AllianceFlipUtil.apply(new Pose2d(3.630, 5.844, new Rotation2d(0.000))); - public Command EightBalls() { + /** + * Helper function for a single cycle auto with a constant shooting speed + * + * @param path The pathplanner path file name to use + * @param startPose A supplier that returns that starting position of the robot for this path + * @return A command that follows the path and shoots + */ + private Command ppCycle(String path, Supplier startPose) { return Commands.sequence( + Commands.runOnce(() -> drive.setPose(startPose.get())), Commands.deadline( - orchestrator.driveToHub().withTimeout(3.0), orchestrator.spinUpShooterHub()), - Commands.parallel(orchestrator.spinUpShooterHub(), orchestrator.feedUp())); + drive.getAutonomousCommand(path), + intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5), + orchestrator.spinUpShooterHub()) + .withTimeout(14.5), + orchestrator.aimToHub().withTimeout(2.5), + Commands.parallel(orchestrator.spinUpShooter(1215), orchestrator.feedUp()).withTimeout(2.5), + Commands.parallel( + intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS), + orchestrator.spinUpShooter(1214), + orchestrator.feedUp())); } - public Command ACCManuelAuto() { + private Command ppCycleConnect(String path) { return Commands.sequence( Commands.deadline( - new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseA.get())).withTimeout(5), - Commands.parallel( - intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS), - orchestrator.preloadBalls())), - new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseA.get())).withTimeout(5), - intake.stopIntakeCommand().withTimeout(0.05), - Commands.deadline( - orchestrator.driveToHub().withTimeout(3), orchestrator.spinUpShooterHub()), + drive.getAutonomousCommand(path), + intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5), + orchestrator.spinUpShooterHub()) + .withTimeout(14.5), + orchestrator.aimToHub().withTimeout(2.5), + Commands.parallel(orchestrator.spinUpShooter(1215), orchestrator.feedUp()).withTimeout(2.5), Commands.parallel( - orchestrator.spinUpShooterHub(), - orchestrator.feedUp(), - Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0)))); + intake.extendToAngleAndIntake(IntakeConstants.COLLAPSE_POS), + orchestrator.spinUpShooter(1214), + orchestrator.feedUp())); } - public Command ADepot() { + /** + * Helper function for a single cycle auto that uses shooter regression to shoot into the hub + * + * @param path The pathplanner path file name to use + * @param startPose A Supplier that returns the starting position of the robot for this path + * @return A command that follows the path and shoots + */ + private Command ppCycleRegression(String path, Supplier startPose) { return Commands.sequence( - new DriveToPose(drive, () -> AllianceFlipUtil.apply(depotPoseStopPoint.get())) - .withTimeout(2), - Commands.deadline( - new DriveToPose(drive, () -> AllianceFlipUtil.apply(depotPose.get())).withTimeout(3.5), - Commands.parallel( - intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS), - orchestrator.preloadBalls())), - intake.stopIntakeCommand().withTimeout(0.05), - Commands.deadline( - new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseAAlt2.get())) - .withTimeout(2), - orchestrator.spinUpShooter(1250)), + Commands.runOnce(() -> drive.setPose(startPose.get())), Commands.deadline( - Commands.waitSeconds(5), - orchestrator.spinUpShooter(1250), - orchestrator.feedUp(), - Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0))), - intake.stopIntakeCommand().withTimeout(0.05), - shooter.stop(), - new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseAAlt1.get())).withTimeout(2), + drive.getAutonomousCommand(path), + intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5), + Commands.waitSeconds(5) + .andThen(orchestrator.spinUpShooter(ShooterConstants.CLOSE_HUB_SHOOTER_RPM))) + .withTimeout(15.5), Commands.deadline( - new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseA.get())).withTimeout(3.5), - Commands.parallel( - intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS), - orchestrator.preloadBalls()))); + orchestrator.aimToHub().withTimeout(1).withTimeout(2.5), + orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())) + .andThen( + Commands.parallel( + Commands.waitSeconds(0.7).andThen(intake.slowlyBringInIntake()), + orchestrator.aimToHub().withTimeout(1).repeatedly(), + orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), + Commands.waitSeconds(0.4).andThen(orchestrator.feedUp())) + .withTimeout(1.6))); } - public Command ACCManuelAutoAlt() { + private Command ppCycleRegressionDepo(String path, Supplier startPose) { return Commands.sequence( + Commands.runOnce(() -> drive.setPose(startPose.get())), Commands.deadline( - new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseA.get())).withTimeout(5), - Commands.parallel( - intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS), - orchestrator.preloadBalls())), - new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseAAlt1.get())) - .withTimeout(3.5), - intake.stopIntakeCommand().withTimeout(0.05), + drive.getAutonomousCommand(path), + intake.extendToAngleAndIntake(IntakeConstants.FUNNEL_POS).withTimeout(5.5), + Commands.waitSeconds(5) + .andThen(orchestrator.spinUpShooter(ShooterConstants.CLOSE_HUB_SHOOTER_RPM))) + .withTimeout(15.5), Commands.deadline( - new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseAAlt2.get())) - .withTimeout(2), - orchestrator.spinUpShooter(1240)), - Commands.parallel( - orchestrator.spinUpShooter(1240), - orchestrator.feedUp(), - Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0)))); + orchestrator.aimToHub().withTimeout(1).withTimeout(2.5), + orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())) + .andThen( + Commands.parallel( + Commands.waitSeconds(0.7).andThen(intake.slowlyBringInIntake()), + orchestrator.aimToHub().withTimeout(1).repeatedly(), + orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), + Commands.waitSeconds(0.4).andThen(orchestrator.feedUp())) + .withTimeout(1.6))); } - public Command ACCManuelAutoOverBump() { + private Command ppCycleRegressionConnect(String path) { return Commands.sequence( Commands.deadline( - new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseA.get())).withTimeout(5), - Commands.parallel( - intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS), - orchestrator.preloadBalls())), - new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseAAlt1.get())) - .withTimeout(3.5), - intake.stopIntakeCommand().withTimeout(0.05), - Commands.deadline( - new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseAAlt2.get())) - .withTimeout(2), - orchestrator.spinUpShooter(1250)), - Commands.deadline( - Commands.waitSeconds(5), - orchestrator.spinUpShooter(1250), - orchestrator.feedUp(), - Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0))), - intake.stopIntakeCommand().withTimeout(0.05), - shooter.stop(), - new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseAAlt1.get())).withTimeout(2), + drive.getAutonomousCommand(path), + intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS).withTimeout(5.5), + Commands.waitSeconds(5) + .andThen(orchestrator.spinUpShooter(ShooterConstants.CLOSE_HUB_SHOOTER_RPM))) + .withTimeout(14.5), Commands.deadline( - new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseA.get())).withTimeout(3.5), - Commands.parallel( - intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS), - orchestrator.preloadBalls()))); + orchestrator.aimToHub().withTimeout(1).withTimeout(2.5), + orchestrator.spinUpShooterDistance(orchestrator.getHubDistance())) + .andThen( + Commands.parallel( + Commands.waitSeconds(0.7).andThen(intake.slowlyBringInIntake()), + orchestrator.aimToHub().withTimeout(1).repeatedly(), + orchestrator.spinUpShooterDistance(orchestrator.getHubDistance()), + Commands.waitSeconds(0.4).andThen(orchestrator.feedUp())))); + } + + /** + * Helper function for a double cycle auto that uses shooter regression to shoot into the hub + * twice (once after each cycle) + * + * @param path1 The pathplanner path file name to use for the first cycle + * @param path2 The pathplanner path file name to use for the second cycle + * @param startPose The starting position of the robot for the first path and the position it ends + * up in after the path + * @return A command that follows the paths and shoots + */ + private Command pp2CycleRegression(String path1, String path2, Supplier startPose) { + return ppCycleRegression(path1, startPose) + .withTimeout(14.5) + .andThen(orchestrator.stopShootingAuto().withTimeout(0.1)) + .andThen(ppCycleRegressionConnect(path2)); + } + + /** Depot Auto */ + public Command ppDepot() { + return ppCycleRegression("A-Cycle-LeftSweep", startDepot); + } + ; + + /** + * Left side pathplanner auto for a single cycle with fixed shooting distance + * + * @return A command that executes the auto + */ + public Command ppACycleLeft() { + return ppCycle("A-Cycle-LeftSweep", startAside); + } + + /** + * Right side pathplanner auto for a single cycle with fixed shooting distance + * + * @return A command that executes the auto + */ + public Command ppBCycleRight() { + return ppCycle("B-Cycle-RightSweep", startBside); + } + + /** + * Left side pathplanner auto for a single cycle that supports any shooting distance + * + * @return A command that executes the auto + */ + public Command ppB2CycleRightRegression() { + return pp2CycleRegression("B-Cycle1", "B-Cycle2", startBside); } - public Command BCCManuelAuto() { + /** + * Left side pathplanner auto for 2 cycles that support any shooting distance + * + * @return A comand that executes the auto + */ + public Command ppA2CycleRightRegression() { + return pp2CycleRegression("A-Cycle1", "A-Cycle2", startAside); + } + + /** + * Right side pathplanner auto for a single cycle that supports any shooting distance + * + * @return A command that executes the auto + */ + public Command ppBCycleRightRegression() { + return ppCycleRegression("B-Cycle-RightSweep", startBside); + } + + /** + * Left side pathplanner auto for a single cycle that supports any shooting distance + * + * @return A command that executes the auto + */ + public Command ppACycleLeftRegression() { + return ppCycleRegression("A-Cycle-LeftSweep", startAside); + } + + /** + * Gets relevant poses for the autos + * + * @param isA Whether to get the poses for A path or B path (true for A, false for B) + * @return The relevant poses for the calling auto + */ + AutoPositions getPoses(boolean isA) { + if (isA) { + return poseA; + } + return poseB; + } + + /** + * A helper function for a manual, straightDriveToPose auto that does one cycle with support for + * any shooting distance + * + * @param isA Whether to get the poses for A path or B path (true for A, false for B) + * @return A command that calls the auto + */ + private Command ccManualAuto(boolean isA) { + AutoPositions poses = getPoses(isA); + return Commands.sequence( Commands.deadline( - new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseB.get())).withTimeout(5), - Commands.parallel( - intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS), - orchestrator.preloadBalls())), - new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseB.get())).withTimeout(5), - intake.stopIntakeCommand().withTimeout(0.05), + new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.intakeSimple)).withTimeout(5), + intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)), + new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.overBumpAllianceAlt)) + .withTimeout(5), + rollers.stopIntakeCommand().withTimeout(0.05), Commands.deadline( orchestrator.driveToHub().withTimeout(3), orchestrator.spinUpShooterHub()), Commands.parallel( @@ -210,18 +323,45 @@ public Command BCCManuelAuto() { Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0)))); } - public Command BCCManuelAutoAlt() { + /** + * Left side straight drive to pose auto that does one cycle with support for any shooting + * distance + * + * @return A command that calls the auto + */ + public Command ACCManualAuto() { + return ccManualAuto(true); + } + + /** + * Right side straight drive to pose auto that does one cycle with support for any shooting + * distance + * + * @return A command that calls the auto + */ + public Command BCCManualAuto() { + return ccManualAuto(false); + } + + /** + * A helper function for a manual, straightDriveToPose auto that does one cycle with fixed + * shooting distance + * + * @param isA Whether to get the poses for A path or B path (true for A, false for B) + * @return A command that calls the auto + */ + private Command ccManualAutoAlt(boolean isA) { + AutoPositions poses = getPoses(isA); + return Commands.sequence( Commands.deadline( - new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseB.get())).withTimeout(5), - Commands.parallel( - intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS), - orchestrator.preloadBalls())), - new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseBAlt1.get())) + new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.intakeSimple)).withTimeout(5), + intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)), + new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.overBumpAllianceAlt)) .withTimeout(3.5), - intake.stopIntakeCommand().withTimeout(0.05), + rollers.stopIntakeCommand().withTimeout(0.05), Commands.deadline( - new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseBAlt2.get())) + new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.shootFromCorner)) .withTimeout(2), orchestrator.spinUpShooter(1240)), Commands.parallel( @@ -230,18 +370,43 @@ public Command BCCManuelAutoAlt() { Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0)))); } - public Command BCCManuelAutoOverBump() { + /** + * Left side straight drive to pose auto that does one cycle with fixed shooting distance + * + * @return A command that calls the auto + */ + public Command ACCManuelAutoAlt() { + return ccManualAutoAlt(true); + } + + /** + * Right side straight drive to pose auto that does one cycle with fixed shooting distance + * + * @return A command that calls the auto + */ + public Command BCCManuelAutoAlt() { + return ccManualAutoAlt(false); + } + + /** + * A helper function for a manual, straightDriveToPose auto that does one cycle with fixed + * shooting distance and then goes back to pick up more balls from the center. + * + * @param isA Whether to get the poses for A path or B path (true for A, false for B) + * @return A command that calls the auto + */ + private Command ccManualAutoOverBump(boolean isA) { + AutoPositions poses = getPoses(isA); + return Commands.sequence( Commands.deadline( - new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseB.get())).withTimeout(5), - Commands.parallel( - intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS), - orchestrator.preloadBalls())), - new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseBAlt1.get())) + new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.intakeSimple)).withTimeout(5), + intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS)), + new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.overBumpAllianceAlt)) .withTimeout(3.5), - intake.stopIntakeCommand().withTimeout(0.05), + rollers.stopIntakeCommand().withTimeout(0.05), Commands.deadline( - new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseBAlt2.get())) + new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.shootFromCorner)) .withTimeout(2), orchestrator.spinUpShooter(1250)), Commands.deadline( @@ -249,13 +414,45 @@ public Command BCCManuelAutoOverBump() { orchestrator.spinUpShooter(1250), orchestrator.feedUp(), Commands.waitSeconds(2).andThen(intake.extendToAngleAndIntake(0.0))), - intake.stopIntakeCommand().withTimeout(0.05), + rollers.stopIntakeCommand().withTimeout(0.05), shooter.stop(), - new DriveToPose(drive, () -> AllianceFlipUtil.apply(secondPoseBAlt1.get())).withTimeout(2), + new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.overBumpAllianceAlt)) + .withTimeout(2), Commands.deadline( - new DriveToPose(drive, () -> AllianceFlipUtil.apply(firstPoseB.get())).withTimeout(3.5), - Commands.parallel( - intake.holdAngleAndIntake(IntakeConstants.EXTEND_POS), - orchestrator.preloadBalls()))); + new DriveToPose(drive, () -> AllianceFlipUtil.apply(poses.intakeSimple)) + .withTimeout(3.5), + intake.extendToAngleAndIntake(IntakeConstants.EXTEND_POS))); + } + + /** + * Left side straight drive to pose auto that does one cycle with fixed shooting distance and then + * goes back to pick up more balls from the center. + * + * @return A command that cals the auto + */ + public Command ACCManualAutoOverBump() { + return ccManualAutoOverBump(true); + } + + /** + * Right side straight drive to pose auto that does one cycle with fixed shooting distance and + * then goes back to pick up more balls from the center. + * + * @return A command that cals the auto + */ + public Command BCCManualAutoOverBump() { + return ccManualAutoOverBump(false); + } + + /** + * Center auto that shoots the 8 balls we preload with and nothing else + * + * @return A command that calls the auto + */ + public Command EightBalls() { + return Commands.sequence( + Commands.deadline( + orchestrator.driveToHub().withTimeout(3.0), orchestrator.spinUpShooterHub()), + Commands.parallel(orchestrator.spinUpShooterHub(), orchestrator.feedUp())); } } diff --git a/src/main/java/frc/robot/commands/auto/DriveToPose.java b/src/main/java/frc/robot/commands/auto/DriveToPose.java index 9f51e5912..4b9eafb20 100644 --- a/src/main/java/frc/robot/commands/auto/DriveToPose.java +++ b/src/main/java/frc/robot/commands/auto/DriveToPose.java @@ -64,7 +64,7 @@ public class DriveToPose extends Command { thetaMaxVelocity.initDefault(Units.degreesToRadians(360.0)); thetaMaxAcceleration.initDefault(Units.degreesToRadians(720.0)); driveTolerance.initDefault(0.01); - thetaTolerance.initDefault(Units.degreesToRadians(1.0)); + thetaTolerance.initDefault(Units.degreesToRadians(0.2)); case ROBOT_2025_COMP: driveKp.initDefault(2.5); driveKd.initDefault(0.0); diff --git a/src/main/java/frc/robot/commands/auto/README.md b/src/main/java/frc/robot/commands/auto/README.md new file mode 100644 index 000000000..91c964929 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/README.md @@ -0,0 +1,10 @@ +# Autos + +This folder contains all the code that runs our autos and programs them. **Always sim your autos!** + +## Naming Scheme + +- "A" is left side +- "B" is right side +- "pp" stands for pathplanner autos +- "manual" stands for an auto that uses drive to pose diff --git a/src/main/java/frc/robot/commands/auto/RotateToOrientation.java b/src/main/java/frc/robot/commands/auto/RotateToOrientation.java index 48bcea7bc..9bdeec173 100644 --- a/src/main/java/frc/robot/commands/auto/RotateToOrientation.java +++ b/src/main/java/frc/robot/commands/auto/RotateToOrientation.java @@ -8,12 +8,13 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.drive.Drive; +import java.util.Objects; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; public class RotateToOrientation extends Command { private final Drive drive; - private final Supplier poseSupplier; + private final Supplier rotationSupplier; // private final ProfiledPIDController driveController = // new ProfiledPIDController( // 2.5, 0, 0.0, new Constraints(Units.inchesToMeters(150), Units.inchesToMeters(450.0))); @@ -40,14 +41,28 @@ public RotateToOrientation(Drive drive, Rotation2d targetPose) { this(drive, () -> targetPose, DRIVE_TOLERANCE); } - public RotateToOrientation(Drive drive, Supplier targetPoseSupplier) { - this(drive, targetPoseSupplier, DRIVE_TOLERANCE); + public RotateToOrientation(Drive drive, Pose2d targetPose) { + this( + drive, + () -> targetPose.getTranslation().minus(drive.getPose().getTranslation()).getAngle(), + DRIVE_TOLERANCE); + } + + public RotateToOrientation(Drive drive, Supplier targetPoseSupplier) { + this( + drive, + () -> + Objects.requireNonNull(targetPoseSupplier.get(), "targetPoseSupplier.get()") + .getTranslation() + .minus(drive.getPose().getTranslation()) + .getAngle(), + DRIVE_TOLERANCE); } public RotateToOrientation( Drive drive, Supplier targetPoseSupplier, double driveTolerance) { this.drive = drive; - this.poseSupplier = targetPoseSupplier; + this.rotationSupplier = Objects.requireNonNull(targetPoseSupplier, "targetPoseSupplier"); addRequirements(drive); thetaController.enableContinuousInput(-Math.PI, Math.PI); } @@ -55,8 +70,8 @@ public RotateToOrientation( @Override public void initialize() { Pose2d currentPose = drive.getPose(); - targetPose = poseSupplier.get(); - Logger.recordOutput("StraightDriveToPose/TargetPose", targetPose); + targetPose = rotationSupplier.get(); + Logger.recordOutput("RotateToOrientation/TargetPose", targetPose); thetaController.enableContinuousInput(-Math.PI, Math.PI); thetaController.setTolerance(THETA_TOLERANCE); thetaController.reset(currentPose.getRotation().getRadians()); @@ -65,6 +80,7 @@ public void initialize() { @Override public void execute() { Pose2d currentPose = drive.getPose(); + targetPose = rotationSupplier.get(); // Calculate theta speed double thetaVelocity = diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java deleted file mode 100644 index 8352349f6..000000000 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ /dev/null @@ -1,64 +0,0 @@ -package frc.robot.subsystems.climber; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import org.littletonrobotics.junction.Logger; - -public class Climber extends SubsystemBase { - - public final ClimberIO io; - private final ClimberIOInputsAutoLogged inputs; - private boolean rotateToTarget = false; - - public Climber(ClimberIO io) { - this.io = io; - inputs = new ClimberIOInputsAutoLogged(); - } - - @Override - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("Climber", inputs); - if (rotateToTarget) { - io.goToRotation(); - } - } - - public boolean atTargetRotation() { - return inputs.atTargetRotation; - } - - public Command toRotation(double degrees) { - return Commands.run( - () -> { - rotateToTarget = true; - io.setRotation(degrees); - }, - this) - .until(() -> inputs.atTargetRotation) - .withName("toRotation"); - } - - public Command runPercent(double percent) { - return Commands.run( - () -> { - rotateToTarget = false; - io.setPercent(percent); - }, - this) - .withName("runPercent"); - } - - public double getPositionDegrees() { - return inputs.positionDegrees; - } - - public boolean isCalibrated() { - return inputs.isCalibrated; - } - - public boolean limitSwitchPressed() { - return inputs.limitSwitch; - } -} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java deleted file mode 100644 index c4a775836..000000000 --- a/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java +++ /dev/null @@ -1,23 +0,0 @@ -package frc.robot.subsystems.climber; - -public class ClimberConstants { - public static final int CLIMBER_CURRENT_LIMIT = 80; - public static final int CLIMBER_SUPPLY_CURRENT_LIMIT = 40; - - public static final double ENCODER_CONVERSION_FACTOR = 360.0; - - public static final double STARTING_DEGREES = 0.0; - - // PID constants - public static final double CLIMBER_KP = 1.0; - public static final double CLIMBER_KI = 0.0; - public static final double CLIMBER_KD = 0.0; - - public static final double TOLERANCE = 1.0; // measured in degrees - - // Calibration constants - public static final int LIMIT_SWITCH_ID = 3; - public static final double CALIBRATION_POSITION_DEGREES = - 0.0; // position when limit switch is hit - public static final double CALIBRATION_PERCENT = -0.15; // slow speed to find limit switch -} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java deleted file mode 100644 index c162944a1..000000000 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ /dev/null @@ -1,25 +0,0 @@ -package frc.robot.subsystems.climber; - -import org.littletonrobotics.junction.AutoLog; - -public interface ClimberIO { - @AutoLog - class ClimberIOInputs { - public double positionDegrees = 0.0; - public double velocityDegreesPerSec = 0.0; - public double appliedVolts = 0.0; - public double currentAmps = 0.0; - public double targetRotation = 0.0; - public boolean atTargetRotation = false; - public boolean limitSwitch = false; - public boolean isCalibrated = false; - } - - default void updateInputs(ClimberIOInputs inputs) {} - - default void setPercent(double percent) {} - - default void setRotation(double degrees) {} - - default void goToRotation() {} -} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOPhysical.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOPhysical.java deleted file mode 100644 index e57a1bebd..000000000 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOPhysical.java +++ /dev/null @@ -1,98 +0,0 @@ -package frc.robot.subsystems.climber; - -import static frc.lib.utils.PhoenixUtil.*; -import static frc.robot.Schematic.climberMotorCanId; -import static frc.robot.subsystems.climber.ClimberConstants.*; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.units.measure.*; -import edu.wpi.first.wpilibj.DigitalInput; - -public class ClimberIOPhysical implements ClimberIO { - private final TalonFX talon; - private final DigitalInput limitSwitch; - - private final StatusSignal position; - private final StatusSignal velocity; - private final StatusSignal appliedVolts; - private final StatusSignal current; - - private final VoltageOut voltageRequest = new VoltageOut(0); - private final PositionVoltage positionRequest = new PositionVoltage(0); - - private boolean isCalibrated = false; - private double targetRotation; - - public ClimberIOPhysical() { - talon = new TalonFX(climberMotorCanId); - limitSwitch = new DigitalInput(LIMIT_SWITCH_ID); - - var config = new TalonFXConfiguration(); - config.MotorOutput.NeutralMode = NeutralModeValue.Brake; - config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - config.CurrentLimits.StatorCurrentLimit = CLIMBER_CURRENT_LIMIT; - config.CurrentLimits.StatorCurrentLimitEnable = true; - config.CurrentLimits.SupplyCurrentLimit = CLIMBER_SUPPLY_CURRENT_LIMIT; - config.CurrentLimits.SupplyCurrentLimitEnable = true; - config.Slot0.kP = CLIMBER_KP; - config.Slot0.kI = CLIMBER_KI; - config.Slot0.kD = CLIMBER_KD; - - tryUntilOk(5, () -> talon.getConfigurator().apply(config)); - tryUntilOk(5, () -> talon.setPosition(STARTING_DEGREES / ENCODER_CONVERSION_FACTOR)); - - position = talon.getPosition(); - velocity = talon.getVelocity(); - appliedVolts = talon.getMotorVoltage(); - current = talon.getStatorCurrent(); - - BaseStatusSignal.setUpdateFrequencyForAll(50.0, position, velocity, appliedVolts, current); - } - - @Override - public void updateInputs(ClimberIOInputs inputs) { - BaseStatusSignal.refreshAll(position, velocity, appliedVolts, current); - - inputs.positionDegrees = position.getValueAsDouble() * ENCODER_CONVERSION_FACTOR; - inputs.velocityDegreesPerSec = velocity.getValueAsDouble() * ENCODER_CONVERSION_FACTOR; - inputs.appliedVolts = appliedVolts.getValueAsDouble(); - inputs.currentAmps = current.getValueAsDouble(); - inputs.targetRotation = targetRotation; - inputs.atTargetRotation = - isCalibrated && Math.abs(targetRotation - inputs.positionDegrees) < TOLERANCE; - inputs.limitSwitch = !limitSwitch.get(); - - if (inputs.limitSwitch && !this.isCalibrated) { - this.isCalibrated = true; - tryUntilOk( - 5, () -> talon.setPosition(CALIBRATION_POSITION_DEGREES / ENCODER_CONVERSION_FACTOR)); - } - inputs.isCalibrated = this.isCalibrated; - } - - @Override - public void setPercent(double percent) { - talon.setControl(voltageRequest.withOutput(percent * 12.0)); - } - - @Override - public void setRotation(double degrees) { - this.targetRotation = degrees; - } - - @Override - public void goToRotation() { - if (!isCalibrated) { - setPercent(CALIBRATION_PERCENT); - } else { - talon.setControl(positionRequest.withPosition(targetRotation / ENCODER_CONVERSION_FACTOR)); - } - } -} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java deleted file mode 100644 index 47fa0aaac..000000000 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java +++ /dev/null @@ -1,3 +0,0 @@ -package frc.robot.subsystems.climber; - -public class ClimberIOSim {} diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index e9b93a329..4d405d38d 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -98,7 +98,7 @@ public Drive( @Override public void periodic() { - // logCameraPositions(); // uncomment to show camera positions in advantage scope + logCameraPositions(); // uncomment to show camera positions in advantage scope odometryLock.lock(); // Prevents odometry updates while reading data gyroIO.updateInputs(gyroInputs); Logger.processInputs("Drive/Gyro", gyroInputs); @@ -276,10 +276,6 @@ private void logCameraPositions() { "CameraPos/camera2position", new Pose3d(getPose().getX(), getPose().getY(), 0.0, new Rotation3d(getRotation())) .transformBy(VisionConstants.robotToCamera2)); - Logger.recordOutput( - "CameraPos/camera3position", - new Pose3d(getPose().getX(), getPose().getY(), 0.0, new Rotation3d(getRotation())) - .transformBy(VisionConstants.robotToCamera3)); } /** Returns the average velocity of the modules in rad/sec. */ diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 82f8d896e..a469e5c73 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.drive; import static frc.robot.Schematic.*; +import static java.lang.Math.PI; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants.ClosedLoopOutputType; @@ -15,8 +16,8 @@ public class DriveConstants { public static final double maxSpeedMetersPerSec = 11.8; public static final double odometryFrequency = 100.0; // Hz - public static final double trackWidth = 0.273 * 2; - public static final double wheelBase = 0.488; + public static final double trackWidth = 0.59055; + public static final double wheelBase = 0.5207; public static final double driveBaseRadius = Math.hypot(trackWidth / 2.0, wheelBase / 2.0); public static final Translation2d[] moduleTranslations = new Translation2d[] { @@ -35,10 +36,10 @@ public class DriveConstants { static { switch (Constants.getRobot()) { case ROBOT_2026_COMP -> { - frontLeftZeroRotation = Rotation2d.fromDegrees(-2.46); - frontRightZeroRotation = Rotation2d.fromDegrees(-10.63); - backLeftZeroRotation = Rotation2d.fromDegrees(17.11 + 180); - backRightZeroRotation = Rotation2d.fromDegrees(-45.27 + 180); + frontLeftZeroRotation = Rotation2d.fromRadians(0.126 + PI); + frontRightZeroRotation = Rotation2d.fromRadians(0.759 + 0.03); + backLeftZeroRotation = Rotation2d.fromRadians(-1.630); + backRightZeroRotation = Rotation2d.fromRadians(-2.072); } case ROBOT_2025_COMP -> { frontLeftZeroRotation = Rotation2d.fromDegrees(88.42 - 45); @@ -60,15 +61,15 @@ public class DriveConstants { public static final SwerveModuleConstants.ClosedLoopOutputType driveClosedLoopOutput = ClosedLoopOutputType.Voltage; public static final int driveMotorCurrentLimit = 50; - public static final double wheelRadiusMeters = Units.inchesToMeters(1.905); + public static final double wheelRadiusMeters = Units.inchesToMeters(2); public static final double driveMotorReduction = 8.16; public static final DCMotor driveGearbox = DCMotor.getKrakenX60Foc(1); // Drive encoder configuration public static final double driveEncoderPositionFactor = - 2 * Math.PI / driveMotorReduction; // Rotor Rotations -> Wheel Radians + 2 * PI / driveMotorReduction; // Rotor Rotations -> Wheel Radians public static final double driveEncoderVelocityFactor = - (2 * Math.PI) / 60.0 / driveMotorReduction; // Rotor RPM -> Wheel Rad/Sec + (2 * PI) / 60.0 / driveMotorReduction; // Rotor RPM -> Wheel Rad/Sec // Drive PID configuration public static final double driveKp = 2.525; @@ -84,26 +85,26 @@ public class DriveConstants { // Turn motor configuration public static final boolean turnInverted = false; public static final int turnMotorCurrentLimit = 10; - public static final double turnMotorReduction = 12.8; + public static final double turnMotorReduction = 26; public static final DCMotor turnGearbox = DCMotor.getNEO(1); // Turn encoder configuration public static final boolean turnEncoderInverted = false; public static final double turnEncoderPositionFactor = - 2 * Math.PI / turnMotorReduction; // Rotations -> Radians + 2 * PI / turnMotorReduction; // Rotations -> Radians public static final double turnEncoderVelocityFactor = - (2 * Math.PI) / 60.0 / turnMotorReduction; // RPM -> Rad/Sec + (2 * PI) / 60.0 / turnMotorReduction; // RPM -> Rad/Sec // Turn PID configuration - public static final double turnKp = 3.5; + public static final double turnKp = 12.0; public static final double turnKd = 0.0; public static final double turnSimP = 9.0; public static final double turnSimD = 0.0; - public static final double turnPIDMinInput = -Math.PI; // Radians - public static final double turnPIDMaxInput = Math.PI; // Radians + public static final double turnPIDMinInput = -PI; // Radians + public static final double turnPIDMaxInput = PI; // Radians // PathPlanner configuration - public static final double robotMassKg = 74.088; + public static final double robotMassKg = 51.00; public static final double robotMOI = 6.883; public static final double wheelCOF = 1.2; public static final RobotConfig ppConfig = diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index 2d91cb276..6f43a6785 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -1,7 +1,6 @@ package frc.robot.subsystems.indexer; -import static frc.robot.subsystems.indexer.IndexConstants.FEEDUP_VOLT; -import static frc.robot.subsystems.indexer.IndexConstants.PRELOAD_VOLT; +import static frc.robot.subsystems.indexer.IndexerConstants.FEEDUP_VOLT; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -20,9 +19,8 @@ public Indexer(IndexerIO io) { @Override public void periodic() { io.updateInputs(inputs); - RobotState.getInstance().indexerHasFuel = inputs.ballAtLeftSwitch || inputs.ballAtRightSwitch; - RobotState.getInstance().indexerRunning = inputs.feedUpVolts > 0; Logger.processInputs("Index", inputs); + RobotState.getInstance().indexerRunning = inputs.feedUpVolts > 0; } private void setPercent(double indexPercent, double feedUpPercent) { @@ -37,14 +35,6 @@ public double getVoltage() { return inputs.feedUpVolts; } - public boolean isLeftSwitchPressed() { - return io.isLeftSwitchPressed(); - } - - public boolean isRightSwitchPressed() { - return io.isRightSwitchPressed(); - } - public Command run() { return Commands.run( () -> { @@ -55,16 +45,6 @@ public Command run() { .withName("run"); } - public Command runPreloadSpeeds() { - return Commands.run( - () -> { - setVoltage(PRELOAD_VOLT); - }, - this) - .finallyDo(this::stop) - .withName("runPeriodic"); - } - public Command reverse() { return Commands.run( () -> { diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexConstants.java b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java similarity index 74% rename from src/main/java/frc/robot/subsystems/indexer/IndexConstants.java rename to src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java index b547905d1..2125a44b1 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexConstants.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java @@ -1,8 +1,7 @@ package frc.robot.subsystems.indexer; -public class IndexConstants { +public class IndexerConstants { public static final double ENCODER_FEEDUP_POSITION_CONVERSION = 1; public static final double ENCODER_FEEDUP_VELOCITY_CONVERSION = 1; - public static final double PRELOAD_VOLT = 3.0; public static final double FEEDUP_VOLT = 12; } diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java index f03f1af2b..9dad526d8 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java @@ -10,8 +10,6 @@ class IndexerIOInputs { public double feedUpPercentOutput = 0.0; public double feedUpVolts = 0.0; public double feedUpAmps = 0.0; - public boolean ballAtLeftSwitch = false; - public boolean ballAtRightSwitch = false; } default void updateInputs(IndexerIOInputs inputs) {} @@ -21,12 +19,4 @@ default void setPercent(double indexPercent, double feedUpPercent) {} default void setVoltage(double feedUpVolts) {} default void stop() {} - - default boolean isLeftSwitchPressed() { - return false; - } - - default boolean isRightSwitchPressed() { - return false; - } } diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java index a532d67d1..4536f6fdb 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkMax.java @@ -1,10 +1,8 @@ package frc.robot.subsystems.indexer; import static frc.robot.Schematic.indexerFeedupCanId; -import static frc.robot.Schematic.indexerLeftLimitSwitchDIO; -import static frc.robot.Schematic.indexerRightLimitSwitchDIO; -import static frc.robot.subsystems.indexer.IndexConstants.ENCODER_FEEDUP_POSITION_CONVERSION; -import static frc.robot.subsystems.indexer.IndexConstants.ENCODER_FEEDUP_VELOCITY_CONVERSION; +import static frc.robot.subsystems.indexer.IndexerConstants.ENCODER_FEEDUP_POSITION_CONVERSION; +import static frc.robot.subsystems.indexer.IndexerConstants.ENCODER_FEEDUP_VELOCITY_CONVERSION; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; @@ -13,15 +11,11 @@ import com.revrobotics.spark.config.EncoderConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; -import edu.wpi.first.wpilibj.DigitalInput; public class IndexerIOSparkMax implements IndexerIO { private final SparkMax feedUpMotor; - private final DigitalInput leftLimitSwitch; - private final DigitalInput rightLimitSwitch; - public IndexerIOSparkMax() { feedUpMotor = new SparkMax(indexerFeedupCanId, MotorType.kBrushless); @@ -42,9 +36,6 @@ public IndexerIOSparkMax() { feedUpMotor.configure( feedUpConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - - leftLimitSwitch = new DigitalInput(indexerLeftLimitSwitchDIO); - rightLimitSwitch = new DigitalInput(indexerRightLimitSwitchDIO); } @Override @@ -52,8 +43,6 @@ public void updateInputs(IndexerIOInputs inputs) { inputs.feedUpPercentOutput = feedUpMotor.get(); inputs.feedUpVolts = feedUpMotor.getBusVoltage() * feedUpMotor.getAppliedOutput(); inputs.feedUpAmps = feedUpMotor.getOutputCurrent(); - inputs.ballAtLeftSwitch = !leftLimitSwitch.get(); - inputs.ballAtRightSwitch = !rightLimitSwitch.get(); } @Override @@ -70,14 +59,4 @@ public void setVoltage(double feedUpVolts) { public void stop() { feedUpMotor.set(0); } - - @Override - public boolean isLeftSwitchPressed() { - return !leftLimitSwitch.get(); - } - - @Override - public boolean isRightSwitchPressed() { - return rightLimitSwitch.get(); - } } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 7ce9bc9a2..4d41fba74 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -2,256 +2,52 @@ import static frc.robot.subsystems.intake.IntakeConstants.*; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.RobotState; -import frc.robot.RobotState.IntakePosition; -import java.util.function.BooleanSupplier; -import org.littletonrobotics.junction.Logger; +import frc.robot.subsystems.intake.extend.IntakeExtend; +import frc.robot.subsystems.intake.rollers.IntakeRollers; -public class Intake extends SubsystemBase { - private final Timer stallExtendTimer = new Timer(); - private final Timer stallCollapseTimer = new Timer(); +public class Intake { + private final IntakeRollers rollers; + private final IntakeExtend extend; - private boolean stalledExtend = false; - private boolean stalledCollapse = false; - private boolean isStowed = false; - private boolean isExtended = false; - - private double targetExtendPosition = 0; - - private final IntakeIO io; - private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); - private final BooleanSupplier limitSwitchDisabled; - private final BooleanSupplier pidModeDisabled; - - /** - * Constructor for the Intake subsystem - * - * @param io The IO implementation to use - * @param limitSwitchDisabled A supplier to return whether the limit switch is currently disabled - * or not. If disabled, uses slipping to control intake. - */ - public Intake(IntakeIO io, BooleanSupplier limitSwitchDisabled, BooleanSupplier pidModeDisabled) { - this.io = io; - this.limitSwitchDisabled = () -> false; - this.pidModeDisabled = () -> false; - } - - @Override - public void periodic() { - io.updateInputs(inputs); - inputs.stalledExtended = stalledExtend; - inputs.stalledCollapsed = stalledCollapse; - inputs.stallExtendTimer = stallExtendTimer.get(); - inputs.stallCollapseTimer = stallCollapseTimer.get(); - Logger.processInputs("Intake", inputs); - RobotState.getInstance().intaking = inputs.intakeVolts > 0; - if (inputs.setpointValue == COLLAPSE_POS - && Math.abs(inputs.getExtendPos - inputs.setpointValue) < 10) { - RobotState.getInstance().intakePosition = IntakePosition.STOWED; - } - if (inputs.setpointValue == EXTEND_POS - && Math.abs(inputs.getExtendPos - inputs.setpointValue) < 10) { - RobotState.getInstance().intakePosition = IntakePosition.DEPLOYED; - } - - // setPID mode in io to true or false depending on the passed in trigger - if (pidModeDisabled.getAsBoolean() == io.getPIDEnabled()) { - io.setPIDEnabled(!pidModeDisabled.getAsBoolean()); - } - // Non-slipping control calibration based on the limit switch state - if (!limitSwitchDisabled.getAsBoolean() && isHopperCollapsed()) { - io.resetExtendEncoder(0); - isStowed = true; - stalledCollapse = false; - } - - // Slipping system - if (limitSwitchDisabled.getAsBoolean()) { - - // If we are stalling in extend, start timing, otherwise stop timing - if (!stalledExtend && isStallingExtend()) { - stallExtendTimer.start(); - } else { - stallExtendTimer.stop(); - stallExtendTimer.reset(); - } - - // If we are not stalled extended but the timer has exceeded the stall time, set to stalled - if (!stalledExtend && stallExtendTimer.get() > STALL_TIME) { - stalledExtend = true; - stallExtendTimer.stop(); - stallExtendTimer.reset(); - io.resetExtendEncoder(EXTEND_POS); - isExtended = true; - } - - // If we are stalling in collapse, start timing, otherwise stop timing - if (!stalledCollapse && isStallingCollapse()) { - stallCollapseTimer.start(); - } else { - stallCollapseTimer.stop(); - stallCollapseTimer.reset(); - } - - // If we are not stalled collapsed but the timer has exceeded the stall time, set to stalled - if (!stalledCollapse && stallCollapseTimer.get() > STALL_TIME) { - stalledCollapse = true; - stallCollapseTimer.stop(); - stallCollapseTimer.reset(); - io.resetExtendEncoder(0); - isStowed = true; - } - - // If we are moving, we are not stalled - if (isMoving()) { - stalledExtend = false; - stalledCollapse = false; - isExtended = false; - isStowed = false; - } - } - - Logger.recordOutput("Intake/StalledExtend", stalledExtend); - Logger.recordOutput("Intake/StalledCollapse", stalledCollapse); - } - - private void setPercentIntake(double intakePercent) { - io.setPercentIntake(intakePercent); - } - - private void setPercentExtend(double extendPercent) { - io.setPercentExtend(extendPercent); - } - - private void setVoltageIntake(double intakeVolts) { - io.setVoltageIntake(intakeVolts); - } - - private void setVoltageExtend(double extendVolts) { - io.setVoltageExtend(extendVolts); - } - - private void stopIntake() { - io.setVoltageIntake(0); - } - - private void stopExtend() { - io.setVoltageExtend(0); - } - - private boolean isHopperCollapsed() { - return io.isCollapsed(); - } - - private boolean isStallingExtend() { - // For our volts, we are not getting the right velocity - return Math.abs(inputs.extendVelocity) < STALL_VELOCITY && inputs.extendVolts > EXTEND_VOLTS; - } - - private boolean isStallingCollapse() { - return Math.abs(inputs.extendVelocity) < STALL_VELOCITY && inputs.extendVolts < COLLAPSE_VOLTS; - } - - public Command resetExtendPosition() { - return Commands.runOnce(() -> io.resetExtendEncoder(0), this); - } - - private boolean isMoving() { - return Math.abs(inputs.extendVelocity) > STALL_VELOCITY; - } - - public Command intake() { - return Commands.run(() -> setVoltageIntake(INTAKE_VOLTS), this) - .withName("intake") - .finallyDo(this::stopIntake); - } - - public Command outtake() { - return Commands.run(() -> setVoltageIntake(OUTTAKE_VOLTS), this) - .withName("outtake") - .finallyDo(this::stopIntake); - } - - public Command stopIntakeCommand() { - return Commands.run(this::stopIntake, this).withName("stopIntakeCommand"); - } - - public Command runIntakeExtendVolts(double volts) { - return Commands.run( - () -> { - io.setPIDEnabled(false); - io.setVoltageExtend(volts); - }, - this); - } - - public Command stopExtendingCommand() { - return Commands.run(this::stopExtend, this); + public Intake(IntakeRollers rollers, IntakeExtend extend) { + this.rollers = rollers; + this.extend = extend; } public Command extendToAngleAndIntake(double angle) { - return Commands.run( - () -> { - io.setPIDEnabled(true); - io.goToPos(angle); - io.setVoltageIntake(INTAKE_VOLTS); - }, - this) - .until(() -> inputs.atSetpoint) - .finallyDo(this::stopExtend) + return Commands.parallel(extend.extendToAngle(angle), runIntakeMotor()) .withName("extendToAngleAndIntake"); } public Command extendToAngle(double angle) { - return Commands.run( - () -> { - io.setPIDEnabled(true); - io.goToPos(angle); - }, - this) - .until(() -> inputs.atSetpoint) - .finallyDo(this::stopExtend) - .withName("extendToAngle"); + return extend.extendToAngle(angle); } - public Command holdAngleAndIntake(double angle) { - return Commands.run( - () -> { - io.setPIDEnabled(true); - io.goToPos(angle); - io.setVoltageIntake(INTAKE_VOLTS); - }, - this) - .finallyDo( - () -> { - stopExtend(); - stopIntake(); - }) - .withName("holdAngleAndIntake"); + public Command runIntakeMotor() { + return rollers.intake(); } - // Jack's Chugga Chugga mode + public Command slowlyBringInIntake() { + return Commands.parallel(rollers.intake(), extend.runIntakeExtendVolts(SLOW_VOLTS)) + .andThen(extend.extendToAngle(COLLAPSE_POS).repeatedly()); + } - // public Command shakeAndIntake() { - // return Commands.repeatingSequence( - // Commands.runOnce( - // () -> - // Commands.deadline( - // moveToAnglePrivate(FUNNEL_POS + SHAKE_POS_OFFSET), intakePrivate()), - // this), - // Commands.runOnce( - // () -> - // Commands.deadline( - // moveToAnglePrivate(FUNNEL_POS + SHAKE_POS_OFFSET), intakePrivate()), - // this)) - // .withName("shakeAndIntake"); - // } + // Jack's Chugga Chugga mode + public Command shakeAndIntake() { + return Commands.repeatingSequence( + Commands.deadline( + extend.extendToAngle(FUNNEL_POS + SHAKE_POS_OFFSET), runIntakeMotor()), + Commands.deadline( + extend.extendToAngle(FUNNEL_POS - SHAKE_POS_OFFSET), runIntakeMotor())) + .withName("shakeAndIntake"); + } - // private because it doesn't have requirements and therefore it shouldn't be called beyond the - // subsystem - // itself + public Command shake() { + return Commands.repeatingSequence( + extend.extendToAngle(FUNNEL_POS + SHAKE_POS_OFFSET), + extend.extendToAngle(FUNNEL_POS - SHAKE_POS_OFFSET)) + .withName("shake"); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index fe3914124..263ce3b4d 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -10,25 +10,30 @@ public class IntakeConstants { public static final double STALL_TIME = 0.1; public static final double STALL_VELOCITY = 0.1; - public static final double INTAKE_VOLTS = 12; - public static final double OUTTAKE_VOLTS = -10; + public static final double INTAKE_VOLTS = 8; + public static final double OUTTAKE_VOLTS = -12; public static final double EXTEND_VOLTS = 0.01; public static final double COLLAPSE_VOLTS = -0.01; public static final double INTAKE_MAX_VELOCITY = 200; - public static final int EXTEND_LIMIT_ID = 1; + public static final int EXTEND_LIMIT_ID = 0; - public static final double PID_P = 0.023; // arbritrary values + public static final double PID_P = 0.046; // arbritrary values public static final double PID_I = 0.000004; public static final double PID_D = 0.00001; - public static final double EXTEND_POS = -33; // TODO: change the actual values - public static final double FUNNEL_POS = -16.5; + public static final double EXTEND_POS = -13; // TODO: change the actual values + public static final double FUNNEL_POS = -10; // shake around the funnel pos by this much public static final double SHAKE_POS_OFFSET = 1; + public static final double HOME_VOLTAGE = 3; public static final double COLLAPSE_POS = 4.0; + public static final double SLOW_VOLTS = 2; + // How close we need to be to collapse position to stop the intake rollers + public static final double SAFETY_TOLERANCE = 2; public static final double POSITION_TOLERANCE = 7; - public static final double INTAKE_INTAKE_MOTOR_CURRENT_LIMIT = 45; + public static final double INTAKE_EXTEND_MOTOR_CURRENT_LIMIT = 30; + public static final double INTAKE_ROLLERS_MOTOR_CURRENT_LIMIT = 45; } diff --git a/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java new file mode 100644 index 000000000..bbf147252 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtend.java @@ -0,0 +1,166 @@ +package frc.robot.subsystems.intake.extend; + +import static frc.robot.subsystems.intake.IntakeConstants.COLLAPSE_POS; +import static frc.robot.subsystems.intake.IntakeConstants.EXTEND_POS; +import static frc.robot.subsystems.intake.IntakeConstants.HOME_VOLTAGE; +import static frc.robot.subsystems.intake.IntakeConstants.POSITION_TOLERANCE; +import static frc.robot.subsystems.intake.IntakeConstants.STALL_VELOCITY; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotState; +import frc.robot.RobotState.IntakePosition; +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.Logger; + +public class IntakeExtend extends SubsystemBase { + private final IntakeExtendIO io; + private final IntakeExtendIOInputsAutoLogged inputs = new IntakeExtendIOInputsAutoLogged(); + private final Timer stallExtendTimer = new Timer(); + private final Timer stallCollapseTimer = new Timer(); + + private boolean stalledExtend = false; + private boolean stalledCollapse = false; + private boolean isStowed = false; + private boolean isExtended = false; + private boolean hasPose = false; + + private double targetExtendPosition = 0; + + /** + * @param limitSwitchDisabled A supplier to return whether the limit switch is currently disabled + * or not. If disabled, uses slipping to control intake. + */ + private final BooleanSupplier limitSwitchDisabled; + + public void periodic() { + inputs.stalledExtended = stalledExtend; + inputs.stalledCollapsed = stalledCollapse; + inputs.stowed = isStowed; + inputs.stallExtendTimer = stallExtendTimer.get(); + inputs.stallCollapseTimer = stallCollapseTimer.get(); + io.updateInputs(inputs); + Logger.processInputs("Intake", inputs); + Logger.recordOutput("Intake/IntakeExtend/StalledExtend", stalledExtend); + Logger.recordOutput("Intake/IntakeExtended/StalledCollapse", stalledCollapse); + Logger.recordOutput("Intake/stallingExtend", isStallingExtend()); + Logger.recordOutput("Intake/stallingCollapse", isStallingCollapse()); + if (inputs.getExtendPos > EXTEND_POS / 2) { + RobotState.getInstance().intakePosition = IntakePosition.STOWED; + } + if (inputs.getExtendPos <= EXTEND_POS / 2) { + RobotState.getInstance().intakePosition = IntakePosition.DEPLOYED; + } + if (inputs.isCollapsed) { + if (!hasPose) { + Logger.recordOutput("Intake/IntakeExtend/HomeCompleted", true); + } + io.resetExtendEncoder(0.0); + hasPose = true; + } + inputs.hasPose = hasPose; + Logger.recordOutput("Intake/IntakeExtend/HasPose", hasPose); + } + + public IntakeExtend(IntakeExtendIO io, BooleanSupplier limitSwitchDisabled) { + this.io = io; + this.limitSwitchDisabled = limitSwitchDisabled; + } + + public boolean isHopperCollapsed() { + return io.isCollapsed(); + } + + public DoubleSupplier getAngle() { + return () -> inputs.getExtendPos; + } + + public void setIdleMode(boolean idleMode) { + io.setIdleMode(idleMode); + } + + public Command setPose(double pose) { + return Commands.runOnce(() -> io.resetExtendEncoder(pose), this); + } + + private boolean isStallingExtend() { + // For our volts, we are not getting the right velocity + return Math.abs(inputs.extendVelocity) < STALL_VELOCITY && inputs.extendVolts < -0.01; + } + + private boolean isStallingCollapse() { + return Math.abs(inputs.extendVelocity) < STALL_VELOCITY && inputs.extendVolts > 0.01; + } + + private void setPercentExtend(double extendPercent) { + io.setPercentExtend(extendPercent); + } + + public void setVoltageExtend(double extendVolts) { + io.setVoltageExtend(extendVolts); + } + + public void stopExtend() { + io.setVoltageExtend(0); + } + + public Command resetExtendPosition() { + return Commands.runOnce(() -> io.resetExtendEncoder(0), this); + } + + public Command moveToExtendedPosition() { + return Commands.run(() -> io.extendToPosition(EXTEND_POS)) + .until(() -> Math.abs(inputs.getExtendPos - EXTEND_POS) <= POSITION_TOLERANCE) + .withName("moveToExtendedPosition"); + } + + public Command moveToCollapsedPosition() { + return Commands.run(() -> io.extendToPosition(COLLAPSE_POS)) + .until(() -> Math.abs(inputs.getExtendPos - COLLAPSE_POS) <= POSITION_TOLERANCE) + .withName("moveToCollapsedPosition"); + } + + public Command runIntakeExtendVolts(double volts) { + return Commands.run( + () -> { + io.setPIDEnabled(false); + io.setVoltageExtend(volts); + }, + this); + } + + public Command stopExtendingCommand() { + return Commands.run(this::stopExtend, this); + } + + public Command homeExtend() { + return Commands.run( + () -> { + io.setPIDEnabled(false); + io.setVoltageExtend(HOME_VOLTAGE); + }, + this) + .beforeStarting(() -> Logger.recordOutput("Intake/IntakeExtend/HomeStarted", true)) + .until(() -> inputs.isCollapsed) + .finallyDo(this::stopExtend) + .withName("homeExtend"); + } + + public Command extendToAngle(double angle) { + return new ConditionalCommand(homeExtend(), Commands.none(), () -> !hasPose) + .andThen( + Commands.run( + () -> { + io.setPIDEnabled(true); + io.goToPos(angle); + }, + this) + .until(() -> inputs.atSetpoint) + .finallyDo(this::stopExtend) + .withName("extendToAngle")); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java similarity index 74% rename from src/main/java/frc/robot/subsystems/intake/IntakeIO.java rename to src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java index 21cfef84a..6a8695012 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIO.java @@ -1,15 +1,13 @@ -package frc.robot.subsystems.intake; +package frc.robot.subsystems.intake.extend; import org.littletonrobotics.junction.AutoLog; -public interface IntakeIO { +public interface IntakeExtendIO { + @AutoLog - class IntakeIOInputs { - public double intakePercentOutput = 0.0; + class IntakeExtendIOInputs { public double extendPercentOutput = 0.0; - public double intakeVolts = 0.0; public double extendVolts = 0.0; - public double intakeAmps = 0.0; public double extendVelocity = 0.0; public double extendAmps = 0.0; public boolean isCollapsed = false; @@ -22,16 +20,14 @@ class IntakeIOInputs { public boolean stalledCollapsed = false; public double stallExtendTimer = 0.0; public double stallCollapseTimer = 0.0; + public boolean stowed = false; + public boolean hasPose = false; } - default void updateInputs(IntakeIOInputs inputs) {} - - default void setPercentIntake(double intakePercent) {} + default void updateInputs(IntakeExtendIOInputs inputs) {} default void setPercentExtend(double extendPercent) {} - default void setVoltageIntake(double intakeVolts) {} - default void setVoltageExtend(double extendVolts) {} default void stop() {} @@ -40,6 +36,9 @@ default void goToPos(double pos) {} default void setPIDEnabled(boolean enabled) {} + default void setIdleMode(boolean cost) {} + ; + default boolean getPIDEnabled() { return false; } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOKraken.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOKraken.java similarity index 72% rename from src/main/java/frc/robot/subsystems/intake/IntakeIOKraken.java rename to src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOKraken.java index b9ee3ca05..814cfc72f 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOKraken.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOKraken.java @@ -1,20 +1,15 @@ -package frc.robot.subsystems.intake; +package frc.robot.subsystems.intake.extend; -import static frc.lib.utils.PhoenixUtil.tryUntilOk; import static frc.robot.Schematic.intakeExtendCanId; -import static frc.robot.Schematic.intakeMotorCanId; import static frc.robot.subsystems.intake.IntakeConstants.EXTEND_LIMIT_ID; import static frc.robot.subsystems.intake.IntakeConstants.EXTEND_POSITION_CONVERSION; import static frc.robot.subsystems.intake.IntakeConstants.EXTEND_VELOCITY_CONVERSION; -import static frc.robot.subsystems.intake.IntakeConstants.INTAKE_INTAKE_MOTOR_CURRENT_LIMIT; +import static frc.robot.subsystems.intake.IntakeConstants.INTAKE_EXTEND_MOTOR_CURRENT_LIMIT; import static frc.robot.subsystems.intake.IntakeConstants.PID_D; import static frc.robot.subsystems.intake.IntakeConstants.PID_I; import static frc.robot.subsystems.intake.IntakeConstants.PID_P; -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.revrobotics.RelativeEncoder; @@ -27,13 +22,9 @@ import com.revrobotics.spark.config.EncoderConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.DigitalInput; -public class IntakeIOKraken implements IntakeIO { - - private final TalonFX intakeMotor; +public class IntakeExtendIOKraken implements IntakeExtendIO { private final SparkMax extendMotor; private final DigitalInput collapsedLimitSwitch; private final RelativeEncoder extendMotorEncoder; @@ -41,17 +32,13 @@ public class IntakeIOKraken implements IntakeIO { private double setPos = 0; private boolean usingPID = false; - private final StatusSignal intakeAppliedVolts; - private final StatusSignal intakeCurrent; - - public IntakeIOKraken() { - intakeMotor = new TalonFX(intakeMotorCanId); + public IntakeExtendIOKraken() { extendMotor = new SparkMax(intakeExtendCanId, MotorType.kBrushless); var intakeConfig = new TalonFXConfiguration(); - intakeConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + intakeConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; intakeConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; - intakeConfig.CurrentLimits.StatorCurrentLimit = INTAKE_INTAKE_MOTOR_CURRENT_LIMIT; + intakeConfig.CurrentLimits.StatorCurrentLimit = INTAKE_EXTEND_MOTOR_CURRENT_LIMIT; intakeConfig.CurrentLimits.StatorCurrentLimitEnable = true; var extendConfig = new SparkMaxConfig(); @@ -63,13 +50,6 @@ public IntakeIOKraken() { .closedLoop .pid(PID_P, PID_I, PID_D); - tryUntilOk(5, () -> intakeMotor.getConfigurator().apply(intakeConfig)); - - intakeAppliedVolts = intakeMotor.getMotorVoltage(); - intakeCurrent = intakeMotor.getStatorCurrent(); - - BaseStatusSignal.setUpdateFrequencyForAll(50.0, intakeAppliedVolts, intakeCurrent); - EncoderConfig extendEnc = new EncoderConfig(); extendEnc.positionConversionFactor(EXTEND_POSITION_CONVERSION); extendEnc.velocityConversionFactor(EXTEND_VELOCITY_CONVERSION); @@ -84,14 +64,10 @@ public IntakeIOKraken() { } @Override - public void updateInputs(IntakeIOInputs inputs) { - BaseStatusSignal.refreshAll(intakeAppliedVolts, intakeCurrent); - + public void updateInputs(IntakeExtendIOInputs inputs) { inputs.extendPercentOutput = extendMotor.get(); inputs.extendVelocity = extendMotorEncoder.getVelocity(); - inputs.intakeVolts = intakeAppliedVolts.getValueAsDouble(); inputs.extendVolts = extendMotor.getAppliedOutput(); - inputs.intakeAmps = intakeCurrent.getValueAsDouble(); inputs.extendAmps = extendMotor.getOutputCurrent(); inputs.isCollapsed = !collapsedLimitSwitch.get(); inputs.getExtendPos = extendMotorEncoder.getPosition(); @@ -105,21 +81,11 @@ public void extendToPosition(double position) { pidController.setSetpoint(position, ControlType.kPosition); } - @Override - public void setPercentIntake(double intakePercent) { - intakeMotor.set(intakePercent); - } - @Override public void setPercentExtend(double extendPercent) { extendMotor.set(extendPercent); } - @Override - public void setVoltageIntake(double intakeVolts) { - intakeMotor.setVoltage(intakeVolts); - } - @Override public void setVoltageExtend(double extendVolts) { extendMotor.setVoltage(extendVolts); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java similarity index 67% rename from src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java rename to src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java index e84851154..c21bc77d9 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/intake/extend/IntakeExtendIOSparkMax.java @@ -1,16 +1,16 @@ -package frc.robot.subsystems.intake; +package frc.robot.subsystems.intake.extend; import static frc.robot.Schematic.intakeExtendCanId; -import static frc.robot.Schematic.intakeMotorCanId; import static frc.robot.subsystems.intake.IntakeConstants.EXTEND_LIMIT_ID; import static frc.robot.subsystems.intake.IntakeConstants.EXTEND_POSITION_CONVERSION; import static frc.robot.subsystems.intake.IntakeConstants.EXTEND_VELOCITY_CONVERSION; -import static frc.robot.subsystems.intake.IntakeConstants.INTAKE_POSITION_CONVERSION; +import static frc.robot.subsystems.intake.IntakeConstants.INTAKE_EXTEND_MOTOR_CURRENT_LIMIT; import static frc.robot.subsystems.intake.IntakeConstants.PID_D; import static frc.robot.subsystems.intake.IntakeConstants.PID_I; import static frc.robot.subsystems.intake.IntakeConstants.PID_P; import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.FeedbackSensor; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; @@ -22,9 +22,8 @@ import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.wpilibj.DigitalInput; -public class IntakeIOSparkMax implements IntakeIO { +public class IntakeExtendIOSparkMax implements IntakeExtendIO { - private final SparkMax intakeMotor; private final SparkMax extendMotor; private final DigitalInput collapsedLimitSwitch; private final RelativeEncoder extendMotorEncoder; @@ -32,59 +31,45 @@ public class IntakeIOSparkMax implements IntakeIO { private double setPos = 0; private boolean usingPID = false; - public IntakeIOSparkMax() { - intakeMotor = new SparkMax(intakeMotorCanId, MotorType.kBrushless); + public IntakeExtendIOSparkMax() { extendMotor = new SparkMax(intakeExtendCanId, MotorType.kBrushless); - var intakeConfig = new SparkMaxConfig(); - intakeConfig - .inverted(false) - .idleMode(IdleMode.kCoast) - .voltageCompensation(12) - .smartCurrentLimit(30); - var extendConfig = new SparkMaxConfig(); - extendConfig - .inverted(false) - .idleMode(IdleMode.kBrake) - .voltageCompensation(12) - .smartCurrentLimit(30) - .closedLoop - .pid(PID_P, PID_I, PID_D); - - EncoderConfig intakeEnc = new EncoderConfig(); - intakeEnc.positionConversionFactor(INTAKE_POSITION_CONVERSION); - intakeEnc.velocityConversionFactor(INTAKE_POSITION_CONVERSION); - intakeConfig.apply(intakeEnc); - - EncoderConfig extendEnc = new EncoderConfig(); - extendEnc.positionConversionFactor(EXTEND_POSITION_CONVERSION); - extendEnc.velocityConversionFactor(EXTEND_VELOCITY_CONVERSION); - extendConfig.apply(extendEnc); - pidController = extendMotor.getClosedLoopController(); extendMotorEncoder = extendMotor.getEncoder(); - intakeMotor.configure( - intakeConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); extendMotor.configure( - extendConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + getSparkMaxConfig(), ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); collapsedLimitSwitch = new DigitalInput(EXTEND_LIMIT_ID); } + public SparkMaxConfig getSparkMaxConfig() { + EncoderConfig extendEnc = new EncoderConfig(); + extendEnc.positionConversionFactor(EXTEND_POSITION_CONVERSION); + extendEnc.velocityConversionFactor(EXTEND_VELOCITY_CONVERSION); + var extendConfig = new SparkMaxConfig(); + extendConfig + .inverted(false) + .idleMode(IdleMode.kCoast) + .voltageCompensation(12) + .smartCurrentLimit((int) Math.round(INTAKE_EXTEND_MOTOR_CURRENT_LIMIT)) + .closedLoop + .pid(PID_P, PID_I, PID_D) + .feedbackSensor(FeedbackSensor.kPrimaryEncoder); + extendConfig.encoder.apply(extendEnc); + return extendConfig; + } + @Override - public void updateInputs(IntakeIOInputs inputs) { - inputs.intakePercentOutput = intakeMotor.get(); + public void updateInputs(IntakeExtendIOInputs inputs) { inputs.extendPercentOutput = extendMotor.get(); inputs.extendVelocity = extendMotorEncoder.getVelocity(); - inputs.intakeVolts = intakeMotor.getAppliedOutput(); inputs.extendVolts = extendMotor.getAppliedOutput(); - inputs.intakeAmps = intakeMotor.getOutputCurrent(); inputs.extendAmps = extendMotor.getOutputCurrent(); - inputs.isCollapsed = collapsedLimitSwitch.get(); + inputs.isCollapsed = isCollapsed(); inputs.getExtendPos = extendMotorEncoder.getPosition(); inputs.atSetpoint = extendMotor.getClosedLoopController().isAtSetpoint() && usingPID; inputs.hasSetpoint = usingPID; - inputs.setpointValue = usingPID ? extendMotor.getClosedLoopController().getSetpoint() : 0.0; + inputs.setpointValue = extendMotor.getClosedLoopController().getSetpoint(); } @Override @@ -97,21 +82,11 @@ public boolean isCollapsed() { return collapsedLimitSwitch.get(); } - @Override - public void setPercentIntake(double intakePercent) { - intakeMotor.set(intakePercent); - } - @Override public void setPercentExtend(double extendPercent) { extendMotor.set(extendPercent); } - @Override - public void setVoltageIntake(double intakeVolts) { - intakeMotor.setVoltage(intakeVolts); - } - @Override public void setVoltageExtend(double extendVolts) { extendMotor.setVoltage(extendVolts); @@ -129,6 +104,21 @@ public void setPIDEnabled(boolean enabled) { this.usingPID = enabled; } + @Override + public void setIdleMode(boolean coast) { + if (coast) { + extendMotor.configure( + getSparkMaxConfig().idleMode(IdleMode.kCoast), + ResetMode.kResetSafeParameters, + PersistMode.kNoPersistParameters); + } else { + extendMotor.configure( + getSparkMaxConfig().idleMode(IdleMode.kBrake), + ResetMode.kResetSafeParameters, + PersistMode.kNoPersistParameters); + } + } + @Override public void resetExtendEncoder(double position) { extendMotorEncoder.setPosition(position); diff --git a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java new file mode 100644 index 000000000..0cec68ba6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollers.java @@ -0,0 +1,53 @@ +package frc.robot.subsystems.intake.rollers; + +import static frc.robot.subsystems.intake.IntakeConstants.INTAKE_VOLTS; +import static frc.robot.subsystems.intake.IntakeConstants.OUTTAKE_VOLTS; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotState; +import org.littletonrobotics.junction.Logger; + +public class IntakeRollers extends SubsystemBase { + private final IntakeRollersIO io; + private final IntakeRollersIOInputsAutoLogged inputs = new IntakeRollersIOInputsAutoLogged(); + + public IntakeRollers(IntakeRollersIO io) { + this.io = io; + } + + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Intake/IntakeRollers", inputs); + RobotState.getInstance().intaking = inputs.intakeVolts > 0; + } + + private void setPercentIntake(double intakePercent) { + io.setPercentIntake(intakePercent); + } + + public void setVoltageIntake(double intakeVolts) { + io.setVoltageIntake(intakeVolts); + } + + private void stopIntake() { + io.setVoltageIntake(0); + } + + public Command intake() { + return Commands.run(() -> setVoltageIntake(INTAKE_VOLTS), this) + .finallyDo(this::stopIntake) + .withName("intake"); + } + + public Command outtake() { + return Commands.run(() -> setVoltageIntake(OUTTAKE_VOLTS), this) + .finallyDo(this::stopIntake) + .withName("outtake"); + } + + public Command stopIntakeCommand() { + return Commands.run(this::stopIntake).withName("stopIntakeCommand"); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIO.java b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIO.java new file mode 100644 index 000000000..305898aa5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIO.java @@ -0,0 +1,22 @@ +package frc.robot.subsystems.intake.rollers; + +import org.littletonrobotics.junction.AutoLog; + +public interface IntakeRollersIO { + @AutoLog + class IntakeRollersIOInputs { + public double intakePercentOutput = 0.0; + public double intakeVolts = 0.0; + public double intakeAmps = 0.0; + public double intakeRPM = 0.0; + // public boolean manualModeInput; + } + + default void updateInputs(IntakeRollersIOInputs inputs) {} + + default void setPercentIntake(double intakePercent) {} + + default void setVoltageIntake(double intakeVolts) {} + + default void stop() {} +} diff --git a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOKraken.java b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOKraken.java new file mode 100644 index 000000000..cbe116e01 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOKraken.java @@ -0,0 +1,58 @@ +package frc.robot.subsystems.intake.rollers; + +import static frc.lib.utils.PhoenixUtil.tryUntilOk; +import static frc.robot.Schematic.intakeMotorCanId; +import static frc.robot.subsystems.intake.IntakeConstants.INTAKE_ROLLERS_MOTOR_CURRENT_LIMIT; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; + +public class IntakeRollersIOKraken implements IntakeRollersIO { + + private final TalonFX intakeMotor; + private double setPos = 0; + + private final StatusSignal intakeAppliedVolts; + private final StatusSignal intakeCurrent; + + public IntakeRollersIOKraken() { + intakeMotor = new TalonFX(intakeMotorCanId); + + var intakeConfig = new TalonFXConfiguration(); + intakeConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + intakeConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + intakeConfig.CurrentLimits.StatorCurrentLimit = INTAKE_ROLLERS_MOTOR_CURRENT_LIMIT; + intakeConfig.CurrentLimits.StatorCurrentLimitEnable = true; + + tryUntilOk(5, () -> intakeMotor.getConfigurator().apply(intakeConfig)); + + intakeAppliedVolts = intakeMotor.getMotorVoltage(); + intakeCurrent = intakeMotor.getStatorCurrent(); + + BaseStatusSignal.setUpdateFrequencyForAll(50.0, intakeAppliedVolts, intakeCurrent); + } + + @Override + public void updateInputs(IntakeRollersIOInputs inputs) { + BaseStatusSignal.refreshAll(intakeAppliedVolts, intakeCurrent); + inputs.intakeVolts = intakeAppliedVolts.getValueAsDouble(); + inputs.intakeAmps = intakeCurrent.getValueAsDouble(); + inputs.intakeRPM = intakeMotor.getVelocity().getValueAsDouble() * 60; + } + + @Override + public void setPercentIntake(double intakePercent) { + intakeMotor.set(intakePercent); + } + + @Override + public void setVoltageIntake(double intakeVolts) { + intakeMotor.setVoltage(intakeVolts); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOSparkMax.java new file mode 100644 index 000000000..39b4ded2e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/rollers/IntakeRollersIOSparkMax.java @@ -0,0 +1,56 @@ +package frc.robot.subsystems.intake.rollers; + +import static frc.robot.Schematic.intakeMotorCanId; +import static frc.robot.subsystems.intake.IntakeConstants.INTAKE_EXTEND_MOTOR_CURRENT_LIMIT; +import static frc.robot.subsystems.intake.IntakeConstants.INTAKE_POSITION_CONVERSION; + +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.EncoderConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; + +public class IntakeRollersIOSparkMax implements IntakeRollersIO { + + private final SparkMax intakeMotor; + private double setPos = 0; + private boolean usingPID = false; + + public IntakeRollersIOSparkMax() { + intakeMotor = new SparkMax(intakeMotorCanId, MotorType.kBrushless); + + var intakeConfig = new SparkMaxConfig(); + intakeConfig + .inverted(false) + .idleMode(IdleMode.kCoast) + .voltageCompensation(12) + .smartCurrentLimit((int) Math.round(INTAKE_EXTEND_MOTOR_CURRENT_LIMIT)); + + EncoderConfig intakeEnc = new EncoderConfig(); + intakeEnc.positionConversionFactor(INTAKE_POSITION_CONVERSION); + intakeEnc.velocityConversionFactor(INTAKE_POSITION_CONVERSION); + intakeConfig.apply(intakeEnc); + } + + @Override + public void updateInputs(IntakeRollersIOInputs inputs) { + inputs.intakePercentOutput = intakeMotor.get(); + inputs.intakeVolts = intakeMotor.getAppliedOutput(); + inputs.intakeAmps = intakeMotor.getOutputCurrent(); + inputs.intakeRPM = intakeMotor.getEncoder().getVelocity(); + } + + @Override + public void setPercentIntake(double intakePercent) { + intakeMotor.set(intakePercent); + } + + @Override + public void setVoltageIntake(double intakeVolts) { + intakeMotor.setVoltage(intakeVolts); + } + + public void setPIDEnabled(boolean enabled) { + this.usingPID = enabled; + } +} diff --git a/src/main/java/frc/robot/subsystems/leds/LedConstants.java b/src/main/java/frc/robot/subsystems/leds/LedConstants.java index 66590c588..b37a506a3 100644 --- a/src/main/java/frc/robot/subsystems/leds/LedConstants.java +++ b/src/main/java/frc/robot/subsystems/leds/LedConstants.java @@ -7,7 +7,7 @@ public class LedConstants { public static final int LED_COUNT; /* num LEDs - change the first two numbers based on actual led strip */ - public static final int FULL_LENGTH = 42; // 2026 robot LED count + public static final int FULL_LENGTH = 43; // 2026 robot LED count public static final int BAR_LENGTH = Math.min(10, FULL_LENGTH); public static final int BASE_LENGTH = (FULL_LENGTH - BAR_LENGTH) / 2; @@ -46,6 +46,10 @@ public class LedConstants { LED_CHANNEL = 0; LED_COUNT = 14; } + case ROBOT_2026_COMP -> { + LED_CHANNEL = 0; + LED_COUNT = FULL_LENGTH; + } default -> { LED_CHANNEL = 0; LED_COUNT = 0; diff --git a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java index aa507f61b..3b49a1522 100644 --- a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java +++ b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java @@ -3,29 +3,55 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotState; import org.littletonrobotics.junction.Logger; public class MagicCarpet extends SubsystemBase { private final MagicCarpetIO io; private final MagicCarpetIOInputsAutoLogged inputs = new MagicCarpetIOInputsAutoLogged(); + public boolean manualRun; + /** + * Initializes the magic carpet + * + * @param io The implementation of the hardware + */ public MagicCarpet(MagicCarpetIO io) { this.io = io; + this.manualRun = false; } + /** + * Runs the magic carpet at a constant speed + * + * @return A command to run the magic carpet at a constant speed + */ public Command run() { - return Commands.run(() -> io.setSpeed(MagicCarpetConstants.BELT_SPEED), this) - .withName("start") - .finallyDo(this::stop); + return setManualControl(true) + .andThen(Commands.run(() -> io.setSpeed(MagicCarpetConstants.BELT_SPEED), this)) + .finallyDo(() -> setManualControl(false).schedule()) + .withName("start"); } - public Command stop() { - return Commands.run(() -> io.setSpeed(0.0), this).withName("stop"); + /** + * Control whether to run magic carpet through commands or periodic + * + * @param manual Call run manually instead of through periodic + */ + public Command setManualControl(boolean manual) { + return Commands.runOnce(() -> this.manualRun = manual, this); } @Override public void periodic() { io.updateInputs(inputs); Logger.processInputs("magicCarpet", inputs); + if (!manualRun) { + if (RobotState.getInstance().indexerRunning) { + io.setSpeed(MagicCarpetConstants.BELT_SPEED); + } else { + io.setSpeed(0.0); + } + } } } diff --git a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java index c318f0ee9..8d715dfe1 100644 --- a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java +++ b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetConstants.java @@ -2,10 +2,10 @@ public final class MagicCarpetConstants { - public static final boolean MOTOR_INVERTED = true; + public static final boolean MOTOR_INVERTED = false; - public static final int CURRENT_LIMIT = 30; - public static final double BELT_SPEED = 0.4; + public static final int CURRENT_LIMIT = 20; + public static final double BELT_SPEED = 0.8; // TODO: bump this value up if needed private MagicCarpetConstants() {} ; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index b9aac7cf9..80c857bb0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,12 +1,22 @@ package frc.robot.subsystems.shooter; +import static edu.wpi.first.units.Units.*; import static edu.wpi.first.units.Units.Second; import static edu.wpi.first.units.Units.Volts; +import static frc.robot.subsystems.shooter.ShooterConstants.FF_SCALAR; +import static frc.robot.subsystems.shooter.ShooterConstants.KA; +import static frc.robot.subsystems.shooter.ShooterConstants.KS; +import static frc.robot.subsystems.shooter.ShooterConstants.KV; import static frc.robot.subsystems.shooter.ShooterConstants.TOLERANCE; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.units.*; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.Time; +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.SubsystemBase; @@ -14,7 +24,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; import frc.robot.RobotState; -import java.util.function.DoubleSupplier; +import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; public class Shooter extends SubsystemBase { @@ -23,12 +33,11 @@ public class Shooter extends SubsystemBase { private final SysIdRoutine sysId; public boolean controllerEnabled = true; - private double targetRadPerSec = 0.0; + private AngularVelocity targetSpeed = RadiansPerSecond.of(0); private double rampedTarget = 0.0; // Feedforward: handles steady-state voltage (V = KS + KV * velocity) - private final SimpleMotorFeedforward feedforward = - new SimpleMotorFeedforward(ShooterConstants.KS, ShooterConstants.KV); + private final SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(KS, KV, KA); // PID: handles error correction on top of feedforward // P only — no I term (causes integral windup oscillation) private final PIDController pid = new PIDController(0.001, 0.1, 0.1, 0.02); @@ -36,8 +45,19 @@ public class Shooter extends SubsystemBase { // Slew rate limiter: ramps the target velocity gradually (rad/s per second) // This prevents current spikes that cause oscillation with a 20A limit private final SlewRateLimiter targetRamper = new SlewRateLimiter(800); + private double feedForwardScalar; + /** + * Initializes the shooter with a Shooter IO + * + * @param io A ShooterIO implementing instance + */ public Shooter(ShooterIO io) { + SmartDashboard.putNumber("Shooter/KS", KS * FF_SCALAR); + SmartDashboard.putNumber("Shooter/KA", KA * FF_SCALAR); + SmartDashboard.putNumber("Shooter/KV", KV * FF_SCALAR); + SmartDashboard.putNumber("Shooter/FeedForward_Scalar", FF_SCALAR); + feedForwardScalar = FF_SCALAR; this.io = io; sysId = new SysIdRoutine( @@ -51,11 +71,43 @@ public Shooter(ShooterIO io) { @Override public void periodic() { + + if (SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar) + != feedForwardScalar + || SmartDashboard.getNumber( + "Shooter/KS", + feedforward.getKs() + * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar)) + * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar) + != feedforward.getKs() + || SmartDashboard.getNumber("Shooter/KA", feedforward.getKa()) != feedforward.getKa() + || SmartDashboard.getNumber( + "Shooter/KV", + feedforward.getKv() + * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar)) + * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar) + != feedforward.getKv()) { + feedForwardScalar = SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar); + feedforward.setKa(SmartDashboard.getNumber("Shooter/KA", feedforward.getKa())); + feedforward.setKv( + SmartDashboard.getNumber( + "Shooter/KV", + feedforward.getKv() + * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar)) + * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar)); + feedforward.setKa( + SmartDashboard.getNumber( + "Shooter/KS", + feedforward.getKs() + * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar)) + * SmartDashboard.getNumber("Shooter/FeedForward_Scalar", feedForwardScalar)); + } io.updateInputs(inputs); - RobotState.getInstance().shooterAtSpeed = isAtSetpoint() && targetRadPerSec > 0; + RobotState.getInstance().shooterAtSpeed = + isAtSetpoint() && targetSpeed.gt(RadiansPerSecond.of(0)); if (controllerEnabled) { // Ramp toward the target to avoid current spikes - rampedTarget = targetRamper.calculate(targetRadPerSec); + rampedTarget = targetRamper.calculate(targetSpeed.in(RadiansPerSecond)); double ff = feedforward.calculate(rampedTarget); double pidOutput = pid.calculate(inputs.shooterWheelVelocityRadPerSec, rampedTarget); @@ -70,34 +122,57 @@ public void periodic() { Logger.recordOutput("Shooter/PIDVoltage", pidOutput); Logger.recordOutput("Shooter/CommandedVoltage", voltage); Logger.recordOutput("Shooter/RampedTargetRadPerSec", rampedTarget); - Logger.recordOutput("Shooter/Setpoint", targetRadPerSec); + Logger.recordOutput("Shooter/Setpoint", targetSpeed.in(RadiansPerSecond)); } Logger.processInputs("Shooter", inputs); Logger.recordOutput("Shooter/AtSetpoint", isAtSetpoint()); + RobotState.getInstance().shooterAtSpeed = isAtSetpoint(); } + /** + * Runs characterization voltage (used for FF) + * + * @param voltage The voltage to apply + */ public void runCharacterization(double voltage) { io.setVoltage(voltage); } + /** + * Runs SysID Quasistatic for feed forward + * + * @param direction The direction to run it in + * @return A command that runs SysID Quasistatic + */ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { return Commands.runOnce(() -> controllerEnabled = false, this) .andThen(run(() -> runCharacterization(0.0)).withTimeout(1.0)) .andThen(sysId.quasistatic(direction)); } + /** + * Runs SysID Quasistatic for feed forward + * + * @param direction The direction to run it in + * @return A command that runs SysID Dynamic + */ public Command sysIdDynamic(SysIdRoutine.Direction direction) { return Commands.runOnce(() -> controllerEnabled = false, this) .andThen(run(() -> runCharacterization(0.0)).withTimeout(1.0)) .andThen(sysId.dynamic(direction)); } + /** + * Stop the shooter + * + * @return A command that stops the shooter + */ public Command stop() { return Commands.runOnce( () -> { controllerEnabled = false; - targetRadPerSec = 0.0; + targetSpeed = RadiansPerSecond.of(0); rampedTarget = 0.0; targetRamper.reset(0.0); io.stop(); @@ -106,82 +181,105 @@ public Command stop() { .withName("shooter_stop_please"); } + /** + * Sets raw voltage for the shooter + * + * @param volts The volts amount to set it to (max 12) + * @return A Command that sets the shooter voltage + */ public Command setVoltage(double volts) { - return Commands.sequence( - Commands.runOnce(() -> controllerEnabled = false, this), - Commands.run(() -> io.setVoltage(volts), this)); - } - - public Command setTargetVelocityRPM(double rpm) { - return Commands.run( - () -> { - targetRadPerSec = ((rpm / 2 * Math.PI) * 60.0); - controllerEnabled = true; - }, - this) - .withName("setTargetVelocityRPM"); + return Commands.runOnce(() -> controllerEnabled = false, this) + .andThen(Commands.run(() -> io.setVoltage(volts), this)); } - public Command setTargetVelocityRadians(double radPerSec) { + /** + * Set the target velocity of the shooter + * + * @param velocity A supplier that returns the target velocity of the shooter + * @return A command that sets the target velocity of the shooter to the value returned by the + * supplier + */ + public Command setTargetVelocity(Supplier velocity) { return Commands.run( () -> { - targetRadPerSec = radPerSec; + targetSpeed = velocity.get(); controllerEnabled = true; }, this) - .withName("setTargetVelocityRadians"); + .finallyDo(io::stop) + .withName("setTargetVelocity"); } - public Command setTargetVelocityRadians(DoubleSupplier radPerSec) { + /** + * Set the target velocity of the shooter + * + * @param velocity The target velocity of the shooter + * @return A command that sets the target velocity of the shooter to the specified value + */ + public Command setTargetVelocity(AngularVelocity velocity) { return Commands.run( () -> { - targetRadPerSec = radPerSec.getAsDouble(); + targetSpeed = velocity; controllerEnabled = true; }, this) - .withName("setTargetVelocityRadians"); + .withName("setTargetVelocity"); } - /** - * Continuously re-asserts shooter target while scheduled. - * - *

Useful for toggle/manual control flows where another command may temporarily disable the - * shooter controller. - */ - public Command setTargetVelocityRadiansRepeatedly(double radPerSec) { + public Command setTargetVelocityRepeatedly(AngularVelocity velocity) { return Commands.repeatingSequence( Commands.runOnce( () -> { - targetRadPerSec = radPerSec; + targetSpeed = velocity; controllerEnabled = true; }, this), Commands.waitSeconds(0.02)) - .withName("setTargetVelocityRadiansRepeatedly"); + .withName("setTargetVelocityRepeatedly"); } + /** + * Check if the shooter is at setpoint (+- TOLERANCE) + * + * @return A boolean asserting whether the shooter is at setpoint (true for yes) + */ public boolean isAtSetpoint() { - return Math.abs(inputs.shooterWheelVelocityRadPerSec - (targetRadPerSec)) < TOLERANCE; + return Math.abs(inputs.shooterWheelVelocityRadPerSec - (targetSpeed.in(RadiansPerSecond))) + < TOLERANCE; } // TODO: empirically determine the relationship between distance and air time - public double getAirTimeSeconds(DoubleSupplier distance) { - return 0.0617 * distance.getAsDouble() + 0.872; + public Time getAirTimeSeconds(Distance distance) { + return Seconds.of(0.0617 * distance.in(Meters) + 0.872); } // TODO: empirically determine the relationship between distance and shooter velocity - - public DoubleSupplier calculateSetpoint(DoubleSupplier distance) { - // calculate rad/s depending on distance + /** + * Calculates a setpoint (in radians per second) based on distance + * + * @param distance Distance + * @return Setpoint in radians per second + */ + public Supplier calculateSetpoint(Supplier distance) { + // calculate rpm depending on distance return () -> { - double setpoint = 183.35 * distance.getAsDouble() + 750.93; - if (setpoint > 5000) { - return 5000; - } else return setpoint; + AngularVelocity velocity = + RPM.of( + 25.30184 * Math.pow(distance.get().in(Meters), 2) + - 47.12642 * distance.get().in(Meters) + + 1001.70713); + if (velocity.gt(RPM.of(1550))) { + return RPM.of(1550); + } else return velocity; }; } - public double getSetpoint() { - return targetRadPerSec; + /** + * Get the current target velocity of the shooter + * + * @return The target velocity of the shooter + */ + public AngularVelocity getSetpoint() { + return targetSpeed; } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index b5d07b404..f784e5bea 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -4,26 +4,33 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; public class ShooterConstants { public static final double ENCODER_POSITION_CONVERSION = 1; public static final double ENCODER_VELOCITY_CONVERSION = (2 * Math.PI) / 60; public static final double VOLTAGE_COMPENSATION = 12.0; - public static final int CURRENT_LIMIT = 20; + public static final int CURRENT_LIMIT = 35; public static final IdleMode IDLE_MODE = IdleMode.kCoast; - public static final double KV = 0.0465; - public static final double KA = 0.031259; - public static final double KS = 0.25; + public static final double KV = 0.0662; + public static final double KA = 0.028853; + public static final double KS = 0.2723; - public static final double SHOOTER_WHEEL_GEAR_RATIO = 2.5; - public static final double CLOSE_HUB_SHOOTER_RPM = 1085; + // for 3.7647 gear ratio + // public static final double KV = 0.0662; + // public static final double KA = 0.028853; + // public static final double KS = 0.2723; + public static final double FF_SCALAR = 1; + public static final double SHOOTER_WHEEL_GEAR_RATIO = 3.5; + public static final double CLOSE_HUB_SHOOTER_RPM = 1100; public static final double MAX_VOLTAGE = 12.0; - public static final double TOLERANCE = 50; // measured in radians + public static final double TOLERANCE = + Units.rotationsPerMinuteToRadiansPerSecond(100); // measured in radians public static Transform2d kShooterOffsetFromRobotCenter = - new Transform2d(new Translation2d(-0.163, 0.0), new Rotation2d(0.0)); + new Transform2d(new Translation2d(-0.1585, 0.0), new Rotation2d(0.0)); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index 03f011667..06d151543 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -6,20 +6,21 @@ public interface ShooterIO { @AutoLog class ShooterIOInputs { - public double middleMotorVelocityRadPerSec; - public double middleMotorAppliedVolts; - public double middleMotorCurrentAmps; + public double topLeftMotorVelocityRadPerSec; + public double topLeftMotorAppliedVolts; + public double topLeftMotorCurrentAmps; - public double bottomMotorVelocityRadPerSec; - public double bottomMotorAppliedVolts; - public double bottomMotorCurrentAmps; + public double topRightMotorVelocityRadPerSec; + public double topRightMotorAppliedVolts; + public double topRightMotorCurrentAmps; - public double topMotorVelocityRadPerSec; - public double topMotorAppliedVolts; - public double topMotorCurrentAmps; + public double bottomLeftMotorVelocityRadPerSec; + public double bottomLeftMotorAppliedVolts; + public double bottomLeftMotorCurrentAmps; - public boolean atSetpoint = false; - public double setpointRPM = 0; + public double bottomRightMotorVelocityRadPerSec; + public double bottomRightMotorAppliedVolts; + public double bottomRightMotorCurrentAmps; public double totalAmps; @@ -31,5 +32,13 @@ default void updateInputs(ShooterIOInputs inputs) {} default void setVoltage(double volts) {} + default void setTopLeftVoltage(double volts) {} + + default void setTopRightVoltage(double volts) {} + + default void setBottomLeftVoltage(double volts) {} + + default void setBottomRightVoltage(double volts) {} + default void stop() {} } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java index bb97cfc34..17cb653a8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOSparkMax.java @@ -1,8 +1,9 @@ package frc.robot.subsystems.shooter; -import static frc.robot.Schematic.shooterBottomMotorCanId; -import static frc.robot.Schematic.shooterMiddleMotorCanId; -import static frc.robot.Schematic.shooterTopMotorCanId; +import static frc.robot.Schematic.shooterBottomLeftMotorCanId; +import static frc.robot.Schematic.shooterBottomRightMotorCanId; +import static frc.robot.Schematic.shooterTopLeftMotorCanId; +import static frc.robot.Schematic.shooterTopRightMotorCanId; import static frc.robot.subsystems.shooter.ShooterConstants.*; import com.revrobotics.RelativeEncoder; @@ -15,97 +16,126 @@ public class ShooterIOSparkMax implements ShooterIO { - private final SparkMax middleMotor; - private final SparkMax bottomMotor; - private final SparkMax topMotor; - private final RelativeEncoder middleMotorEncoder; - private final RelativeEncoder bottomMotorEncoder; - private final RelativeEncoder topMotorEncoder; + private final SparkMax topLeftMotor; + private final SparkMax topRightMotor; + private final SparkMax bottomLeftMotor; + private final SparkMax bottomRightMotor; + private final RelativeEncoder topLeftMotorEncoder; + private final RelativeEncoder topRightMotorEncoder; + private final RelativeEncoder bottomLeftMotorEncoder; + private final RelativeEncoder bottomRightMotorEncoder; public ShooterIOSparkMax() { - middleMotor = new SparkMax(shooterMiddleMotorCanId, MotorType.kBrushless); - bottomMotor = new SparkMax(shooterBottomMotorCanId, MotorType.kBrushless); - topMotor = new SparkMax(shooterTopMotorCanId, MotorType.kBrushless); + this(false); + } + + public ShooterIOSparkMax(boolean independentMode) { + topLeftMotor = new SparkMax(shooterTopLeftMotorCanId, MotorType.kBrushless); + topRightMotor = new SparkMax(shooterTopRightMotorCanId, MotorType.kBrushless); + bottomLeftMotor = new SparkMax(shooterBottomLeftMotorCanId, MotorType.kBrushless); + bottomRightMotor = new SparkMax(shooterBottomRightMotorCanId, MotorType.kBrushless); + // + EncoderConfig enc = new EncoderConfig(); + enc.positionConversionFactor(ENCODER_POSITION_CONVERSION); + enc.velocityConversionFactor(ENCODER_VELOCITY_CONVERSION); - var middleMotorConfig = new SparkMaxConfig(); - middleMotorConfig + var bottomLeftMotorConfig = new SparkMaxConfig(); + bottomLeftMotorConfig .inverted(false) .idleMode(IDLE_MODE) .voltageCompensation(VOLTAGE_COMPENSATION) - .smartCurrentLimit(CURRENT_LIMIT); + .smartCurrentLimit(CURRENT_LIMIT) + .secondaryCurrentLimit(CURRENT_LIMIT); + bottomLeftMotorConfig.apply(enc); - var bottomMotorConfig = new SparkMaxConfig(); - bottomMotorConfig + var topLeftMotorConfig = new SparkMaxConfig(); + topLeftMotorConfig .inverted(true) .idleMode(IDLE_MODE) .voltageCompensation(VOLTAGE_COMPENSATION) - .smartCurrentLimit(CURRENT_LIMIT); + .smartCurrentLimit(CURRENT_LIMIT) + .secondaryCurrentLimit(CURRENT_LIMIT); + topLeftMotorConfig.apply(enc); - var topMotorConfig = new SparkMaxConfig(); - topMotorConfig - .inverted(false) + var bottomRightMotorConfig = new SparkMaxConfig(); + bottomRightMotorConfig + .inverted(true) .idleMode(IDLE_MODE) .voltageCompensation(VOLTAGE_COMPENSATION) - .smartCurrentLimit(CURRENT_LIMIT); - - topMotorConfig.follow(bottomMotor.getDeviceId(), false); - middleMotorConfig.follow(bottomMotor.getDeviceId(), true); + .smartCurrentLimit(CURRENT_LIMIT) + .secondaryCurrentLimit(CURRENT_LIMIT); + bottomRightMotorConfig.apply(enc); - EncoderConfig enc = new EncoderConfig(); - enc.positionConversionFactor(ENCODER_POSITION_CONVERSION); - enc.velocityConversionFactor(ENCODER_VELOCITY_CONVERSION); - middleMotorConfig.apply(enc); - bottomMotorConfig.apply(enc); - topMotorConfig.apply(enc); - - middleMotor.configure( - middleMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - bottomMotor.configure( - bottomMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - topMotor.configure( - topMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - - middleMotorEncoder = middleMotor.getEncoder(); - bottomMotorEncoder = bottomMotor.getEncoder(); - topMotorEncoder = topMotor.getEncoder(); + var topRightMotorConfig = new SparkMaxConfig(); + topRightMotorConfig + .inverted(false) + .idleMode(IDLE_MODE) + .voltageCompensation(VOLTAGE_COMPENSATION) + .smartCurrentLimit(CURRENT_LIMIT) + .secondaryCurrentLimit(CURRENT_LIMIT); + topRightMotorConfig.apply(enc); + + bottomLeftMotor.configure( + bottomLeftMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + topLeftMotor.configure( + topLeftMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + bottomRightMotor.configure( + bottomRightMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + topRightMotor.configure( + topRightMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + topLeftMotorEncoder = topLeftMotor.getEncoder(); + topRightMotorEncoder = topRightMotor.getEncoder(); + bottomLeftMotorEncoder = bottomLeftMotor.getEncoder(); + bottomRightMotorEncoder = bottomRightMotor.getEncoder(); } @Override public void updateInputs(ShooterIOInputs inputs) { - inputs.middleMotorCurrentAmps = middleMotor.getOutputCurrent(); - inputs.middleMotorAppliedVolts = middleMotor.getBusVoltage() * middleMotor.getAppliedOutput(); - inputs.middleMotorVelocityRadPerSec = middleMotorEncoder.getVelocity(); - - inputs.bottomMotorCurrentAmps = bottomMotor.getOutputCurrent(); - inputs.bottomMotorAppliedVolts = bottomMotor.getBusVoltage() * bottomMotor.getAppliedOutput(); - inputs.bottomMotorVelocityRadPerSec = bottomMotorEncoder.getVelocity(); - - inputs.topMotorCurrentAmps = topMotor.getOutputCurrent(); - inputs.topMotorAppliedVolts = topMotor.getBusVoltage() * topMotor.getAppliedOutput(); - inputs.topMotorVelocityRadPerSec = topMotorEncoder.getVelocity(); + inputs.topLeftMotorCurrentAmps = topLeftMotor.getOutputCurrent(); + inputs.topLeftMotorAppliedVolts = + topLeftMotor.getBusVoltage() * topLeftMotor.getAppliedOutput(); + inputs.topLeftMotorVelocityRadPerSec = topLeftMotorEncoder.getVelocity(); + + inputs.topRightMotorCurrentAmps = topRightMotor.getOutputCurrent(); + inputs.topRightMotorAppliedVolts = + topRightMotor.getBusVoltage() * topRightMotor.getAppliedOutput(); + inputs.topRightMotorVelocityRadPerSec = topRightMotorEncoder.getVelocity(); + + inputs.bottomLeftMotorCurrentAmps = bottomLeftMotor.getOutputCurrent(); + inputs.bottomLeftMotorAppliedVolts = + bottomLeftMotor.getBusVoltage() * bottomLeftMotor.getAppliedOutput(); + inputs.bottomLeftMotorVelocityRadPerSec = bottomLeftMotorEncoder.getVelocity(); + + inputs.bottomRightMotorCurrentAmps = bottomRightMotor.getOutputCurrent(); + inputs.bottomRightMotorAppliedVolts = + bottomRightMotor.getBusVoltage() * bottomRightMotor.getAppliedOutput(); + inputs.bottomRightMotorVelocityRadPerSec = bottomRightMotorEncoder.getVelocity(); inputs.totalAmps = - inputs.middleMotorCurrentAmps + inputs.bottomMotorCurrentAmps + inputs.topMotorCurrentAmps; + inputs.topLeftMotorCurrentAmps + + inputs.topRightMotorCurrentAmps + + inputs.bottomLeftMotorCurrentAmps + + inputs.bottomRightMotorCurrentAmps; inputs.shooterWheelVelocityRadPerSec = - inputs.bottomMotorVelocityRadPerSec / SHOOTER_WHEEL_GEAR_RATIO; - inputs.shooterWheelPosition = bottomMotorEncoder.getPosition() / SHOOTER_WHEEL_GEAR_RATIO; - - inputs.setpointRPM = bottomMotor.getClosedLoopController().getSetpoint(); - inputs.atSetpoint = bottomMotor.getClosedLoopController().isAtSetpoint(); + inputs.topRightMotorVelocityRadPerSec / SHOOTER_WHEEL_GEAR_RATIO; + inputs.shooterWheelPosition = topRightMotorEncoder.getPosition() / SHOOTER_WHEEL_GEAR_RATIO; } @Override public void setVoltage(double volts) { - bottomMotor.setVoltage(volts); + topRightMotor.setVoltage(volts); + topLeftMotor.setVoltage(volts); + bottomRightMotor.setVoltage(volts); + bottomLeftMotor.setVoltage(volts); } @Override public void stop() { - bottomMotor.set(0); - } - - private double distanceToRPM(double distanceMeters) { - return distanceMeters * 2.79775342767 + 16.8379527141; + topLeftMotor.set(0); + topRightMotor.set(0); + bottomLeftMotor.set(0); + bottomRightMotor.set(0); } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java index 3c7351771..463e6ce91 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionConstants.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionConstants.java @@ -24,49 +24,37 @@ public class VisionConstants { private static Rotation3d RotationCorrection = new Rotation3d(0, 0, Math.PI / 2); // 90 degree roation around z-axis public static Transform3d robotToCamera0 = - new Transform3d( // front camera + new Transform3d( new Translation3d( - Units.inchesToMeters(8.779), - Units.inchesToMeters(10.445), - Units.inchesToMeters(27.152 + 1.75)) + Units.inchesToMeters(-8.931), + Units.inchesToMeters(12.349), + Units.inchesToMeters(15.2025)) .rotateBy(RotationCorrection), new Rotation3d( Units.degreesToRadians(0.0), - Units.degreesToRadians(-25.2), - Units.degreesToRadians(0.0))); + Units.degreesToRadians(0.0), + Units.degreesToRadians(-158.771))); public static Transform3d robotToCamera1 = - new Transform3d( // rear left camera + new Transform3d( new Translation3d( - Units.inchesToMeters(9.562), // x - Units.inchesToMeters(10.974), // y - Units.inchesToMeters(17.035 + 1.75)) // z + Units.inchesToMeters(8.931), // x + Units.inchesToMeters(12.349), // y + Units.inchesToMeters(15.2025)) // z .rotateBy(RotationCorrection), new Rotation3d( Units.degreesToRadians(0.0), - Units.degreesToRadians(-11.32), - Units.degreesToRadians(155.3))); + Units.degreesToRadians(0.0), + Units.degreesToRadians(158.771))); public static Transform3d robotToCamera2 = new Transform3d( // rear right camera new Translation3d( - Units.inchesToMeters(-9.562), - Units.inchesToMeters(10.974), - Units.inchesToMeters(17.035 + 1.75)) - .rotateBy(RotationCorrection), - new Rotation3d( - Units.degreesToRadians(0), - Units.degreesToRadians(-11.32), - Units.degreesToRadians(-155.3))); - - public static Transform3d robotToCamera3 = - new Transform3d( // left camera - new Translation3d( - Units.inchesToMeters(9.669), - Units.inchesToMeters(10.961), - Units.inchesToMeters(24.656 + 1.75)) + Units.inchesToMeters(11.425), + Units.inchesToMeters(-2.361), + Units.inchesToMeters(20.5805)) .rotateBy(RotationCorrection), new Rotation3d( - Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(75))); + Units.degreesToRadians(0), Units.degreesToRadians(-30), Units.degreesToRadians(0))); // Basic filtering thresholds public static double maxAmbiguity = 0.3; @@ -85,6 +73,5 @@ public class VisionConstants { 1.0, // Camera 0 1.0, // Camera 1 1.0, // CAMERA 2 - 1.0 // Camera 3 }; } diff --git a/src/main/java/frc/robot/util/ShooterLeadCompensator.java b/src/main/java/frc/robot/util/ShooterLeadCompensator.java index 2405d0979..ccaecb1dc 100644 --- a/src/main/java/frc/robot/util/ShooterLeadCompensator.java +++ b/src/main/java/frc/robot/util/ShooterLeadCompensator.java @@ -1,8 +1,12 @@ package frc.robot.util; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Seconds; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.units.measure.Time; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterConstants; @@ -49,8 +53,8 @@ public ShootWhileDrivingResult shootWhileDriving(Translation2d targetPosition) { */ for (int i = 0; i < 10; i++) { double distance = aimPoint.minus(shooterPos).getNorm(); - double t = shooter.getAirTimeSeconds(() -> distance); - aimPoint = targetPosition.plus(v.times(t)); + Time t = shooter.getAirTimeSeconds(Meters.of(distance)); + aimPoint = targetPosition.plus(v.times(t.in(Seconds))); } double finalDistance = aimPoint.minus(shooterPos).getNorm(); diff --git a/src/main/java/frc/robot/util/Zone.java b/src/main/java/frc/robot/util/Zone.java new file mode 100644 index 000000000..3862daccd --- /dev/null +++ b/src/main/java/frc/robot/util/Zone.java @@ -0,0 +1,95 @@ +package frc.robot.util; + +import edu.wpi.first.math.geometry.Pose2d; +import frc.lib.utils.AllianceFlipUtil; +import java.util.Objects; +import java.util.function.Supplier; + +public class Zone { + public record Tuple2d(double x, double y) {} + + private Tuple2d minCorner; + private Tuple2d maxCorner; + private final Supplier poseSupplier; + + public Zone(Supplier poseSupplier) { + this.poseSupplier = Objects.requireNonNull(poseSupplier, "poseSupplier"); + } + + public Zone(Supplier poseSupplier, Tuple2d cornerA, Tuple2d cornerB) { + this(poseSupplier); + initializeZone(cornerA, cornerB); + } + + public static Tuple2d corner(double x, double y) { + return new Tuple2d(x, y); + } + + public void initializeZone(Tuple2d cornerA, Tuple2d cornerB) { + Objects.requireNonNull(cornerA, "cornerA"); + Objects.requireNonNull(cornerB, "cornerB"); + + double minX = Math.min(cornerA.x(), cornerB.x()); + double maxX = Math.max(cornerA.x(), cornerB.x()); + double minY = Math.min(cornerA.y(), cornerB.y()); + double maxY = Math.max(cornerA.y(), cornerB.y()); + + this.minCorner = new Tuple2d(minX, minY); + this.maxCorner = new Tuple2d(maxX, maxY); + } + + public boolean isInitialized() { + return minCorner != null && maxCorner != null; + } + + public boolean isInZone() { + return contains(poseSupplier.get()); + } + + public boolean isInZoneForAlliance() { + return containsForAlliance(poseSupplier.get()); + } + + public boolean contains(Pose2d pose) { + Objects.requireNonNull(pose, "pose"); + return contains(pose.getX(), pose.getY()); + } + + public boolean containsForAlliance(Pose2d pose) { + Objects.requireNonNull(pose, "pose"); + return containsForAlliance(pose.getX(), pose.getY()); + } + + public boolean contains(double x, double y) { + if (!isInitialized()) { + return false; + } + + return x >= minCorner.x() && x <= maxCorner.x() && y >= minCorner.y() && y <= maxCorner.y(); + } + + public boolean containsForAlliance(double x, double y) { + if (!isInitialized()) { + return false; + } + + double minX = minCorner.x(); + double maxX = maxCorner.x(); + double minY = minCorner.y(); + double maxY = maxCorner.y(); + + if (AllianceFlipUtil.shouldFlip()) { + double flippedMinX = AllianceFlipUtil.applyX(minX); + double flippedMaxX = AllianceFlipUtil.applyX(maxX); + double flippedMinY = AllianceFlipUtil.applyY(minY); + double flippedMaxY = AllianceFlipUtil.applyY(maxY); + + minX = Math.min(flippedMinX, flippedMaxX); + maxX = Math.max(flippedMinX, flippedMaxX); + minY = Math.min(flippedMinY, flippedMaxY); + maxY = Math.max(flippedMinY, flippedMaxY); + } + + return x >= minX && x <= maxX && y >= minY && y <= maxY; + } +}