diff --git a/.OutlineViewer/outlineviewer.json b/.OutlineViewer/outlineviewer.json new file mode 100644 index 0000000..0967ef4 --- /dev/null +++ b/.OutlineViewer/outlineviewer.json @@ -0,0 +1 @@ +{} diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index eadec71..8ef6df4 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -2,5 +2,5 @@ "enableCppIntellisense": false, "currentLanguage": "java", "projectYear": "2026", - "teamNumber": 7762 -} + "teamNumber": 7415 +} \ No newline at end of file diff --git a/simgui-ds.json b/simgui-ds.json index 69b1a3c..c4b7efd 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -91,7 +91,8 @@ ], "robotJoysticks": [ { - "guid": "Keyboard0" + "guid": "78696e70757401000000000000000000", + "useGamepad": true } ] } diff --git a/src/main/deploy/pathplanner/autos/4414-973.auto b/src/main/deploy/pathplanner/autos/4414-973.auto new file mode 100644 index 0000000..fa1a9a0 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/4414-973.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "S4-Wait" + } + }, + { + "type": "path", + "data": { + "pathName": "Wait-Hub" + } + }, + { + "type": "path", + "data": { + "pathName": "MidHub-LowFuel" + } + }, + { + "type": "path", + "data": { + "pathName": "LowFuel-Hub" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 4.0 + } + }, + { + "type": "named", + "data": { + "name": "ShooterOff" + } + }, + { + "type": "path", + "data": { + "pathName": "Hub-Wall" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Depot.auto b/src/main/deploy/pathplanner/autos/Depot.auto new file mode 100644 index 0000000..71fa50e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Depot.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "S1-D1" + } + }, + { + "type": "path", + "data": { + "pathName": "D1-D2" + } + }, + { + "type": "path", + "data": { + "pathName": "D2-Hub" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "path", + "data": { + "pathName": "Move" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Gay Maybe Yay.auto b/src/main/deploy/pathplanner/autos/Gay Maybe Yay.auto new file mode 100644 index 0000000..1e6da8b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Gay Maybe Yay.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Gay Yay" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Low Mid NO OUTPOST.auto b/src/main/deploy/pathplanner/autos/Low Mid NO OUTPOST.auto new file mode 100644 index 0000000..440a1ea --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Low Mid NO OUTPOST.auto @@ -0,0 +1,12 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Low Mid.auto b/src/main/deploy/pathplanner/autos/Low Mid.auto new file mode 100644 index 0000000..9566419 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Low Mid.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "S4-F3" + } + }, + { + "type": "path", + "data": { + "pathName": "F3-ok" + } + }, + { + "type": "path", + "data": { + "pathName": "ok-Hub" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "named", + "data": { + "name": "ShooterOff" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Mid Depot.auto b/src/main/deploy/pathplanner/autos/Mid Depot.auto new file mode 100644 index 0000000..9586de1 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Mid Depot.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "S1-F1" + } + }, + { + "type": "path", + "data": { + "pathName": "F1-F2" + } + }, + { + "type": "path", + "data": { + "pathName": "F2-Hub" + } + }, + { + "type": "path", + "data": { + "pathName": "Hub-Depot" + } + }, + { + "type": "path", + "data": { + "pathName": "D1-D2" + } + }, + { + "type": "path", + "data": { + "pathName": "D2-Hub" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Mid.auto b/src/main/deploy/pathplanner/autos/Mid.auto new file mode 100644 index 0000000..84dc835 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Mid.auto @@ -0,0 +1,56 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "S1-F1" + } + }, + { + "type": "path", + "data": { + "pathName": "F1-F2" + } + }, + { + "type": "path", + "data": { + "pathName": "F2-Hub" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 5.0 + } + }, + { + "type": "named", + "data": { + "name": "ShooterOff" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Taxi Preload.auto b/src/main/deploy/pathplanner/autos/Taxi Preload.auto new file mode 100644 index 0000000..2e44f91 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Taxi Preload.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Taxi" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/D1-D2.path b/src/main/deploy/pathplanner/paths/D1-D2.path new file mode 100644 index 0000000..7e26ff5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/D1-D2.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.7659077380952382, + "y": 6.977909226190477 + }, + "prevControl": null, + "nextControl": { + "x": 0.7659077380952383, + "y": 5.977909226190477 + }, + "isLocked": false, + "linkedName": "D1" + }, + { + "anchor": { + "x": 0.7659077380952382, + "y": 5.0920982142857145 + }, + "prevControl": { + "x": 0.7659077380952382, + "y": 5.905115543748579 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "D2" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 45.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": 45.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/D2-Hub.path b/src/main/deploy/pathplanner/paths/D2-Hub.path new file mode 100644 index 0000000..c595d8a --- /dev/null +++ b/src/main/deploy/pathplanner/paths/D2-Hub.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.7659077380952382, + "y": 5.0920982142857145 + }, + "prevControl": null, + "nextControl": { + "x": 1.4410197882863613, + "y": 5.309391914374445 + }, + "isLocked": false, + "linkedName": "D2" + }, + { + "anchor": { + "x": 2.628411712511091, + "y": 4.819671694764862 + }, + "prevControl": { + "x": 1.8880035492457847, + "y": 5.342786157941436 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "hubok" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "IntakeOff", + "waypointRelativePos": 0.21226415094339626, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeOff" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 45.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/D2-O.path b/src/main/deploy/pathplanner/paths/D2-O.path new file mode 100644 index 0000000..d1715a5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/D2-O.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.7659077380952382, + "y": 5.0920982142857145 + }, + "prevControl": null, + "nextControl": { + "x": 3.613956789173673, + "y": 5.05990655501331 + }, + "isLocked": false, + "linkedName": "D2" + }, + { + "anchor": { + "x": 0.4071872227151724, + "y": 0.6267080745341604 + }, + "prevControl": { + "x": 3.367907081247016, + "y": 0.9808163265306109 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "O" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 45.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F1-F2.path b/src/main/deploy/pathplanner/paths/F1-F2.path new file mode 100644 index 0000000..eca676b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F1-F2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.109041703637978, + "y": 6.9909598214285715 + }, + "prevControl": null, + "nextControl": { + "x": 8.109041703637978, + "y": 6.1440530006096745 + }, + "isLocked": false, + "linkedName": "F1" + }, + { + "anchor": { + "x": 8.109041703637978, + "y": 4.4080403225806455 + }, + "prevControl": { + "x": 8.109041703637978, + "y": 5.29056308056252 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "F2" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 59.99999999999999 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 59.99999999999999 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F2-Hub.path b/src/main/deploy/pathplanner/paths/F2-Hub.path new file mode 100644 index 0000000..5a4155d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F2-Hub.path @@ -0,0 +1,87 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.109041703637978, + "y": 4.4080403225806455 + }, + "prevControl": null, + "nextControl": { + "x": 5.373451612903226, + "y": 4.1812903225806455 + }, + "isLocked": false, + "linkedName": "F2" + }, + { + "anchor": { + "x": 5.699032738100059, + "y": 5.705476190473729 + }, + "prevControl": { + "x": 6.762475509200177, + "y": 5.501199048002521 + }, + "nextControl": { + "x": 4.54405505952863, + "y": 5.927336309521348 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.397232142857143, + "y": 5.02031994047619 + }, + "prevControl": { + "x": 3.74796875, + "y": 5.9012351190476195 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "ScoringDepot" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "okkkkk", + "waypointRelativePos": 0.5059055118110236, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeOff" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.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": 59.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F3-ok.path b/src/main/deploy/pathplanner/paths/F3-ok.path new file mode 100644 index 0000000..47d9684 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F3-ok.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.992048387096775, + "y": 0.904387096774194 + }, + "prevControl": null, + "nextControl": { + "x": 7.992048387096775, + "y": 2.65640839631369 + }, + "isLocked": false, + "linkedName": "F3" + }, + { + "anchor": { + "x": 8.021306451612903, + "y": 3.6985322580645166 + }, + "prevControl": { + "x": 8.021306451612903, + "y": 2.754988028682996 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "okyay" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -59.99999999999999 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -59.99999999999999 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Fart-Outpost.path b/src/main/deploy/pathplanner/paths/Fart-Outpost.path new file mode 100644 index 0000000..3d513d9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Fart-Outpost.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.754854838709678, + "y": 2.806161290322582 + }, + "prevControl": null, + "nextControl": { + "x": 1.196577380952381, + "y": 2.2927455357142854 + }, + "isLocked": false, + "linkedName": "fart" + }, + { + "anchor": { + "x": 0.5897247023809524, + "y": 0.6418452380952386 + }, + "prevControl": { + "x": 2.1166443452380976, + "y": 0.5896428571428578 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Outpost" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 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/Gay Yay.path b/src/main/deploy/pathplanner/paths/Gay Yay.path new file mode 100644 index 0000000..dd26b41 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Gay Yay.path @@ -0,0 +1,162 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.617462797619048, + "y": 2.5733333333333337 + }, + "prevControl": null, + "nextControl": { + "x": 5.790386904761905, + "y": 2.8473958333333336 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.886636904761905, + "y": 1.7772470238095242 + }, + "prevControl": { + "x": 5.886636904761905, + "y": 1.7772470238095244 + }, + "nextControl": { + "x": 7.886636904761905, + "y": 1.7772470238095242 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.8850074404761905, + "y": 1.7772470238095242 + }, + "prevControl": { + "x": 8.40067005470561, + "y": 1.7626182971647182 + }, + "nextControl": { + "x": 6.964940476190476, + "y": 1.8033482142857142 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.579947916666666, + "y": 1.7772470238095242 + }, + "prevControl": { + "x": 7.1812827764407565, + "y": 1.7951973181311385 + }, + "nextControl": { + "x": 6.142752976190476, + "y": 1.764196428571429 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.650096726190476, + "y": 2.5733333333333337 + }, + "prevControl": { + "x": 6.136227678571429, + "y": 2.605959821428572 + }, + "nextControl": { + "x": 8.262420121002597, + "y": 2.560136708445141 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.0693377976190477, + "y": 2.9648511904761903 + }, + "prevControl": { + "x": 4.635409226190475, + "y": 2.0643601190476186 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.8739352640545147, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.9212598425196845, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 0.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 3.553149606299209, + "maxWaypointRelativePos": 4.202755905511809, + "constraints": { + "maxVelocity": 0.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.4990157480314956, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Intake" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Hub-Depot.path b/src/main/deploy/pathplanner/paths/Hub-Depot.path new file mode 100644 index 0000000..9e157d2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Hub-Depot.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.397232142857143, + "y": 5.02031994047619 + }, + "prevControl": null, + "nextControl": { + "x": 1.7189662173968498, + "y": 5.27996100961303 + }, + "isLocked": false, + "linkedName": "ScoringDepot" + }, + { + "anchor": { + "x": 0.7659077380952382, + "y": 6.977909226190477 + }, + "prevControl": { + "x": 1.4104430147998999, + "y": 7.378144841563533 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "D1" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Intake" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 45.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/Hub-Wall.path b/src/main/deploy/pathplanner/paths/Hub-Wall.path new file mode 100644 index 0000000..10ce8e8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Hub-Wall.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.7300223214285713, + "y": 2.782142857142858 + }, + "prevControl": null, + "nextControl": { + "x": 1.3466592261904762, + "y": 2.051309523809524 + }, + "isLocked": false, + "linkedName": "HubThing" + }, + { + "anchor": { + "x": 0.4331175595238095, + "y": 2.475453869047619 + }, + "prevControl": { + "x": 1.6925000000000026, + "y": 2.384099702380952 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 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/LowFuel-Hub.path b/src/main/deploy/pathplanner/paths/LowFuel-Hub.path new file mode 100644 index 0000000..89dc935 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LowFuel-Hub.path @@ -0,0 +1,71 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.96983630952381, + "y": 1.3792038690476192 + }, + "prevControl": null, + "nextControl": { + "x": 6.188430059523809, + "y": 2.6646874999999994 + }, + "isLocked": false, + "linkedName": "LowFuel" + }, + { + "anchor": { + "x": 2.7300223214285713, + "y": 2.782142857142858 + }, + "prevControl": { + "x": 4.615833333333331, + "y": 2.2274925595238106 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HubThing" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2810902896081766, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "IntakeOff", + "waypointRelativePos": 0.2864173228346455, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeOff" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.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": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/MidHub-LowFuel.path b/src/main/deploy/pathplanner/paths/MidHub-LowFuel.path new file mode 100644 index 0000000..a29be29 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/MidHub-LowFuel.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.849114583333333, + "y": 3.9762723214285716 + }, + "prevControl": null, + "nextControl": { + "x": 6.553846726190476, + "y": 5.0529464285714285 + }, + "isLocked": false, + "linkedName": "MidHub" + }, + { + "anchor": { + "x": 7.96983630952381, + "y": 1.3792038690476192 + }, + "prevControl": { + "x": 9.203117559523811, + "y": 3.5912797619047625 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "LowFuel" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.549403747870527, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 150.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Move.path b/src/main/deploy/pathplanner/paths/Move.path new file mode 100644 index 0000000..ac748f5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Move.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.628411712511091, + "y": 4.819671694764862 + }, + "prevControl": null, + "nextControl": { + "x": 2.207998511904762, + "y": 6.2079241071428575 + }, + "isLocked": false, + "linkedName": "hubok" + }, + { + "anchor": { + "x": 2.0122395833333333, + "y": 6.840877976190477 + }, + "prevControl": { + "x": 2.207998511904762, + "y": 6.1296205357142854 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 0000000..8397ce6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.526108630952381, + "y": 4.021949404761905 + }, + "prevControl": null, + "nextControl": { + "x": 2.0057142857142853, + "y": 5.13125 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.661510416666666, + "y": 5.640223214285714 + }, + "prevControl": { + "x": 3.6615104166666663, + "y": 5.640223214285714 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 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/Outpost-Hub.path b/src/main/deploy/pathplanner/paths/Outpost-Hub.path new file mode 100644 index 0000000..9de9810 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Outpost-Hub.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.5897247023809524, + "y": 0.6418452380952386 + }, + "prevControl": null, + "nextControl": { + "x": 1.5897247023809524, + "y": 0.6418452380952386 + }, + "isLocked": false, + "linkedName": "Outpost" + }, + { + "anchor": { + "x": 2.5668898809523806, + "y": 3.3237425595238097 + }, + "prevControl": { + "x": 1.5668898809523806, + "y": 3.3237425595238097 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 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/S1-D1.path b/src/main/deploy/pathplanner/paths/S1-D1.path new file mode 100644 index 0000000..5f1f2af --- /dev/null +++ b/src/main/deploy/pathplanner/paths/S1-D1.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.586113575865128, + "y": 7.5479148181011535 + }, + "prevControl": null, + "nextControl": { + "x": 1.952386867787004, + "y": 7.145519077193867 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.7659077380952382, + "y": 6.977909226190477 + }, + "prevControl": { + "x": 0.8696178361975893, + "y": 7.625141824285953 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "D1" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "ok", + "waypointRelativePos": 0.3596698113207547, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Intake" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3, + "maxAcceleration": 3, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 45.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/S1-F1.path b/src/main/deploy/pathplanner/paths/S1-F1.path new file mode 100644 index 0000000..86a831b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/S1-F1.path @@ -0,0 +1,71 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5539219165927243, + "y": 5.56812777284827 + }, + "prevControl": null, + "nextControl": { + "x": 6.040727595386525, + "y": 4.626521739136872 + }, + "isLocked": false, + "linkedName": "S2" + }, + { + "anchor": { + "x": 8.109041703637978, + "y": 6.9909598214285715 + }, + "prevControl": { + "x": 6.958189884650058, + "y": 8.109619981151067 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "F1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4612326043737585, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "yay", + "waypointRelativePos": 0.4675196850393696, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Intake" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 59.99999999999999 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/S4-F3.path b/src/main/deploy/pathplanner/paths/S4-F3.path new file mode 100644 index 0000000..cd62068 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/S4-F3.path @@ -0,0 +1,71 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6325967741935488, + "y": 2.557467741935485 + }, + "prevControl": null, + "nextControl": { + "x": 6.6608064516129035, + "y": 2.9597661290322583 + }, + "isLocked": false, + "linkedName": "S4" + }, + { + "anchor": { + "x": 7.992048387096775, + "y": 0.904387096774194 + }, + "prevControl": { + "x": 7.311798387096775, + "y": -0.06844354838709554 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "F3" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.3644289450741062, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "spinnnynnyny", + "waypointRelativePos": 0.322097378277154, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Intake" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -59.99999999999999 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/S4-Wait.path b/src/main/deploy/pathplanner/paths/S4-Wait.path new file mode 100644 index 0000000..817a027 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/S4-Wait.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6325967741935488, + "y": 2.557467741935485 + }, + "prevControl": null, + "nextControl": { + "x": 4.632596774193549, + "y": 2.5574677419354845 + }, + "isLocked": false, + "linkedName": "S4" + }, + { + "anchor": { + "x": 5.679456845238096, + "y": 2.557467741935485 + }, + "prevControl": { + "x": 4.679456845238096, + "y": 2.557467741935485 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Wait" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "okkkkk", + "waypointRelativePos": 1.0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Intake" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Taxi.path b/src/main/deploy/pathplanner/paths/Taxi.path new file mode 100644 index 0000000..ad9ee46 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Taxi.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.604412202380953, + "y": 4.015424107142858 + }, + "prevControl": null, + "nextControl": { + "x": 2.6213636091500776, + "y": 4.015424107142858 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.397232142857143, + "y": 4.015424107142858 + }, + "prevControl": { + "x": 3.3972321428571433, + "y": 4.015424107142858 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 45.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/Wait-Hub.path b/src/main/deploy/pathplanner/paths/Wait-Hub.path new file mode 100644 index 0000000..433d4e3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Wait-Hub.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.679456845238096, + "y": 2.557467741935485 + }, + "prevControl": null, + "nextControl": { + "x": 5.705558035714286, + "y": 3.0170535714285722 + }, + "isLocked": false, + "linkedName": "Wait" + }, + { + "anchor": { + "x": 5.849114583333333, + "y": 3.9762723214285716 + }, + "prevControl": { + "x": 5.914367559523809, + "y": 3.4542485119047615 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "MidHub" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 150.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ok-Hub.path b/src/main/deploy/pathplanner/paths/ok-Hub.path new file mode 100644 index 0000000..f54edfe --- /dev/null +++ b/src/main/deploy/pathplanner/paths/ok-Hub.path @@ -0,0 +1,71 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.021306451612903, + "y": 3.6985322580645166 + }, + "prevControl": null, + "nextControl": { + "x": 4.941895161290323, + "y": 5.468645161290323 + }, + "isLocked": false, + "linkedName": "okyay" + }, + { + "anchor": { + "x": 2.754854838709678, + "y": 2.806161290322582 + }, + "prevControl": { + "x": 7.889645161290307, + "y": 1.0433629032258083 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "fart" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.08369659982563099, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "asdasd", + "waypointRelativePos": 0.6882022471910065, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeOff" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.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": -59.99999999999999 + }, + "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 4a0d130..dac61a8 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,6 +1,6 @@ { - "robotWidth": 0.762, - "robotLength": 0.762, + "robotWidth": 1.002, + "robotLength": 0.799, "holonomicMode": true, "pathFolders": [], "autoFolders": [], @@ -9,23 +9,23 @@ "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 75.0, - "robotMOI": 6.883, + "robotMass": 68.0, + "robotMOI": 6.35, "robotTrackwidth": 0.5588, - "driveWheelRadius": 0.05504, - "driveGearing": 7.363636, - "maxDriveSpeed": 4.7, + "driveWheelRadius": 0.0508, + "driveGearing": 6.03, + "maxDriveSpeed": 5.1206, "driveMotorType": "krakenX60FOC", "driveCurrentLimit": 120.0, - "wheelCOF": 1.7, - "flModuleX": 0.267, - "flModuleY": 0.2667, - "frModuleX": 0.267, - "frModuleY": -0.267, - "blModuleX": -0.267, - "blModuleY": 0.267, - "brModuleX": -0.267, - "brModuleY": -0.267, + "wheelCOF": 2.255, + "flModuleX": 0.232, + "flModuleY": 0.327, + "frModuleX": 0.232, + "frModuleY": -0.327, + "blModuleX": -0.232, + "blModuleY": 0.327, + "brModuleX": -0.232, + "brModuleY": -0.327, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, "robotFeatures": [] diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d4caa5c..871a4a4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,70 +4,73 @@ package frc.robot; -import static edu.wpi.first.units.Units.Feet; -import static edu.wpi.first.units.Units.Inches; -import static edu.wpi.first.units.Units.Meter; -import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.*; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.units.AngleUnit; +import edu.wpi.first.units.DistanceUnit; import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; /** Add your docs here. */ public class Constants { + public static abstract class ShooterConstants { - public static final int ShooterLeaderID = 20; //all IDs should be changed (I think theyre 40-43 and 0?) + public static final int ShooterLeaderID = 20; public static final int ShooterFollowerID = 21; - public static final int ShooterFollowerReversedID = 22; - public static final int ShooterFollowerReversed2ID = 23; - public static final AngularVelocity FastShot = RPM.of(3000); //HESHEL CHANGE ME FAST - public static final AngularVelocity SlowShot = RPM.of(1500); //HESKEL CHANGE ME SLOW - public static final AngularVelocity SetRPMHardStop = RPM.of(3200); - public static double ShootingDistance = Feet.of(6).in(Meter); //6ft starting - public static int RPMTolarance = 50; + public static final int ShooterFollowerReversedID = 22; // + public static final int ShooterFollowerReversed2ID = 23; // + public static final double FastShot = 100; // IN RPS + public static final double SlowShot = 20; //HESKEL CHANGE ME SLOW + public static final double RPSHardStop = 100.0; + public static final double RPSTolarance = 1; + public static final double kS = 0.3; //volt to overcome static friction feedforawd TUNED ALR + public static final double kV = 0.13; //volts per rps 0.125 } public static abstract class IntakeConstants { - public static final int IntakeMotorID = 31; - public static final int IntakeFollowerReversedMotorID = 30; - public static final double FastIntake = 1; - public static final double SlowIntake = 0.5; + public static final int IntakeMotorID = 30; + public static final int IntakeFollowerReversedMotorID = 31; + public static final double FastIntake = 0.87; + public static final double SlowIntake = 0.67; public static final double SlowReverse = -0.3; - public static final double FastReverse = -1; + public static final double FastReverse = -0.87; + + public static final int IntakeSlideMotorID = 35; + public static final double IntakeSlideOutSetPoint = 25; //In ROTATIONS + public static final double IntakeSlideMiddleSetPoint = 10; + public static final double IntakeSlideInSetPoint = 0; + public static final Angle SafeOutPosition = Degree.of(9.3*360); //in rotations + + public static final double IntakeSlideOutHardStop = 25.5; //rotations; Used for re-zero out + + public static final double PositionTolerance = 0.3; //in degreee + } public static abstract class HopperConstants { public static final int HopperMotorID = 32; // - public static final double FastRoll = -0.3; - public static final double SlowRoll = -0.2; - public static final double FastOutRoll = 0.8; - public static final double SlowOutRoll = 0.4; + public static final double FastRoll = 0.8; + public static final double SlowRoll = 0.3; //0.2 + public static final double FastOutRoll = -0.4; + public static final double SlowOutRoll = -0.2; } public static abstract class IndexerConstants { - public static final int IndexerLowMotorID = 33; // - public static final int IndexerHighMotorID = 34; //NOT RIGHTFAKE FIX NOW - public static final double FastRoll = 1; - public static final double SlowRoll = 0.6; - public static final double FastOutRoll = -0.5; - public static final double SlowOutRoll = -0.3; - - } - - public static abstract class ClimberConstants { - public static final int ClimberMotorID = 32; //change me ts NOT a real motor - public static final double ClimbSpeed = 0.7; - public static final double DescendSpeed = -0.5; - - public static final Distance HighSetPoint = Inches.of(63.0); - public static final Distance MiddleSetPoint = Inches.of(45.0); - public static final Distance LowSetPoint = Inches.of(27.0); + public static final int HighIndexerMotorID = 34; // + public static final int LowIndexerMotorID = 33; // + public static final double FastRoll = -0.87; + public static final double SlowRoll = -0.6; + public static final double FastOutRoll = 0.87; + public static final double SlowOutRoll = 0.3; - public static final Distance LowSetPointDown = Inches.of(23.0); } public static abstract class FieldConstants{ @@ -84,19 +87,42 @@ public static abstract class FieldConstants{ Units.Inches.of(72.0), new Rotation3d() ); + public static final Pose2d blueTargetHighPose = new Pose2d( + Units.Inches.of(20), + Units.Inches.of(265), + new Rotation2d() + ); + + public static final Pose2d blueTargetLowPose = new Pose2d( + Units.Inches.of(20), + Units.Inches.of(50), + new Rotation2d() + ); + + public static final Pose2d redTargetHighPose = new Pose2d( + Units.Inches.of(630), + Units.Inches.of(265), + new Rotation2d() + ); + + public static final Pose2d redTargetLowPose = new Pose2d( + Units.Inches.of(630), + Units.Inches.of(50), + new Rotation2d() + ); } public static abstract class DriveConstants{ - public static final double xyP = 10; + public static final double xyP = 1; public static final double xyI = 0; public static final double xyD = 0; - public static final double rotP = 3; + public static final double rotP = 0.85; //0.05 public static final double rotI = 0; - public static final double rotD = 0.175; + public static final double rotD = 0.05; //0.05 public static final double TranslationDeadband = 0.1; public static final double RotationDeadband = 0.1; - public static final double RotationalToleranceDegrees = 5.0; + public static final double RotationalToleranceDegrees = 3.0; } } \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 190160c..4a5014c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -7,14 +7,13 @@ import com.ctre.phoenix6.HootAutoReplay; import edu.wpi.first.cameraserver.CameraServer; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.simulation.RoboRioSim; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj.simulation.BatterySim; -import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import frc.robot.utils.HubShiftUtil; +import frc.robot.utils.HubShiftUtil.ShiftInfo; public class Robot extends TimedRobot { @@ -27,8 +26,6 @@ public class Robot extends TimedRobot { .withTimestampReplay() .withJoystickReplay(); - private final boolean kUseLimelight = false; - public Robot() { m_robotContainer = new RobotContainer(); } @@ -38,27 +35,23 @@ public void robotPeriodic() { m_timeAndJoystickReplay.update(); CommandScheduler.getInstance().run(); - /* - * This example of adding Limelight is very simple and may not be sufficient for on-field use. - * Users typically need to provide a standard deviation that scales with the distance to target - * and changes with number of tags available. - * - * This example is sufficient to show that vision integration is possible, though exact implementation - * of how to use vision should be tuned per-robot and to the team's specification. - */ - - - if (kUseLimelight) { - var driveState = m_robotContainer.drivetrain.getState(); - double headingDeg = driveState.Pose.getRotation().getDegrees(); - double omegaRps = Units.radiansToRotations(driveState.Speeds.omegaRadiansPerSecond); - - LimelightHelpers.SetRobotOrientation("limelight", headingDeg, 0, 0, 0, 0, 0); - var llMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight"); - if (llMeasurement != null && llMeasurement.tagCount > 0 && Math.abs(omegaRps) < 2.0) { - m_robotContainer.drivetrain.addVisionMeasurement(llMeasurement.pose, llMeasurement.timestampSeconds); - } - } + ShiftInfo official = HubShiftUtil.getOfficialShiftInfo(); + ShiftInfo shifted = HubShiftUtil.getShiftedShiftInfo(); + + // Official shift info + SmartDashboard.putString("Shift/Official/CurrentShift", official.currentShift().toString()); + SmartDashboard.putNumber("Shift/Official/RemainingTime", Math.round(official.remainingTime()*10)/10.0); + SmartDashboard.putNumber("Shift/Official/ElapsedTime", Math.round(official.elapsedTime()*10)/10.0); + SmartDashboard.putNumber("Shift/Official/MatchTime", HubShiftUtil.getMatchTime()); + SmartDashboard.putBoolean("Shift/Official/Active", official.active()); + + // Shifted shift info + SmartDashboard.putString("Shift/Shifted/CurrentShift", shifted.currentShift().toString()); + SmartDashboard.putNumber("Shift/Shifted/ElapsedTime", shifted.elapsedTime()); + SmartDashboard.putNumber("Shift/Shifted/RemainingTime", shifted.remainingTime()); + SmartDashboard.putBoolean("Shift/Shifted/Active", shifted.active()); + + } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1165b83..1817c09 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,51 +4,47 @@ package frc.robot; -import static edu.wpi.first.units.Units.*; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; -import java.util.Set; - -import org.w3c.dom.traversal.TreeWalker; +import java.util.Optional; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.swerve.SwerveRequest; - import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.FollowPathCommand; import com.pathplanner.lib.path.PathConstraints; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; +import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; - import frc.robot.generated.TunerConstants; import frc.robot.handlers.DriveHandler; -import frc.robot.handlers.IntakeHandler; import frc.robot.handlers.ShooterHandler; -import frc.robot.handlers.ClimbHandler; import frc.robot.handlers.Superstructure; import frc.robot.handlers.Superstructure.SuperstructureState; -import frc.robot.subsystems.ClimbSubsystem; +import frc.robot.subsystems.BangBangShooterSubsystem; import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.Elevator; import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.IndexerHighSubsystem; import frc.robot.subsystems.IndexerLowSubsystem; import frc.robot.subsystems.IntakeSubsystem; -import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.Vision; -import pabeles.concurrency.IntOperatorTask.Max; +import frc.robot.utils.HubShiftUtil; +import frc.robot.utils.RumbleUtils; public class RobotContainer { @@ -67,22 +63,49 @@ public class RobotContainer { private final Telemetry logger = new Telemetry(MaxSpeed); private final CommandXboxController joystick = new CommandXboxController(0); + private final CommandXboxController opJoystick = new CommandXboxController(1); public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); public final Vision vision = new Vision(drivetrain); - public final ShooterSubsystem shooter = new ShooterSubsystem(); + public final BangBangShooterSubsystem shooter = new BangBangShooterSubsystem(); public final IntakeSubsystem intake = new IntakeSubsystem(); public final HopperSubsystem hopper = new HopperSubsystem(); public final IndexerHighSubsystem HighIndexer = new IndexerHighSubsystem(); public final IndexerLowSubsystem LowIndexer = new IndexerLowSubsystem(); - public final ClimbSubsystem climb = new ClimbSubsystem(); - + public final Elevator IntakeSlide = new Elevator(); public final Superstructure superstructure = Superstructure.getInstance(); - //private TalonFX intakeMotor = new TalonFX(Constants.IntakeConstants.IntakeMotorID); + Trigger fiveSecWarning = new Trigger(() -> { + var info = HubShiftUtil.getOfficialShiftInfo(); + return info.remainingTime() <= 5.0;}); + + Trigger threeSecWarning = new Trigger(() -> { + var info = HubShiftUtil.getOfficialShiftInfo(); + return info.remainingTime() <= 3.0;}); + + Trigger twoSecWarning = new Trigger(() -> { + var info = HubShiftUtil.getOfficialShiftInfo(); + return info.remainingTime() <= 2.0;}); + + Trigger oneSecWarning = new Trigger(() -> { + var info = HubShiftUtil.getOfficialShiftInfo(); + return info.remainingTime() <= 1.0;}); + + Trigger noButtonsHeld = new Trigger(() -> + !joystick.a().getAsBoolean() && + !joystick.b().getAsBoolean() && + !joystick.y().getAsBoolean() && + !joystick.x().getAsBoolean() && + !joystick.rightTrigger().getAsBoolean() && + !joystick.leftTrigger().getAsBoolean() && + !joystick.rightBumper().getAsBoolean() && + !joystick.leftTrigger().getAsBoolean() && + !joystick.rightBumper().getAsBoolean() + ); + /* Path follower */ private final SendableChooser autoChooser; @@ -90,8 +113,9 @@ public class RobotContainer { public RobotContainer() { DriveHandler.getInstance().initialize(drivetrain, joystick, drive, MaxSpeed, MaxAngularRate); ShooterHandler.getInstance().initialize(drivetrain, shooter); - Superstructure.getInstance().initialize(shooter, drivetrain, climb); - ShooterSubsystem.getInstance().initialize(drivetrain); + Superstructure.getInstance().initialize(shooter, drivetrain); + + configurePathPlanner(); autoChooser = AutoBuilder.buildAutoChooser("Tests"); SmartDashboard.putData("Auto Mode", autoChooser); @@ -101,6 +125,35 @@ public RobotContainer() { // Warmup PathPlanner to avoid Java pauses FollowPathCommand.warmupCommand().schedule(); } + + private void configurePathPlanner() { + NamedCommands.registerCommand("Intake", + new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.INTAKE))); + + NamedCommands.registerCommand("IntakeOff", + new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.IDLE))); + + NamedCommands.registerCommand("Shoot", + new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUP))); + + NamedCommands.registerCommand("ShooterOff", + new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.IDLE))); + + NamedCommands.registerCommand("ShootSafe", + new SequentialCommandGroup( + new InstantCommand( () -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUP)), + new WaitCommand(2.0), + new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.STATIONARYSHOT)) + )); + + NamedCommands.registerCommand("ShootTest", + new SequentialCommandGroup( + new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUP)).withTimeout(0.1), + new WaitCommand(3.0), + new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.IDLE)) + )); + } + private void configureBindings() { // Note that X is defined as forward according to WPILib convention, // and Y is defined as to the left according to WPILib convention. @@ -134,70 +187,132 @@ private void configureBindings() { joystick.rightStick().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric)); drivetrain.registerTelemetry(logger::telemeterize); - PathConstraints constraints = new PathConstraints(3, 4, - Degrees.of(540).in(Radians), Degrees.of(720).in(Radians)); - joystick.start().whileTrue(drivetrain.getSnakeDriveCommand(drive, drivetrain, joystick, MaxSpeed, MaxAngularRate)); - - //joystick.rightBumper().whileTrue(drivetrain.shootOnTheMoveIterative(joystick, MaxSpeed, MaxAngularRate, "no")); //Shoot while moving - joystick.rightBumper().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUP))); - joystick.rightBumper().onFalse(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.IDLE))); + + // Create the constraints to use while pathfinding + PathConstraints constraints = new PathConstraints( + 3.0, 4.0, + Units.degreesToRadians(540), Units.degreesToRadians(720)); + + //ROTATE 90 degreese + /* + joystick.leftBumper().onTrue(Commands.runOnce(() -> { + Rotation2d targetRotation = drivetrain.getPose().getRotation().plus(Rotation2d.fromDegrees(90)); + + // Use your existing rotation controller + drivetrain.applyRequest(() -> { + double rotationalRate = CommandSwerveDrivetrain.rotationController.calculate( + drivetrain.getPose().getRotation().getRadians(), + targetRotation.getRadians() + ); + return new SwerveRequest.FieldCentric() + .withVelocityX(0) + .withVelocityY(0) + .withRotationalRate(rotationalRate * 6); // Max angular rate + }).withTimeout(2.0).schedule(); + })); + */ + + /* Main driver Controller: + * RT - Hold to spin up (and shoot hopefully) - relase to idle + * RB - Shoot (dont use unless robot broken) + * LT - Hold to Intake - release to idle + * B - Spin up Fast (use if broken) + * Y - FAST SHOT (use if broken) + * A - Hold to bump assist - release to idle + * X - Robot off + * Left Stick in - Reverse shi + */ + joystick.rightTrigger().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUP))); + + //joystick.rightBumper().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.STATIONARYSHOT))); + joystick.rightBumper().onTrue(new SequentialCommandGroup( + Commands.runOnce(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUPFAST)), + Commands.waitSeconds(0.5), + Commands.runOnce(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.FASTSHOT)))); //joystick.leftBumper().onTrue(new InstantCommand(() -:drivetrain.setDefaultCommand(drivetrain.headingLocktoHub(joystick, MaxSpeed, MaxAngularRate, "no"))); //shoot while stationary - joystick.b().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUP))); - + joystick.b().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUPFAST))); + joystick.y().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.FASTSHOT))); + + joystick.a().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState. BUMP))); + joystick.x().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.OFF))); - joystick.leftTrigger().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.INTAKE))); - joystick.leftTrigger().onFalse(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.OFF))); + joystick.leftTrigger().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.INTAKESLOW))); + joystick.leftBumper().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.INTAKESLOWSLOW))); joystick.leftStick().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.REVERSE))); - joystick.a().onTrue(new InstantCommand(() -> superstructure.setDesiredState((Superstructure.SuperstructureState.TUNING)))); + noButtonsHeld.onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.IDLE))); + + + //joystick.a().onTrue(new InstantCommand(() -> superstructure.setDesiredState((Superstructure.SuperstructureState.TUNING)))); + //joystick.a().onFalse(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.OFF))); - joystick.rightStick().onTrue(new InstantCommand(() -> drivetrain.seedFieldCentric())); - joystick.leftBumper().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.CLIMBPREP))); - //FOR HESHEL - /* - joystick.leftTrigger().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.INTAKE))); - joystick.leftTrigger().onFalse(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.OFF))); - joystick.a().onTrue( - new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUPSLOW)) - .andThen(new WaitCommand(1.0)) - .andThen(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.SLOWSHOT)))); - joystick.y().onTrue( - new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUPFAST)) - .andThen(new WaitCommand(1.0)) - .andThen(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.FASTSHOT)))); - joystick.x().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.OFF))); - */ + joystick.rightStick().onTrue(new InstantCommand(() -> drivetrain.seedFieldCentric())); - /*BINDS: - Right bumper = auto angle - B = spinup - Y = Shoot with auto shooter speed - Left Trigger = hold for intake/ release for idle - Left stick in = reverse (unstuck shi) - A = Tuning mode (Dpad up or down to change speed by 250 RPM) - */ - + //joystick.povDown().onTrue(Commands.runOnce(() -> IntakeSlideHandler.getInstance().setDesiredState(IntakeSlideState.REZEROIN))); + //joystick.povUp().onTrue(Commands.runOnce(() -> IntakeSlideHandler.getInstance().setDesiredState(IntakeSlideState.REZEROOUT))); //in RPM - - joystick.povDown().onTrue(Commands.runOnce(() -> ShooterHandler.getInstance().adjustFastShot(-250))); //in RPM - joystick.povUp().onTrue(Commands.runOnce(() -> ShooterHandler.getInstance().adjustFastShot(250))); - - joystick.povLeft().whileTrue(drivetrain.shootOnTheMoveIterative(joystick, MaxSpeed, MaxAngularRate, "PovLeft")); //Shoot while moving - - joystick.povRight().whileTrue(drivetrain.shootOnTheMoveIterative(joystick, MaxSpeed, MaxAngularRate, "PovRight")); //Shoot while moving - + joystick.povRight().whileTrue(IntakeSlide.manualDrive(() -> 0.67)); // out + joystick.povLeft().whileTrue(IntakeSlide.manualDrive(() -> -0.67)); //in + + + //CONTROLLER 2 / debug controller + /* + * RT - Shoot + * B - re-zero intake IN + * Y - overide alliance winner (on a switch) + * DPAD - shift hub by 0.1 M in direction (up is away, down is closer) + * A - Reset Shifted Hub to where it should be + */ + + opJoystick.rightTrigger().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.STATIONARYSHOT))); + opJoystick.b().onTrue(new InstantCommand(() -> IntakeSlide.calibrateZeroIn())); + //AUTO WINNER OVERIDES + opJoystick.y().onTrue(Commands.runOnce(() -> { + var current = HubShiftUtil.getAllianceWinOverride(); + HubShiftUtil.setAllianceWinOverride(() -> Optional.of(current.orElse(true) == false)); + })); + + opJoystick.a().onTrue(Commands.runOnce(() -> drivetrain.resetHubOffset())); + opJoystick.a().onTrue(Commands.runOnce(() -> shooter.resetShooterMult())); + + // Change Shooter Power + opJoystick.povUp().onTrue(Commands.runOnce(() -> shooter.changeShooterMult(0.05))); + opJoystick.povDown().onTrue(Commands.runOnce(() -> shooter.changeShooterMult(-0.05))); + + // Adjust Y offset + opJoystick.povRight().onTrue(Commands.runOnce(() -> drivetrain.setHubOffset(0.0, 0.1))); + opJoystick.povLeft().onTrue(Commands.runOnce(() -> drivetrain.setHubOffset(0.0, -0.1))); + + opJoystick.x().onTrue(Commands.runOnce(() -> shooter.toggleShooterMult())); + + /* + opJoystick.a().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.TUNING))); + joystick.povUp().onTrue(Commands.runOnce(() -> ShooterHandler.getInstance().adjustFastShot(1))); + joystick.povDown().onTrue(Commands.runOnce(() -> ShooterHandler.getInstance().adjustFastShot(-1))); + */ + + + RobotModeTriggers.teleop().onTrue(Commands.runOnce(HubShiftUtil::initialize)); + RobotModeTriggers.autonomous().onTrue(Commands.runOnce(HubShiftUtil::initialize)); + + fiveSecWarning.onTrue(RumbleUtils.rumble(joystick, 0.5, 0.5)); + threeSecWarning.onTrue(RumbleUtils.rumble(joystick, 0.5, 0.25)); + twoSecWarning.onTrue(RumbleUtils.rumble(joystick, 0.5, 0.25)); + oneSecWarning.onTrue(RumbleUtils.rumble(joystick, 0.5, 1)); + + fiveSecWarning.onTrue(new InstantCommand(() -> SmartDashboard.putBoolean("isTsWorking", true))); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 59c543b..9600855 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -84,10 +84,10 @@ public class TunerConstants { private static final double kSteerGearRatio = 26.09090909090909; private static final Distance kWheelRadius = Inches.of(2); - private static final boolean kInvertLeftSide = true; - private static final boolean kInvertRightSide = false; + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; - private static final int kPigeonId = 21; + private static final int kPigeonId = 50; // These are only used for simulation private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); @@ -126,48 +126,48 @@ public class TunerConstants { // Front Left - private static final int kFrontLeftDriveMotorId = 1; - private static final int kFrontLeftSteerMotorId = 2; - private static final int kFrontLeftEncoderId = 22; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.373291015625); + private static final int kFrontLeftDriveMotorId = 4; + private static final int kFrontLeftSteerMotorId = 3; + private static final int kFrontLeftEncoderId = 19; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.247802734375); private static final boolean kFrontLeftSteerMotorInverted = false; private static final boolean kFrontLeftEncoderInverted = false; - private static final Distance kFrontLeftXPos = Inches.of(9.25); - private static final Distance kFrontLeftYPos = Inches.of(13); + private static final Distance kFrontLeftXPos = Inches.of(9.125); + private static final Distance kFrontLeftYPos = Inches.of(13.875); // Front Right - private static final int kFrontRightDriveMotorId = 5; - private static final int kFrontRightSteerMotorId = 6; - private static final int kFrontRightEncoderId = 21; - private static final Angle kFrontRightEncoderOffset = Rotations.of(0.0302734375); + private static final int kFrontRightDriveMotorId = 7; + private static final int kFrontRightSteerMotorId = 8; + private static final int kFrontRightEncoderId = 20; + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.035888671875); private static final boolean kFrontRightSteerMotorInverted = false; private static final boolean kFrontRightEncoderInverted = false; - private static final Distance kFrontRightXPos = Inches.of(9.25); - private static final Distance kFrontRightYPos = Inches.of(-13); + private static final Distance kFrontRightXPos = Inches.of(9.125); + private static final Distance kFrontRightYPos = Inches.of(-13.875); // Back Left - private static final int kBackLeftDriveMotorId = 7; - private static final int kBackLeftSteerMotorId = 8; - private static final int kBackLeftEncoderId = 20; - private static final Angle kBackLeftEncoderOffset = Rotations.of(0.03564453125); + private static final int kBackLeftDriveMotorId = 5; + private static final int kBackLeftSteerMotorId = 6; + private static final int kBackLeftEncoderId = 21; + private static final Angle kBackLeftEncoderOffset = Rotations.of(0.0361328125); private static final boolean kBackLeftSteerMotorInverted = false; private static final boolean kBackLeftEncoderInverted = false; - private static final Distance kBackLeftXPos = Inches.of(-9.25); - private static final Distance kBackLeftYPos = Inches.of(13); + private static final Distance kBackLeftXPos = Inches.of(-9.125); + private static final Distance kBackLeftYPos = Inches.of(13.875); // Back Right - private static final int kBackRightDriveMotorId = 4; - private static final int kBackRightSteerMotorId = 3; - private static final int kBackRightEncoderId = 19; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.248291015625); + private static final int kBackRightDriveMotorId = 1; + private static final int kBackRightSteerMotorId = 2; + private static final int kBackRightEncoderId = 22; + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.373046875); private static final boolean kBackRightSteerMotorInverted = false; private static final boolean kBackRightEncoderInverted = false; - private static final Distance kBackRightXPos = Inches.of(-9.25); - private static final Distance kBackRightYPos = Inches.of(-13); + private static final Distance kBackRightXPos = Inches.of(-9.125); + private static final Distance kBackRightYPos = Inches.of(-13.875); public static final SwerveModuleConstants FrontLeft = diff --git a/src/main/java/frc/robot/handlers/DriveHandler.java b/src/main/java/frc/robot/handlers/DriveHandler.java index 7990a1b..ac50b2d 100644 --- a/src/main/java/frc/robot/handlers/DriveHandler.java +++ b/src/main/java/frc/robot/handlers/DriveHandler.java @@ -6,27 +6,32 @@ import com.ctre.phoenix6.swerve.SwerveRequest; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rectangle2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants; import frc.robot.handlers.ShooterHandler.ShooterState; import frc.robot.handlers.StateSubsystem.State; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.HopperSubsystem; -import frc.robot.subsystems.ShooterSubsystem; public class DriveHandler extends SubsystemBase { public enum DriveState implements State { TELEOPDRIVE, + TELEOPDRIVESLOW, AUTOALLIGN, SHOOTONTHEMOVE, - PASSING, SNAKE, - XDRIVE + XDRIVE, + BUMP_LOCK } @@ -39,7 +44,7 @@ public enum DriveState implements State { - private DriveState desiredState = DriveState.AUTOALLIGN; + private DriveState desiredState = DriveState.TELEOPDRIVE; private DriveState currentState = DriveState.TELEOPDRIVE; private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); @@ -53,7 +58,21 @@ public static DriveHandler getInstance(){ return instance; } - public void initialize(CommandSwerveDrivetrain drivetrain, CommandXboxController joystick, SwerveRequest.FieldCentric drive, double maxSpeed, double maxAngularRate) { + Rectangle2d zone1 = new Rectangle2d(new Pose2d(1.0, 1.0, new Rotation2d()), 2.0, 2.0); + Rectangle2d zone2 = new Rectangle2d(new Pose2d(5.0, 3.0, new Rotation2d()), 1.5, 2.0); + Rectangle2d zone3 = new Rectangle2d(new Pose2d(10.0, 2.0, new Rotation2d()), 3.0, 1.0); + Rectangle2d zone4 = new Rectangle2d(new Pose2d(14.0, 5.0, new Rotation2d()), 2.0, 2.0); + + Trigger inAnyZone = new Trigger(() -> { + Translation2d robotPos = drivetrain.getState().Pose.getTranslation(); + return zone1.contains(robotPos) || + zone2.contains(robotPos) || + zone3.contains(robotPos) || + zone4.contains(robotPos); + }); + + + public void initialize(CommandSwerveDrivetrain drivetrain, CommandXboxController joystick, SwerveRequest.FieldCentric drive, double maxSpeed, double maxAngularRate) { this.drivetrain = drivetrain; this.joystick = joystick; this.drive = drive; @@ -61,7 +80,7 @@ public void initialize(CommandSwerveDrivetrain drivetrain, CommandXboxController this.maxAngularRate = maxAngularRate; update(); - } + } public void setDesiredState(DriveState state){ @@ -78,10 +97,6 @@ public void updateStates(){ } } - - - - public void handleStateTransition() { update(); } @@ -94,25 +109,26 @@ public void update() { } switch (desiredState) { case TELEOPDRIVE: - System.out.print("TELEOP"); drivetrain.setDefaultCommand(drivetrain.TeleopDrive(joystick, maxSpeed, maxAngularRate, drive, drivetrain)); break; + case TELEOPDRIVESLOW: + drivetrain.setDefaultCommand(drivetrain.TeleopDriveSLOW(joystick, maxSpeed, maxAngularRate, drive, drivetrain)); + break; case AUTOALLIGN: - drivetrain.setDefaultCommand(drivetrain.headingLocktoHub(joystick, maxSpeed, maxAngularRate, "no")); + drivetrain.setDefaultCommand(drivetrain.headingLocktoHub(joystick, maxSpeed, maxAngularRate)); break; case SHOOTONTHEMOVE: drivetrain.setDefaultCommand(drivetrain.shootOnTheMoveIterative(joystick, maxSpeed, maxAngularRate, "no")); break; - case PASSING: - drivetrain.setDefaultCommand(drivetrain.TeleopDrive(joystick, maxSpeed, maxAngularRate, drive, drivetrain)); //make code for - break; case SNAKE: - System.out.print("SNAKE"); drivetrain.setDefaultCommand(drivetrain.getSnakeDriveCommand(drive, drivetrain, joystick, maxSpeed, maxAngularRate)); break; case XDRIVE: drivetrain.setDefaultCommand(drivetrain.applyRequest(() -> brake)); break; + case BUMP_LOCK: + drivetrain.setDefaultCommand(drivetrain.bumpLockCommand(drive, drivetrain, joystick, maxSpeed, maxAngularRate)); + break; default: drivetrain.setDefaultCommand(drivetrain.TeleopDrive(joystick, maxSpeed, maxAngularRate, drive, drivetrain)); break; diff --git a/src/main/java/frc/robot/handlers/IndexerLowHandler.java b/src/main/java/frc/robot/handlers/IndexerLowHandler.java index e891308..30aeeff 100644 --- a/src/main/java/frc/robot/handlers/IndexerLowHandler.java +++ b/src/main/java/frc/robot/handlers/IndexerLowHandler.java @@ -11,6 +11,7 @@ import frc.robot.handlers.StateSubsystem.State; import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.IndexerHighSubsystem; +import frc.robot.subsystems.IndexerLowSubsystem; import frc.robot.subsystems.IntakeSubsystem; public class IndexerLowHandler extends SubsystemBase implements StateSubsystem { @@ -24,7 +25,7 @@ public enum IndexerLowState implements State { } private static IndexerLowHandler instance; - private final IndexerHighSubsystem index = new IndexerHighSubsystem(); + private final IndexerLowSubsystem index = new IndexerLowSubsystem(); private IndexerLowState desiredState = IndexerLowState.OFF; private IndexerLowState currentState = IndexerLowState.OFF; diff --git a/src/main/java/frc/robot/handlers/IntakeHandler.java b/src/main/java/frc/robot/handlers/IntakeHandler.java index 05d745f..4028bef 100644 --- a/src/main/java/frc/robot/handlers/IntakeHandler.java +++ b/src/main/java/frc/robot/handlers/IntakeHandler.java @@ -4,14 +4,19 @@ package frc.robot.handlers; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants; -import frc.robot.handlers.ShooterHandler.ShooterState; -import frc.robot.handlers.StateSubsystem.State; +import frc.robot.subsystems.Elevator; import frc.robot.subsystems.IntakeSubsystem; public class IntakeHandler extends SubsystemBase implements StateSubsystem { + public enum IntakeState implements State { FASTINTAKE, SLOWINTAKE, @@ -21,11 +26,16 @@ public enum IntakeState implements State { } private static IntakeHandler instance; - private IntakeSubsystem intake = new IntakeSubsystem(); + private Elevator lintake; + private IntakeSubsystem intake = new IntakeSubsystem(); private IntakeState desiredState = IntakeState.OFF; private IntakeState currentState = IntakeState.OFF; - /** Creates a new IntakeHandler. */ + public void initialize(Elevator lintake) { + this.lintake = lintake; + } + + private IntakeHandler() {} public static IntakeHandler getInstance(){ @@ -48,11 +58,11 @@ public void handleStateTransition() { update(); } - @Override + @Override public void update() { switch (desiredState) { case FASTINTAKE: - intake.set(Constants.IntakeConstants.FastIntake); + intake.set(Constants.IntakeConstants.FastIntake); break; case SLOWINTAKE: intake.set(Constants.IntakeConstants.SlowIntake); @@ -79,7 +89,8 @@ public IntakeState getCurrentState() { @Override public void periodic() { + SmartDashboard.putString("IntakeHandlerState", currentState.toString()); update(); // This method will be called once per scheduler run } -} +} diff --git a/src/main/java/frc/robot/handlers/IntakeSlideHandler.java b/src/main/java/frc/robot/handlers/IntakeSlideHandler.java new file mode 100644 index 0000000..aa9572b --- /dev/null +++ b/src/main/java/frc/robot/handlers/IntakeSlideHandler.java @@ -0,0 +1,123 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.handlers; + +import static edu.wpi.first.units.Units.Rotations; + +import com.ctre.phoenix6.signals.Enable5VRailValue; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.subsystems.Elevator; + +public class IntakeSlideHandler extends SubsystemBase implements StateSubsystem { + + public enum IntakeSlideState implements State { + OUT, + MIDDLE, + IN, + SLOWIN, + BRAKE, + REZEROIN, + REZEROOUT, + OSCILLATE + } + + private static IntakeSlideHandler instance; + private final Elevator intakeSlide = new Elevator(); //name it correct + + + Trigger intakeAboveFive = new Trigger(() -> intakeSlide.getPosition().in(Rotations) > 5.0); + + + private IntakeSlideState desiredState = IntakeSlideState.IN; + private IntakeSlideState currentState = IntakeSlideState.IN; + /** Creates a new IntakeHandler. */ + private IntakeSlideHandler() {} + + public static IntakeSlideHandler getInstance(){ + if (instance == null){ + instance = new IntakeSlideHandler(); + } + return instance; + } + + @Override + public void setDesiredState(State state){ + if (state instanceof IntakeSlideState IntakeSlideState && desiredState != IntakeSlideState) { + desiredState = IntakeSlideState; + } + } + + @Override + public void handleStateTransition() { + update(); + } + + boolean isAtLowSetpoint = false; + @Override + public void update() { + if((currentState != desiredState)){ + switch (desiredState) { + case OUT: + intakeSlide.goToSetpoint(() -> Elevator.Setpoint.OUT).schedule(); + break; + case MIDDLE: + intakeSlide.goToSetpoint(() -> Elevator.Setpoint.Middle).schedule(); + break; + case IN: //In also re-zeros it once it reaches "in" + new SequentialCommandGroup( + intakeSlide.goToSetpoint(() -> Elevator.Setpoint.IN) + .until(() -> isAtLowSetpoint), + intakeSlide.calibrateZeroIn() + ).schedule(); + break; + case SLOWIN: + new SequentialCommandGroup( + Commands.waitSeconds(1), + intakeSlide.manualDrive(() -> -0.125).until(intakeSlide.isHardStop).withTimeout( 5) + ).schedule(); + //intakeSlide.goToSetpoint(() -> Elevator.Setpoint.Middle).schedule(); + break; + case OSCILLATE: + new SequentialCommandGroup( + intakeSlide.goToSetpoint(() -> Elevator.Setpoint.OUT).withTimeout(0.5), + Commands.waitSeconds(0.5), + intakeSlide.goToSetpoint(() -> Elevator.Setpoint.IN).withTimeout(0.5) + ).repeatedly().schedule(); + break; + case BRAKE: + intakeSlide.holdPosition(); + break; + case REZEROIN: + intakeSlide.calibrateZeroIn().schedule(); + break; + case REZEROOUT: + intakeSlide.calibrateZeroOut().schedule(); + break; + default: + intakeSlide.holdPosition(); + break; + } + currentState = desiredState; + } + } + + public IntakeSlideState getCurrentState() { + return currentState; + } + + @Override + public void periodic() { + update(); + isAtLowSetpoint = intakeSlide.isAtSetpoint(Elevator.Setpoint.IN); + SmartDashboard.putBoolean("isAtLowSetpoint", isAtLowSetpoint); + SmartDashboard.putString("IntakeSlide State", currentState.toString()); + // This method will be called once per scheduler run + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/handlers/ShooterHandler.java b/src/main/java/frc/robot/handlers/ShooterHandler.java index 6e22125..39d0656 100644 --- a/src/main/java/frc/robot/handlers/ShooterHandler.java +++ b/src/main/java/frc/robot/handlers/ShooterHandler.java @@ -2,22 +2,22 @@ import static edu.wpi.first.units.Units.RPM; +import edu.wpi.first.math.controller.BangBangController; import edu.wpi.first.units.AngularVelocityUnit; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; +import frc.robot.subsystems.BangBangShooterSubsystem; import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.subsystems.ShooterSubsystem; import frc.robot.RobotContainer; public class ShooterHandler extends SubsystemBase implements StateSubsystem { - public Joystick leftJoystick = new Joystick(2); - public enum ShooterState implements State { SHOOTING, @@ -30,13 +30,11 @@ public enum ShooterState implements State { private static ShooterHandler instance; // AFTER: - private CommandSwerveDrivetrain drivetrain; // ← ADDED THIS - private ShooterSubsystem shooter; // ← REMOVED "= new ShooterSubsystem()" + private CommandSwerveDrivetrain drivetrain; + private BangBangShooterSubsystem shooter; private ShooterState desiredState = ShooterState.OFF; private ShooterState currentState = ShooterState.OFF; - private Command shooterCommand = null; // ← ADD THIS - private ShooterHandler() {} @@ -47,17 +45,17 @@ public static ShooterHandler getInstance() { return instance; } - public void initialize(CommandSwerveDrivetrain drivetrain, ShooterSubsystem shooter) { + public void initialize(CommandSwerveDrivetrain drivetrain, BangBangShooterSubsystem shooter) { this.drivetrain = drivetrain; this.shooter = shooter; } - AngularVelocity TuneablefastShot = Constants.ShooterConstants.FastShot; + double TuneablefastShot = 40; public void adjustFastShot(double valu){ - TuneablefastShot = TuneablefastShot.plus(RPM.of(valu)); + TuneablefastShot = TuneablefastShot + (valu); if(currentState == ShooterState.TUNING){ - shooter.setVelocity(TuneablefastShot); + shooter.setTargetVelocity(TuneablefastShot); } } @@ -88,60 +86,45 @@ public void update() { System.out.println("ERROR: ShooterHandler not initialized! Call initialize() first."); return; } - if (currentState != desiredState) { - System.out.println(" State changing from " + currentState + " to " + desiredState); - handleStateChange(); // switch states + if (currentState != desiredState || currentState == ShooterState.SHOOTING) { + handleStateChange(); } - updateContinuousStates(); } + double DistMeters = 0; + private void handleStateChange(){ switch (desiredState) { case SHOOTING: - //double DistMeters = drivetrain.GetFutureDistMeters(); - //shooter.setVelocityWithCalc(DistMeters).schedule(); + shooter.setTargetVeloDistance(DistMeters); break; case SLOW: - CommandScheduler.getInstance().schedule(shooter.setVelocity(Constants.ShooterConstants.SlowShot)); + shooter.setTargetVelocity(Constants.ShooterConstants.SlowShot); break; case FAST: - CommandScheduler.getInstance().schedule(shooter.setVelocity(Constants.ShooterConstants.FastShot)); + shooter.setTargetVelocity(Constants.ShooterConstants.FastShot); break; case TUNING: - //shooter.setVelocity(TuneablefastShot).schedule(); + shooter.setTargetVelocity(TuneablefastShot); break; case OFF: - CommandScheduler.getInstance().schedule(shooter.stop()); + shooter.coast(); break; default: - CommandScheduler.getInstance().schedule(shooter.stop()); + //CommandScheduler.getInstance().schedule(shooter.stop()); break; } currentState = desiredState; } - private void updateContinuousStates() { - switch (currentState) { - case SHOOTING: - double DistMeters = drivetrain.GetFutureDistMeters(); - AngularVelocity targetSpeed = shooter.getCalcedRPM(DistMeters); - CommandScheduler.getInstance().schedule(shooter.setVelocity(targetSpeed)); - break; - - case TUNING: - CommandScheduler.getInstance().schedule(shooter.setVelocity(TuneablefastShot)); - break; - - default: - break; - } - } public ShooterState getCurrentState() { return currentState; } @Override public void periodic() { + DistMeters = drivetrain.getDistance(); update(); // Handle state transitions + SmartDashboard.putString("ShooterState", currentState.toString()); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/handlers/Superstructure.java b/src/main/java/frc/robot/handlers/Superstructure.java index d910936..661c20c 100644 --- a/src/main/java/frc/robot/handlers/Superstructure.java +++ b/src/main/java/frc/robot/handlers/Superstructure.java @@ -1,31 +1,23 @@ package frc.robot.handlers; +import static edu.wpi.first.units.Units.Degrees; + import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.ShooterConstants; -import frc.robot.handlers.IndexerHighHandler.IndexerHighState; -import frc.robot.handlers.IndexerLowHandler.IndexerLowState; -import frc.robot.subsystems.ClimbSubsystem; +import frc.robot.subsystems.BangBangShooterSubsystem; import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.subsystems.HopperSubsystem; -import frc.robot.subsystems.IndexerHighSubsystem; -import frc.robot.subsystems.IndexerLowSubsystem; -import frc.robot.subsystems.IntakeSubsystem; -import frc.robot.subsystems.ShooterSubsystem; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.Commands; - -import static edu.wpi.first.units.Units.Degrees; - -import edu.wpi.first.math.geometry.Pose2d; public class Superstructure extends SubsystemBase { // Define your states public enum SuperstructureState { STATIONARYSHOT, + SHOOTONTHEMOVE, + SHOOTONTHEMOVESPINUP, INTAKE, + INTAKESLOW, + INTAKESLOWSLOW, SLOWSHOT, REVERSE, SPINUP, @@ -35,15 +27,13 @@ public enum SuperstructureState { SPINUPSLOW, SPINUPFAST, FASTSHOT, - CLIMBPREP, - CLIMBED + AIM, + BUMP } private static Superstructure instance; - private ShooterSubsystem shooter; + private BangBangShooterSubsystem shooter; private CommandSwerveDrivetrain drivetrain; - private ClimbSubsystem climber; - //private final ShooterSubsystem Shooter = new ShooterSubsystem(); private final ShooterHandler shooterHandler = ShooterHandler.getInstance(); @@ -51,6 +41,8 @@ public enum SuperstructureState { //private final IntakeSubsystem Intake = new IntakeSubsystem(); private final IntakeHandler intakeHandler = IntakeHandler.getInstance(); + private final IntakeSlideHandler intakeSlideHandler = IntakeSlideHandler.getInstance(); + //private final HopperSubsystem Hopper = new HopperSubsystem(); private final HopperHandler hopperHandler = HopperHandler.getInstance(); @@ -60,11 +52,13 @@ public enum SuperstructureState { private final DriveHandler driveHandler = DriveHandler.getInstance(); - private final ClimbHandler climbHandler = ClimbHandler.getInstance(); - private SuperstructureState desiredState = SuperstructureState.IDLE; private SuperstructureState currentState = SuperstructureState.IDLE; private Angle targetAngle = Degrees.of(0); + + boolean DTaimed = false; + boolean ShooterAtVelo = false; + boolean WaitTimed = false; public static Superstructure getInstance(){ @@ -77,10 +71,9 @@ public static Superstructure getInstance(){ /** * Initialize superstructure with required subsystems */ - public void initialize(ShooterSubsystem shooter, CommandSwerveDrivetrain drivetrain, ClimbSubsystem climber) { + public void initialize(BangBangShooterSubsystem shooter, CommandSwerveDrivetrain drivetrain) { this.shooter = shooter; this.drivetrain = drivetrain; - this.climber = climber; } private Superstructure() {} @@ -104,7 +97,7 @@ private void handleStateTransition() { indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.OFF); indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.OFF); driveHandler.setDesiredState(DriveHandler.DriveState.TELEOPDRIVE); - climbHandler.setDesiredState(ClimbHandler.ClimbState.OFF); + intakeSlideHandler.setDesiredState(IntakeSlideHandler.IntakeSlideState.BRAKE); break; case INTAKE: shooterHandler.setDesiredState(ShooterHandler.ShooterState.OFF); @@ -112,90 +105,119 @@ private void handleStateTransition() { hopperHandler.setDesiredState(HopperHandler.HopperState.OFF); indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.OFF); indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.OFF); - driveHandler.setDesiredState(DriveHandler.DriveState.SNAKE); - climbHandler.setDesiredState(ClimbHandler.ClimbState.OFF); + driveHandler.setDesiredState(DriveHandler.DriveState.TELEOPDRIVE); + intakeSlideHandler.setDesiredState(IntakeSlideHandler.IntakeSlideState.OUT); + break; + case INTAKESLOW: + shooterHandler.setDesiredState(ShooterHandler.ShooterState.OFF); + intakeHandler.setDesiredState(IntakeHandler.IntakeState.FASTINTAKE); + hopperHandler.setDesiredState(HopperHandler.HopperState.OFF); + indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.OFF); + indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.OFF); + driveHandler.setDesiredState(DriveHandler.DriveState.TELEOPDRIVESLOW); + intakeSlideHandler.setDesiredState(IntakeSlideHandler.IntakeSlideState.OUT); + break; + case INTAKESLOWSLOW: + shooterHandler.setDesiredState(ShooterHandler.ShooterState.OFF); + intakeHandler.setDesiredState(IntakeHandler.IntakeState.SLOWINTAKE); + hopperHandler.setDesiredState(HopperHandler.HopperState.OFF); + indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.OFF); + indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.OFF); + driveHandler.setDesiredState(DriveHandler.DriveState.TELEOPDRIVESLOW); + intakeSlideHandler.setDesiredState(IntakeSlideHandler.IntakeSlideState.OUT); break; case SPINUP: - shooterHandler.setDesiredState(ShooterHandler.ShooterState.SHOOTING); + shooterHandler.setDesiredState(ShooterHandler.ShooterState.SHOOTING); //change to shooting intakeHandler.setDesiredState(IntakeHandler.IntakeState.OFF); hopperHandler.setDesiredState(HopperHandler.HopperState.OFF); indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.OFF); indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.OFF); driveHandler.setDesiredState(DriveHandler.DriveState.AUTOALLIGN); - climbHandler.setDesiredState(ClimbHandler.ClimbState.OFF); + ShooterAtVelo = false; break; case STATIONARYSHOT: shooterHandler.setDesiredState(ShooterHandler.ShooterState.SHOOTING); - intakeHandler.setDesiredState(IntakeHandler.IntakeState.FASTINTAKE); + intakeHandler.setDesiredState(IntakeHandler.IntakeState.OFF); hopperHandler.setDesiredState(HopperHandler.HopperState.FAST); indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.SLOWINTAKE); indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.SLOWINTAKE); driveHandler.setDesiredState(DriveHandler.DriveState.AUTOALLIGN); - climbHandler.setDesiredState(ClimbHandler.ClimbState.OFF); + intakeSlideHandler.setDesiredState(IntakeSlideHandler.IntakeSlideState.SLOWIN); break; - case SPINUPSLOW: - shooterHandler.setDesiredState(ShooterHandler.ShooterState.SLOW); + case SHOOTONTHEMOVE: + shooterHandler.setDesiredState(ShooterHandler.ShooterState.SHOOTING); + intakeHandler.setDesiredState(IntakeHandler.IntakeState.OFF); + hopperHandler.setDesiredState(HopperHandler.HopperState.FAST); + indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.SLOWINTAKE); + indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.SLOWINTAKE); + driveHandler.setDesiredState(DriveHandler.DriveState.SHOOTONTHEMOVE); + intakeSlideHandler.setDesiredState(IntakeSlideHandler.IntakeSlideState.SLOWIN); + break; + case SHOOTONTHEMOVESPINUP: + shooterHandler.setDesiredState(ShooterHandler.ShooterState.SHOOTING); intakeHandler.setDesiredState(IntakeHandler.IntakeState.OFF); hopperHandler.setDesiredState(HopperHandler.HopperState.OFF); indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.OFF); indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.OFF); + driveHandler.setDesiredState(DriveHandler.DriveState.SHOOTONTHEMOVE); + break; + case TUNING: //dont use + shooterHandler.setDesiredState(ShooterHandler.ShooterState.TUNING); + intakeHandler.setDesiredState(IntakeHandler.IntakeState.OFF); + hopperHandler.setDesiredState(HopperHandler.HopperState.FAST); + indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.FAST); + indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.FAST); driveHandler.setDesiredState(DriveHandler.DriveState.AUTOALLIGN); - climbHandler.setDesiredState(ClimbHandler.ClimbState.OFF); break; - case SLOWSHOT: + case SLOWSHOT: //dont use shooterHandler.setDesiredState(ShooterHandler.ShooterState.SLOW); intakeHandler.setDesiredState(IntakeHandler.IntakeState.OFF); hopperHandler.setDesiredState(HopperHandler.HopperState.SLOW); indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.SLOWINTAKE); indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.SLOWINTAKE); - driveHandler.setDesiredState(DriveHandler.DriveState.AUTOALLIGN); - climbHandler.setDesiredState(ClimbHandler.ClimbState.OFF); + //driveHandler.setDesiredState(DriveHandler.DriveState.AUTOALLIGN); break; - case SPINUPFAST: - shooterHandler.setDesiredState(ShooterHandler.ShooterState.FAST); + case SPINUPFAST: //dont use + shooterHandler.setDesiredState(ShooterHandler.ShooterState.FAST); //change to shooting intakeHandler.setDesiredState(IntakeHandler.IntakeState.OFF); hopperHandler.setDesiredState(HopperHandler.HopperState.OFF); indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.OFF); - indexerHighHandler.setDesiredState(IndexerLowHandler.IndexerLowState.OFF); - climbHandler.setDesiredState(ClimbHandler.ClimbState.OFF); + indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.OFF); + driveHandler.setDesiredState(DriveHandler.DriveState.AUTOALLIGN); + ShooterAtVelo = false; break; - case FASTSHOT: - shooterHandler.setDesiredState(ShooterHandler.ShooterState.SHOOTING); - intakeHandler.setDesiredState(IntakeHandler.IntakeState.SLOWINTAKE); + case FASTSHOT: //dont use + shooterHandler.setDesiredState(ShooterHandler.ShooterState.FAST); + intakeHandler.setDesiredState(IntakeHandler.IntakeState.OFF); hopperHandler.setDesiredState(HopperHandler.HopperState.FAST); - indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.FAST); - indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.FAST); + indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.SLOWINTAKE); + indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.SLOWINTAKE); driveHandler.setDesiredState(DriveHandler.DriveState.AUTOALLIGN); - climbHandler.setDesiredState(ClimbHandler.ClimbState.OFF); + intakeSlideHandler.setDesiredState(IntakeSlideHandler.IntakeSlideState.SLOWIN); + break; + case REVERSE: + shooterHandler.setDesiredState(ShooterHandler.ShooterState.OFF); + intakeHandler.setDesiredState(IntakeHandler.IntakeState.FASTREVERSE); + hopperHandler.setDesiredState(HopperHandler.HopperState.FASTOUT); + indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.SLOWREVERSE); + indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.FASTREVERSE); + intakeSlideHandler.setDesiredState(IntakeSlideHandler.IntakeSlideState.OUT); break; - case CLIMBPREP: + case AIM: shooterHandler.setDesiredState(ShooterHandler.ShooterState.OFF); intakeHandler.setDesiredState(IntakeHandler.IntakeState.OFF); hopperHandler.setDesiredState(HopperHandler.HopperState.OFF); indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.OFF); indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.OFF); - driveHandler.setDesiredState(DriveHandler.DriveState.TELEOPDRIVE); - climbHandler.setDesiredState(ClimbHandler.ClimbState.LOW); - break; - case CLIMBED: - climbHandler.setDesiredState(ClimbHandler.ClimbState.LOWPULL); - break; - case TUNING: - shooterHandler.setDesiredState(ShooterHandler.ShooterState.TUNING); - intakeHandler.setDesiredState(IntakeHandler.IntakeState.OFF); - hopperHandler.setDesiredState(HopperHandler.HopperState.OFF); - indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.FAST); - indexerHighHandler.setDesiredState(IndexerLowHandler.IndexerLowState.FAST); driveHandler.setDesiredState(DriveHandler.DriveState.AUTOALLIGN); - climbHandler.setDesiredState(ClimbHandler.ClimbState.OFF); break; - case REVERSE: + case BUMP: shooterHandler.setDesiredState(ShooterHandler.ShooterState.OFF); - intakeHandler.setDesiredState(IntakeHandler.IntakeState.FASTREVERSE); + intakeHandler.setDesiredState(IntakeHandler.IntakeState.OFF); hopperHandler.setDesiredState(HopperHandler.HopperState.OFF); indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.OFF); indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.OFF); - climbHandler.setDesiredState(ClimbHandler.ClimbState.OFF); + driveHandler.setDesiredState(DriveHandler.DriveState.BUMP_LOCK); break; case OFF: shooterHandler.setDesiredState(ShooterHandler.ShooterState.OFF); @@ -204,7 +226,7 @@ private void handleStateTransition() { indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.OFF); indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.OFF); driveHandler.setDesiredState(DriveHandler.DriveState.TELEOPDRIVE); - climbHandler.setDesiredState(ClimbHandler.ClimbState.OFF); + intakeSlideHandler.setDesiredState(IntakeSlideHandler.IntakeSlideState.IN); break; default: shooterHandler.setDesiredState(ShooterHandler.ShooterState.OFF); @@ -213,7 +235,7 @@ private void handleStateTransition() { indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.OFF); indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.OFF); driveHandler.setDesiredState(DriveHandler.DriveState.TELEOPDRIVE); - climbHandler.setDesiredState(ClimbHandler.ClimbState.OFF); + intakeSlideHandler.setDesiredState(IntakeSlideHandler.IntakeSlideState.IN); break; } currentState = desiredState; @@ -227,23 +249,29 @@ public void periodic() { return; } - if (currentState == SuperstructureState.SPINUP && (shooter.isAtTargetVelo() || drivetrain.isAimedAtTarget())){ //checks if its at target velo and angle - setDesiredState(SuperstructureState.STATIONARYSHOT); + DTaimed = drivetrain.isAimedAtTarget(); + if (CommandSwerveDrivetrain.isInAllianceZone(drivetrain.getPose())){ + ShooterAtVelo = shooter.atTargetVelo(); } - - if (currentState == SuperstructureState.CLIMBPREP && climbHandler.extendedClimb){ - setDesiredState(SuperstructureState.CLIMBED); + else { + ShooterAtVelo = shooter.atTargetVeloPassing(); } + - /*if (currentState == SuperstructureState.CLIMBPREP && climber.atTarget()){ - setDesiredState(SuperS); - }*/ + + if (currentState == SuperstructureState.SPINUP && ShooterAtVelo && DTaimed){ //checks if its at target velo and angle + setDesiredState(SuperstructureState.STATIONARYSHOT); + WaitTimed = false; + } + + + SmartDashboard.putString("SuperState", currentState.toString()); - SmartDashboard.putBoolean("shooterAtVelo?", shooter.isAtTargetVelo()); - SmartDashboard.putBoolean("Drivetrain aimed?",drivetrain.isAimedAtTarget()); + SmartDashboard.putBoolean("shooterAtVelo?", ShooterAtVelo); + SmartDashboard.putBoolean("Drivetrain aimed?",DTaimed); + SmartDashboard.putBoolean("Timer", WaitTimed); } - public SuperstructureState getCurrentState() { return currentState; } diff --git a/src/main/java/frc/robot/handlers/VisionHandler.java b/src/main/java/frc/robot/handlers/VisionHandler.java new file mode 100644 index 0000000..319a4de --- /dev/null +++ b/src/main/java/frc/robot/handlers/VisionHandler.java @@ -0,0 +1,51 @@ +package frc.robot.handlers; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.handlers.StateSubsystem.State; +import frc.robot.subsystems.Vision; + +public class VisionHandler extends SubsystemBase { + + public enum VisionState implements State { + IDLE, + CHASING + } + + private Vision vision; + private static VisionHandler instance; + + private VisionState desiredState = VisionState.IDLE; + private VisionState currentState = VisionState.IDLE; + + private VisionHandler() {} + + public static VisionHandler getInstance() { + if (instance == null) instance = new VisionHandler(); + return instance; + } + + public void initialize(Vision vision) { + this.vision = vision; + } + + public void setDesiredState(VisionState state) { + if (desiredState != state) { + desiredState = state; + } + } + + public boolean isChasing() { + return currentState == VisionState.CHASING; + } + + public Vision getVision() { + return vision; + } + + @Override + public void periodic() { + currentState = desiredState; + SmartDashboard.putString("VisionState", currentState.toString()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/BangBangShooterSubsystem.java b/src/main/java/frc/robot/subsystems/BangBangShooterSubsystem.java new file mode 100644 index 0000000..6b28d62 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/BangBangShooterSubsystem.java @@ -0,0 +1,226 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.math.controller.BangBangController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + + +public class BangBangShooterSubsystem extends SubsystemBase { + /** Creates a new BangBangShooterSubsystem. */ + private final TalonFX ShooterMotor = new TalonFX(Constants.ShooterConstants.ShooterLeaderID, "Upper"); + private final TalonFX ShooterMotor2 = new TalonFX(Constants.ShooterConstants.ShooterFollowerID, "Upper"); + private final TalonFX ShooterMotorRev3 = new TalonFX(Constants.ShooterConstants.ShooterFollowerReversed2ID, "Upper"); + private final TalonFX ShooterMotorRev4 = new TalonFX(Constants.ShooterConstants.ShooterFollowerReversedID, "Upper"); + + private final BangBangController controllerBangBang = new BangBangController(); + + private static final InterpolatingDoubleTreeMap Shooter1Map = new InterpolatingDoubleTreeMap(); + private static final InterpolatingDoubleTreeMap Shooter2Map = new InterpolatingDoubleTreeMap(); + private static final InterpolatingDoubleTreeMap Shooter3Map = new InterpolatingDoubleTreeMap(); + private static final InterpolatingDoubleTreeMap Shooter4Map = new InterpolatingDoubleTreeMap(); + + //inches to center hub from Robot orign , RPS + static { + Shooter1Map.put(138.4 ,105.0); + Shooter1Map.put(121.4 ,77.0); + Shooter1Map.put(104.6 ,70.0); + Shooter1Map.put(85.8, 64.0); + Shooter1Map.put(69.0, 60.0); + Shooter1Map.put(56.7,52.0); + + Shooter2Map.put(138.4 ,105.0); + Shooter2Map.put(121.4 ,77.0); + Shooter2Map.put(104.6 ,70.0); + Shooter2Map.put(85.8, 64.0); + Shooter2Map.put(69.0, 60.0); + Shooter2Map.put(56.7,52.0); + + + Shooter3Map.put(138.4 ,105.0); + Shooter3Map.put(121.4 ,77.0); + Shooter3Map.put(104.6 ,70.0); + Shooter3Map.put(85.8, 64.0); + Shooter3Map.put(69.0, 60.0); + Shooter3Map.put(56.7,52.0); + + + Shooter4Map.put(138.4 ,105.0); + Shooter4Map.put(121.4 ,77.0); + Shooter4Map.put(104.6 ,70.0); + Shooter4Map.put(85.8, 64.0); + Shooter4Map.put(69.0, 60.0); + Shooter4Map.put(56.7,52.0); + + } + + + private final SimpleMotorFeedforward feedFoward = new SimpleMotorFeedforward(Constants.ShooterConstants.kS, Constants.ShooterConstants.kV); + private final VoltageOut voltageRequest = new VoltageOut(0); + + private double targetVeloRPS1 = 0; + private double targetVeloRPS2 = 0; + private double targetVeloRPS3 = 0; + private double targetVeloRPS4 = 0; + + private double ShooterMult = 0.95; + + + + private boolean shooterEnabled = false; + + private boolean MaxRPM = false; + private boolean MultiplierOn = true; + + public BangBangShooterSubsystem() { + + ShooterMotor.setNeutralMode(NeutralModeValue.Coast); + ShooterMotor2.setNeutralMode(NeutralModeValue.Coast); + ShooterMotorRev3.setNeutralMode(NeutralModeValue.Coast); + ShooterMotorRev4.setNeutralMode(NeutralModeValue.Coast); + + controllerBangBang.setTolerance(Constants.ShooterConstants.RPSTolarance); + } + + + private static BangBangShooterSubsystem instance; + public static BangBangShooterSubsystem getInstance(){ + if (instance == null){ + instance = new BangBangShooterSubsystem(); + } + return instance; + } + + public void changeShooterMult(double multAdd){ + ShooterMult += multAdd; + } + + public void toggleShooterMult(){ + if (MultiplierOn) {MultiplierOn = false;} + if (!MultiplierOn) {MultiplierOn = true;} + } + + public void resetShooterMult(){ + ShooterMult = 1.0; //change to 0.95 + } + + // without mult + public void setTargetVelocity(double VelocityRPS) { + if (VelocityRPS > Constants.ShooterConstants.RPSHardStop) { + targetVeloRPS1 = Constants.ShooterConstants.RPSHardStop; + targetVeloRPS2 = Constants.ShooterConstants.RPSHardStop; + targetVeloRPS3 = Constants.ShooterConstants.RPSHardStop; + targetVeloRPS4 = Constants.ShooterConstants.RPSHardStop; + } + + else { + targetVeloRPS1 = VelocityRPS; + targetVeloRPS2 = VelocityRPS; + targetVeloRPS3 = VelocityRPS; + targetVeloRPS4 = VelocityRPS; + } + shooterEnabled = true; + } + // with mult + public void setTargetVeloDistance(double distance) { + double inches = distance * 39.3701; + targetVeloRPS1 = Shooter1Map.get(inches) * ShooterMult; + targetVeloRPS2 = Shooter2Map.get(inches)* ShooterMult; + targetVeloRPS3 = Shooter3Map.get(inches)* ShooterMult; + targetVeloRPS4 = Shooter4Map.get(inches)* ShooterMult; + + if (targetVeloRPS1 >= Constants.ShooterConstants.RPSHardStop) { targetVeloRPS1 = Constants.ShooterConstants.RPSHardStop;} + if (targetVeloRPS2 >= Constants.ShooterConstants.RPSHardStop) { targetVeloRPS2 = Constants.ShooterConstants.RPSHardStop;} + if (targetVeloRPS3 >= Constants.ShooterConstants.RPSHardStop) { targetVeloRPS3 = Constants.ShooterConstants.RPSHardStop;} + if (targetVeloRPS4 >= Constants.ShooterConstants.RPSHardStop) { targetVeloRPS4 = Constants.ShooterConstants.RPSHardStop;} + + if (targetVeloRPS1 >= 104.0){ + MaxRPM = true; + } + else{ + MaxRPM = false; + } + + shooterEnabled = true; + } + + public void coast(){ + shooterEnabled = false; + targetVeloRPS1 = 0; + targetVeloRPS2 = 0; + targetVeloRPS3 = 0; + targetVeloRPS4 = 0; + } + + public boolean atTargetVelo() { + boolean atTargBelo = Math.abs(ShooterMotor.getVelocity().getValueAsDouble()) >= targetVeloRPS1-2.0; + return atTargBelo; + } + + public boolean atTargetVeloPassing() { + boolean atTargBelo = (Math.abs(ShooterMotor.getVelocity().getValueAsDouble()) >= targetVeloRPS1-2.0) || ShooterMotor.getVelocity().getValueAsDouble() >= 98.0; + return atTargBelo; + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + if (!shooterEnabled){ + ShooterMotor.setControl(voltageRequest.withOutput(0)); + ShooterMotor2.setControl(voltageRequest.withOutput(0)); + ShooterMotorRev3.setControl(voltageRequest.withOutput(0)); + ShooterMotorRev4.setControl(voltageRequest.withOutput(0)); + return; + } + + double currentVelocity1RPS = Math.abs(ShooterMotor.getVelocity().getValueAsDouble()); + double currentVelocity2RPS = Math.abs(ShooterMotor2.getVelocity().getValueAsDouble()); + double currentVelocity3RPS = Math.abs(ShooterMotorRev3.getVelocity().getValueAsDouble()); + double currentVelocity4RPS = Math.abs(ShooterMotorRev4.getVelocity().getValueAsDouble()); + + double bangBangVolts1 = controllerBangBang.calculate(currentVelocity1RPS, targetVeloRPS1)*12; + double bangBangVolts2 = controllerBangBang.calculate(currentVelocity2RPS, targetVeloRPS2)*12; + double bangBangVolts3 = controllerBangBang.calculate(currentVelocity3RPS, targetVeloRPS3)*12; + double bangBangVolts4 = controllerBangBang.calculate(currentVelocity4RPS, targetVeloRPS4)*12; + + double feedfowardVolts1 = feedFoward.calculate(targetVeloRPS1)*0.9; + double feedfowardVolts2 = feedFoward.calculate(targetVeloRPS2)*0.9; + double feedfowardVolts3 = feedFoward.calculate(targetVeloRPS3)*0.9; + double feedfowardVolts4 = feedFoward.calculate(targetVeloRPS4)*0.9; + + ShooterMotor.setControl(voltageRequest.withOutput(-(bangBangVolts1 + feedfowardVolts1))); //idk bru bang bang volts 1 + ShooterMotor2.setControl(voltageRequest.withOutput(-(bangBangVolts2 + feedfowardVolts2))); //backwards in phy tuner + ShooterMotorRev3.setControl(voltageRequest.withOutput(-(bangBangVolts3 + feedfowardVolts3))); //- + ShooterMotorRev4.setControl(voltageRequest.withOutput(-(bangBangVolts4 + feedfowardVolts4))); //- + + + SmartDashboard.putNumber("Shooter1Volts", bangBangVolts1 + feedfowardVolts1); + SmartDashboard.putNumber("Shooter2Volts", bangBangVolts2 + feedfowardVolts2); + SmartDashboard.putNumber("Shooter3Volts", bangBangVolts3 + feedfowardVolts3); + SmartDashboard.putNumber("Shooter4Volts", bangBangVolts4 + feedfowardVolts4); + + SmartDashboard.putNumber("Shooter1RPS", currentVelocity1RPS); + SmartDashboard.putNumber("Shooter2RPS", currentVelocity2RPS); + SmartDashboard.putNumber("Shooter3RPS", currentVelocity3RPS); + SmartDashboard.putNumber("Shooter4RPS", currentVelocity4RPS); + + SmartDashboard.putNumber("TargetRPS1", targetVeloRPS1); + SmartDashboard.putNumber("TargetRPS2", targetVeloRPS2); + SmartDashboard.putNumber("TargetRPS3", targetVeloRPS3); + SmartDashboard.putNumber("TargetRPS4", targetVeloRPS4); + + SmartDashboard.putNumber("ShooterMult", ShooterMult); + SmartDashboard.putBoolean("MaxRPM?", MaxRPM); + SmartDashboard.putBoolean("MultpilerOn?", MultiplierOn); + } +} diff --git a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java index fab88e9..9000790 100644 --- a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java @@ -47,8 +47,6 @@ public class ClimbSubsystem extends SubsystemBase { // Position Limits private final double softLowerLimitMeters = 0.0; private final double softUpperLimitMeters = 2.0; - private final double hardLowerLimitMeters = 0.0; - private final double hardUpperLimitMeters = 3.0; // Control private final ElevatorFeedforward feedforward = new ElevatorFeedforward(0, .5, 0, 0); // Tune kG for gravity compensation diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index b876547..e470f38 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -1,47 +1,48 @@ package frc.robot.subsystems; -import static edu.wpi.first.units.Units.*; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Second; +import static edu.wpi.first.units.Units.Volts; import java.util.Optional; import java.util.function.Supplier; import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.mechanisms.swerve.LegacySwerveRequest.FieldCentric; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveRequest; - import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; + import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import edu.wpi.first.units.*; import frc.robot.Constants; import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** * Class that extends the Phoenix 6 SwerveDrivetrain class and implements * Subsystem so it can easily be used in command-based projects. @@ -345,38 +346,63 @@ public Optional samplePoseAt(double timestampSeconds) { return super.samplePoseAt(Utils.fpgaToCurrentTime(timestampSeconds)); } - // Hub poses for each alliance + // --------------------- END GENERATED CODE ------------------- + + private double hubOffsetX = 0.0; + private double hubOffsetY = 0.0; + + public void setHubOffset(double offsetX, double offsetY) { + hubOffsetX += offsetX; + hubOffsetY += offsetY; + } + + public void resetHubOffset() { + hubOffsetX = 0.0; + hubOffsetY = 0.0; + } // Get hub pose based on alliance - public static Pose3d getHubPose() { - return DriverStation.getAlliance() - .map(alliance -> alliance == Alliance.Red ? Constants.FieldConstants.redHubPose : Constants.FieldConstants.blueHubPose) - .orElse(Constants.FieldConstants.blueHubPose); // Default to blue if alliance unknown +// Replace the existing static getHubPose() with this instance version +public Pose3d getHubPose() { + Pose3d base = DriverStation.getAlliance() + .map(alliance -> alliance == Alliance.Red ? Constants.FieldConstants.redHubPose : Constants.FieldConstants.blueHubPose).orElse(Constants.FieldConstants.blueHubPose); + + // Apply offsets relative to alliance + // For Red alliance, flip X offset direction since field is mirrored + double adjustedX = DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red + ? base.getX() - hubOffsetX + : base.getX() + hubOffsetX; + + double adjustedY = base.getY() + hubOffsetY; + + return new Pose3d(adjustedX, adjustedY, base.getZ(), base.getRotation()); +} + + + public static Distance getCloseBumpY(Pose2d currentPose){ + if (currentPose.getMeasureY().gt(Inches.of(158.845))){ + return Inches.of(218.84); + } else { + return Inches.of(98.84); + } + } + + public static boolean isInAllianceZone(Pose2d robotPose){ + double robotX = robotPose.getX(); + Alliance alliance = DriverStation.getAlliance().orElse(Alliance.Blue); + if (alliance == Alliance.Red) { + return robotX > Inches.of(469.11).in(Meters); + } else { + return robotX < Inches.of(182.11).in(Meters); + } } + /** * Calculates the closest point on the predefined circle to the current robot pose. * * @return The closest point on the circle to the current robot position */ - public Pose2d getClosestPointOnHub() { - - Pose2d hub = getHubPose().toPose2d(); - Pose2d currentPose = getPose(); - - // Calculate vector from center to current position - double deltaX = currentPose.getX() - hub.getX(); - double deltaY = currentPose.getY() - hub.getY(); - - // Calculate angle from center to current position - double angle = Math.atan2(deltaY, deltaX); - - // Calculate the closest point on the circle - double closestX = hub.getX() + Constants.ShooterConstants.ShootingDistance * Math.cos(angle); - double closestY = hub.getY() + Constants.ShooterConstants.ShootingDistance * Math.sin(angle); - - return new Pose2d(closestX, closestY, new Rotation2d(angle)); - } public static final PIDController rotationController = getRotationController(); @@ -400,83 +426,109 @@ public double getDistance(){ return currentDistance; } + public Pose2d getTargetPose(Pose2d currentPose2d){ + final double thresholdY = Inches.of(158.32).in(Meters); //horizontal middle field line + Alliance alliance = DriverStation.getAlliance().orElse(Alliance.Blue); + if (alliance == Alliance.Red){ + if(isInAllianceZone(currentPose2d)){ + return getHubPose().toPose2d(); + } + else if (currentPose2d.getY() >= thresholdY){ + return Constants.FieldConstants.redTargetHighPose; + } + else { + return Constants.FieldConstants.redTargetLowPose; + } + } + if (alliance == Alliance.Blue){ + if(isInAllianceZone(currentPose2d)){ + return getHubPose().toPose2d(); + } + else if (currentPose2d.getY() >= thresholdY){ + return Constants.FieldConstants.blueTargetHighPose; + } + else { + return Constants.FieldConstants.blueTargetLowPose; + } + } + return null; + } + Pose2d ShootingLocation; - public Command headingLocktoHub(CommandXboxController controller, double maxSpeed, double maxAngularRate, String tuning) { + public Command headingLocktoHub(CommandXboxController controller, double maxSpeed, double maxAngularRate) { return applyRequest(() -> { // Get current pose and target hub position Pose2d drivePose = getState().Pose; - Pose2d targetPose = getHubPose().toPose2d(); + Pose2d targetPose = getTargetPose(drivePose); ShootingLocation = targetPose; // Calculate angle from hub to robot Translation2d toRobot = drivePose.getTranslation().minus(targetPose.getTranslation()); + + /* + if (toRobot.getNorm() < 0.1) { // Within 10cm of hub + double veloX = -controller.getLeftY(); + if (Math.abs(veloX) < 0.1) veloX = 0; + + double veloY = -controller.getLeftX(); + if (Math.abs(veloY) < 0.1) veloY = 0; + + return alignRequest + .withVelocityX(veloX * maxSpeed) + .withVelocityY(veloY * maxSpeed) + .withRotationalRate(0); + } */ + Rotation2d angleToRobot = toRobot.getAngle(); // Calculate desired rotation (face the hub) Rotation2d desiredAngle = angleToRobot.rotateBy(Rotation2d.k180deg); // Face toward hub | Take RotateBy out for back to face Hub Rotation2d currentAngle = drivePose.getRotation(); - - if(Math.abs(desiredAngle.getDegrees() - currentAngle.getDegrees()) <= 1) {desiredAngle = currentAngle;} // Calculate rotational rate to face hub double rotationalRate = rotationController.calculate( currentAngle.getRadians(), desiredAngle.getRadians() ); - double veloX = -controller.getLeftY(); - if (Math.abs(veloX) < 0.1 ){ - veloX = 0; - } - - double veloY = -controller.getLeftX(); - if (Math.abs(veloY) < 0.1 ){ - veloY = 0; - } + double xSpeed = MathUtil.applyDeadband(-controller.getLeftY(), 0.1); + double ySpeed = MathUtil.applyDeadband(-controller.getLeftX(), 0.1); - if (tuning.equals("PovLeft")) { veloX = 0; veloY = 0.3;} - if (tuning.equals("PovRight")) { veloX = 0; veloY = -0.3;} - // Apply the request: radial (distance maintenance) + tangential (circling) return alignRequest - .withVelocityX(veloX * maxSpeed) - .withVelocityY(veloY * maxSpeed) - .withRotationalRate(rotationalRate * maxAngularRate); + .withVelocityX(xSpeed * maxSpeed) + .withVelocityY(ySpeed * maxSpeed) + .withRotationalRate(rotationalRate * maxAngularRate*1.5); }); } - public Command TeleopDrive(CommandXboxController joystick, double MaxSpeed, double MaxAngularRate, SwerveRequest.FieldCentric drive, CommandSwerveDrivetrain drivetrain){ - return applyRequest(() ->{ - return alignRequest.withVelocityX(-joystick.getLeftY() * MaxSpeed).withDeadband(Constants.DriveConstants.TranslationDeadband) // Drive forward with negative Y (forward) DriveStraight = robot centric - .withVelocityY(-joystick.getLeftX() * MaxSpeed).withDeadband(Constants.DriveConstants.TranslationDeadband) // Drive left with negative X (left) - .withRotationalRate(-joystick.getRightX() * MaxAngularRate).withDeadband(Constants.DriveConstants.RotationDeadband);} // Drive counterclockwise with negative X (left) - ); } - - double futureDistance = 0.0; - public double GetFutureDistMeters(){ - Pose2d robotPose = getPose(); - double distance = getDistance(); - ChassisSpeeds fieldSpeeds = getState().Speeds; - Pose2d actualHub = getHubPose().toPose2d(); - - // Convert field speeds to robot-relative - ChassisSpeeds robotSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( - fieldSpeeds.vxMetersPerSecond, - fieldSpeeds.vyMetersPerSecond, - fieldSpeeds.omegaRadiansPerSecond, - robotPose.getRotation().unaryMinus() // Inverse rotation - ); +public Command TeleopDrive(CommandXboxController joystick, double MaxSpeed, double MaxAngularRate, SwerveRequest.FieldCentric drive, CommandSwerveDrivetrain drivetrain){ + return applyRequest(() -> { + // Apply 10% deadband to joystick inputs + double xSpeed = MathUtil.applyDeadband(-joystick.getLeftY(), 0.1); + double ySpeed = MathUtil.applyDeadband(-joystick.getLeftX(), 0.1); + double rotSpeed = MathUtil.applyDeadband(-joystick.getRightX(), 0.1); - // Step 2: Predict where ROBOT will be - double futureRobotX = robotPose.getX() + (robotSpeeds.vxMetersPerSecond); - double futureRobotY = robotPose.getY() + (robotSpeeds.vyMetersPerSecond); - Translation2d futureRobotPos = new Translation2d(futureRobotX, futureRobotY); + return alignRequest + .withVelocityX(xSpeed * MaxSpeed) + .withVelocityY(ySpeed * MaxSpeed) + .withRotationalRate(rotSpeed * MaxAngularRate); + }); +} + +public Command TeleopDriveSLOW(CommandXboxController joystick, double MaxSpeed, double MaxAngularRate, SwerveRequest.FieldCentric drive, CommandSwerveDrivetrain drivetrain){ + return applyRequest(() -> { + // Apply 10% deadband to joystick inputs + double xSpeed = MathUtil.applyDeadband(-joystick.getLeftY(), 0.1); + double ySpeed = MathUtil.applyDeadband(-joystick.getLeftX(), 0.1); + double rotSpeed = MathUtil.applyDeadband(-joystick.getRightX(), 0.1); - // Step 3: Calculate angle from future robot position to hub - Translation2d futureToHub = actualHub.getTranslation().minus(futureRobotPos); - futureDistance = futureToHub.getNorm(); - return futureDistance; - } + return alignRequest + .withVelocityX(xSpeed * MaxSpeed*0.5) + .withVelocityY(ySpeed * MaxSpeed*0.5) + .withRotationalRate(rotSpeed * MaxAngularRate); + }); +} //BASED ON MECH A PRAISE THE FRC GODS FOR OPEN ALLIANCE public Command shootOnTheMoveIterative(CommandXboxController controller, double maxSpeed, double maxAngularRate, String tuning) { @@ -563,20 +615,6 @@ public Command shootOnTheMoveIterative(CommandXboxController controller, double // Visualization ShootingLocation = new Pose2d(lookaheadShooterPosition, aimAngle); - // Debug - SmartDashboard.putNumber("Shot/Robot Vx", robotRelativeSpeeds.vxMetersPerSecond); - SmartDashboard.putNumber("Shot/Robot Vy", robotRelativeSpeeds.vyMetersPerSecond); - SmartDashboard.putNumber("Shot/Field Vx", fieldVelocity.vxMetersPerSecond); - SmartDashboard.putNumber("Shot/Field Vy", fieldVelocity.vyMetersPerSecond); - SmartDashboard.putNumber("Shot/Lookahead Distance", lookaheadDistance); - SmartDashboard.putNumber("Shot/Time of Flight", timeOfFlight); - SmartDashboard.putNumber("Shot/Current Angle", currentAngle.getDegrees()); - SmartDashboard.putNumber("Shot/Aim Angle", aimAngle.getDegrees()); - SmartDashboard.putNumber("Shot/Rotational Rate", rotationalRate); - SmartDashboard.putNumber("Shot/Prediction Offset X", lookaheadShooterPosition.getX() - currentPose.getX()); - SmartDashboard.putNumber("Shot/Prediction Offset Y", lookaheadShooterPosition.getY() - currentPose.getY()); - SmartDashboard.putNumber("Shot/Angle Error Deg", aimAngle.minus(currentAngle).getDegrees()); - return alignRequest .withVelocityX(veloX * maxSpeed) .withVelocityY(veloY * maxSpeed) @@ -589,9 +627,9 @@ public boolean isAimedAtTarget() { Rotation2d currentAngle = currentPose.getRotation(); // Calculate required aim angle (same as headingLocktoHub) - Translation2d target = getHubPose().toPose2d().getTranslation(); + Translation2d target = getTargetPose(currentPose).getTranslation(); Translation2d toTarget = target.minus(currentPose.getTranslation()); - Rotation2d targetAngle = toTarget.getAngle(); // Face toward hub + Rotation2d targetAngle = toTarget.getAngle(); // Face towards from hub .plus(Krot180) or sum idk // Calculate angle error double errorDegrees = Math.abs(targetAngle.minus(currentAngle).getDegrees()); @@ -600,6 +638,40 @@ public boolean isAimedAtTarget() { return errorDegrees <= Constants.DriveConstants.RotationalToleranceDegrees; } +public Command bumpLockCommand(SwerveRequest.FieldCentric drive, CommandSwerveDrivetrain drivetrain, CommandXboxController joystick, Double MaxSpeed, double MaxAngularRate){ + return applyRequest(() -> { + double closeTrench = (double)getCloseBumpY(drivetrain.getPose()).in(Meters); + double xSpeed = MathUtil.applyDeadband(-joystick.getLeftY(), 0.1); + distanceController.setSetpoint(closeTrench); + double yVel = distanceController.calculate(drivetrain.getPose().getY()); + if (distanceController.atSetpoint()) { + yVel = 0; + } + + Rotation2d currentRot = drivetrain.getState().Pose.getRotation(); + + // Returns whichever of 0 or 180 the robot is facing closest to + Rotation2d rotSetpoint = Math.abs(currentRot.getDegrees()) < 90 + ? Rotation2d.kZero // closer to 0 + : Rotation2d.fromDegrees(180); // closer to 180 + rotationController.setSetpoint(rotSetpoint.getRadians()); + + double rotSpeedToStraight = + rotationController.calculate(drivetrain.getPose().getRotation().getRadians()); + if (rotationController.atSetpoint()) { + rotSpeedToStraight = 0; + } + + if (DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red){ + yVel = -yVel; + } + return drive + .withVelocityX(xSpeed*MaxSpeed) // + .withVelocityY(yVel *MaxSpeed) + .withRotationalRate(rotSpeedToStraight*MaxAngularRate); + }); + } + public Command getSnakeDriveCommand(SwerveRequest.FieldCentric drive, CommandSwerveDrivetrain drivetrain, CommandXboxController joystick, Double MaxSpeed, double MaxAngularRate) { return applyRequest(() -> { // Get joystick inputs @@ -644,9 +716,12 @@ public Command getSnakeDriveCommand(SwerveRequest.FieldCentric drive, CommandSwe @Override public void periodic() { - SmartDashboard.putNumber("ShootingDistance", Constants.ShooterConstants.ShootingDistance*3.28084); //3.28084 Feet per Meter (converting Meters to Feet) if(ShootingLocation != null){field.getObject("Shooting Target").setPose(ShootingLocation);} field.setRobotPose(getPose()); + SmartDashboard.putNumber("distanceToCenterHubInches", getDistance() * 39.3701); + SmartDashboard.putString("close trench X val", getCloseBumpY(getPose()).toShortString()); + SmartDashboard.putNumber("Hub/OffsetX", hubOffsetX); + SmartDashboard.putNumber("Hub/OffsetY", hubOffsetY); /* * Periodically try to apply the operator perspective. diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java new file mode 100644 index 0000000..b273748 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -0,0 +1,299 @@ +/* Generated by Phoenix Tuner X */ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import java.util.Set; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.sim.ChassisReference; +import com.ctre.phoenix6.sim.TalonFXSimState; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.units.AngleUnit; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants; + +public class Elevator extends SubsystemBase { + /** Position setpoints for the elevator. */ + public enum Setpoint { + IN(Rotations.of(Constants.IntakeConstants.IntakeSlideInSetPoint)), //0 + Middle(Rotations.of(Constants.IntakeConstants.IntakeSlideMiddleSetPoint)), + OUT(Rotations.of(Constants.IntakeConstants.IntakeSlideOutSetPoint)); + + /** The position target of the setpoint in angular units. */ + public final Angle target; + /** The position target of the setpoint in linear units. */ + public final Distance targetDist; + + private Setpoint(Angle target) { + this.target = target; + this.targetDist = kDrumRadius.times(target.in(Radians)); + } + private Setpoint(Distance target) { + this.target = Radians.of(target.div(kDrumRadius).magnitude()); + this.targetDist = target; + } + } + + private static final int kNumConfigAttempts = 2; + + private static final double kGearRatio = 6; + private static final Distance kDrumRadius = Meters.of(0.0254); + private static final Distance kMaxHeight = Inches.of(13.3); + + /* leader and follower motors */ + private final CANBus kCANBus = new CANBus("Upper"); + private final TalonFX motor_id_35 = new TalonFX(Constants.IntakeConstants.IntakeSlideMotorID, "Upper"); + + /* device status signals */ + private final StatusSignal motor_id_35Position = motor_id_35.getPosition(false); + private final StatusSignal motor_id_35Velocity = motor_id_35.getVelocity(false); + private final StatusSignal motor_id_35TorqueCurrent = motor_id_35.getTorqueCurrent(false); + + /* controls used by the leader motors */ + private final MotionMagicVoltage setpointRequest = new MotionMagicVoltage(0); + private final DutyCycleOut manualRequest = new DutyCycleOut(0); + private final DutyCycleOut calibrationRequestIn = new DutyCycleOut(-0.1) + .withIgnoreHardwareLimits(true) + .withIgnoreSoftwareLimits(true); + private final DutyCycleOut calibrationRequestOut = new DutyCycleOut(0.1) + .withIgnoreHardwareLimits(true) + .withIgnoreSoftwareLimits(true); + + /** Trigger to detect when the elevator drives into a hard stop. */ + public final Trigger isHardStop = new Trigger(() -> { + return motor_id_35Velocity.getValue().abs(RotationsPerSecond) < 1 && + motor_id_35TorqueCurrent.getValue().abs(Amps) > 10; + }).debounce(0.1); + + /* simulation */ + private final ElevatorSim elevatorSim_motor_id_35 = new ElevatorSim( + DCMotor.getKrakenX60Foc(1), + kGearRatio, 5, kDrumRadius.in(Meters), + 0.0, kMaxHeight.in(Meters), false, 0.0 + ); + + private static final double kSimLoopPeriod = 0.002; // 2 ms + private Notifier simNotifier = null; + private double lastSimTime = 0.0; + + /* Mechanism2d visualization of the elevator */ + private final Mechanism2d mech2d = new Mechanism2d(1, kMaxHeight.in(Meters)); + private final MechanismLigament2d motor_id_35Mech2d = mech2d.getRoot("motor_id_35 Root", 0.500, 0.4) + .append(new MechanismLigament2d("motor_id_35", elevatorSim_motor_id_35.getPositionMeters(), 200)); + + /** Configs common across all motors. */ + private static final TalonFXConfiguration motorInitialConfigs = new TalonFXConfiguration(); + + /** Configs common across just the leader motors. */ + private static final TalonFXConfiguration leaderInitialConfigs = motorInitialConfigs.clone(); + + /** Configs for {@link #motor_id_35}. */ + private final TalonFXConfiguration motor_id_35Configs = leaderInitialConfigs.clone(); + + static { + // Configure MotionMagic for the leader motors + leaderInitialConfigs.Slot0.kP = 12.0; // Tune these values! 8 + leaderInitialConfigs.Slot0.kI = 0.0; + leaderInitialConfigs.Slot0.kD = 0.1; + + + leaderInitialConfigs.MotionMagic.MotionMagicCruiseVelocity = 50*6; // rps + leaderInitialConfigs.MotionMagic.MotionMagicAcceleration = 160*6; // rps/s + leaderInitialConfigs.MotionMagic.MotionMagicJerk = 1600*2; // rps/s/s + } + + public Elevator() { + for (int i = 0; i < kNumConfigAttempts; ++i) { + var status = motor_id_35.getConfigurator().apply(motor_id_35Configs); + if (status.isOK()) break; + } + + + /* set the default command to neutral output */ + setDefaultCommand(manualDrive(() -> 0.0)); + /* alternatively, the default command can hold position */ + // setDefaultCommand(holdPosition()); + + SmartDashboard.putData("Elevator", mech2d); + + if (Utils.isSimulation()) { + startSimThread(); + } + } + + /** + * @return The Position of the elevator + */ + public Angle getPosition() { + return motor_id_35Position.getValue(); + } + + /** + * @return The Velocity of the elevator + */ + public AngularVelocity getVelocity() { + return motor_id_35Velocity.getValue(); + } + + /** + * @return The TorqueCurrent of the elevator + */ + public Current getTorqueCurrent() { + return motor_id_35TorqueCurrent.getValue(); + } + + /** + * Holds the elevator at the current position using PID. + * + * @return Command to run + */ + public Command holdPosition() { + return runOnce(() -> + setpointRequest.withPosition(motor_id_35Position.getValue()) + ).andThen(run(() -> { + motor_id_35.setControl(setpointRequest); + })); + } + + /** + * Drives the elevator to the provided position setpoint. + * + * @param setpoint Function returning the setpoint to apply + * @return Command to run + */ + public Command goToSetpoint(Supplier setpoint) { + return run(() -> { + setpointRequest.withPosition(setpoint.get().target); + motor_id_35.setControl(setpointRequest); + }); + } + + /** + * Manually drives the elevator with the provided duty cycle output. + * + * @param manualOutput Function returning the duty cycle to apply + * @return Command to run + */ + public Command manualDrive(DoubleSupplier manualOutput) { + return run(() -> { + manualRequest.withOutput(manualOutput.getAsDouble()); + motor_id_35.setControl(manualRequest); + }); + } + + /** + * Recalibrates the elevator zero point. This slowly drives the elevator + * down until we see a drop in velocity and a spike in stator current, + * indicating that we've hit a hard stop.da + * + * @return Command to run + */ + public Command calibrateZeroIn() { + return run(() -> { + motor_id_35.setControl(calibrationRequestIn); + }) + .until(isHardStop) + .andThen( + manualDrive(() -> 0.0).withTimeout(0.25) + .finallyDo(() -> { + motor_id_35.setPosition(Rotations.of(0)); + }) + ); + } + + public Command calibrateZeroOut() { + return run(() -> { + motor_id_35.setControl(calibrationRequestOut); + }) + .until(isHardStop) + .andThen( + manualDrive(() -> 0.0).withTimeout(0.25) + .finallyDo(() -> { + motor_id_35.setPosition(Rotations.of(Constants.IntakeConstants.IntakeSlideOutHardStop)); + }) + ); + } + + public boolean isAtSetpoint(Elevator.Setpoint setpoint){ + return getPosition().isNear(setpoint.target, Rotations.of(0.2)); + } + + @Override + public void periodic() { + /* refresh all status signals */ + BaseStatusSignal.refreshAll( + motor_id_35Position, + motor_id_35Velocity, + motor_id_35TorqueCurrent + ); + + motor_id_35Mech2d.setLength( + motor_id_35Position.getValueAsDouble() * kDrumRadius.in(Meters) * 2 * Math.PI + ); + + SmartDashboard.putString("LintakePosition", getPosition().toString()); + SmartDashboard.putBoolean("LintakeAtSetpoint", isAtSetpoint(Elevator.Setpoint.IN)); + } + + private void startSimThread() { + motor_id_35.getSimState().Orientation = ChassisReference.CounterClockwise_Positive; + motor_id_35.getSimState().setMotorType(TalonFXSimState.MotorType.KrakenX60); + + lastSimTime = Utils.getCurrentTimeSeconds(); + + /* Run simulation at a faster rate so PID gains behave more reasonably */ + simNotifier = new Notifier(() -> { + /* Calculate the time delta */ + final double currentTime = Utils.getCurrentTimeSeconds(); + final double deltaTime = currentTime - lastSimTime; + lastSimTime = currentTime; + + final var motor_id_35Sim = motor_id_35.getSimState(); + + /* First set the supply voltage of all the devices */ + motor_id_35Sim.setSupplyVoltage(RobotController.getBatteryVoltage()); + + /* Then calculate the new position and velocity of the simulated elevator */ + elevatorSim_motor_id_35.setInputVoltage(motor_id_35Sim.getMotorVoltage()); + elevatorSim_motor_id_35.update(deltaTime); + + /* Apply the new rotor position and velocity to the motors (before gear ratio) */ + motor_id_35Sim.setRawRotorPosition( + Radians.of(elevatorSim_motor_id_35.getPositionMeters() / kDrumRadius.in(Meters) * kGearRatio) + ); + motor_id_35Sim.setRotorVelocity( + RadiansPerSecond.of(elevatorSim_motor_id_35.getVelocityMetersPerSecond() / kDrumRadius.in(Meters) * kGearRatio) + ); + }); + simNotifier.startPeriodic(kSimLoopPeriod); + } +} diff --git a/src/main/java/frc/robot/subsystems/HopperSubsystem.java b/src/main/java/frc/robot/subsystems/HopperSubsystem.java index 97e1fe1..5663a93 100644 --- a/src/main/java/frc/robot/subsystems/HopperSubsystem.java +++ b/src/main/java/frc/robot/subsystems/HopperSubsystem.java @@ -15,7 +15,7 @@ import edu.wpi.first.wpilibj2.command.Commands; public class HopperSubsystem extends SubsystemBase { - private final TalonFX hopperMotor = new TalonFX(Constants.HopperConstants.HopperMotorID); + private final TalonFX hopperMotor = new TalonFX(Constants.HopperConstants.HopperMotorID, "Upper"); /** Creates a new Intake. */ public HopperSubsystem() {} diff --git a/src/main/java/frc/robot/subsystems/IndexerHighSubsystem.java b/src/main/java/frc/robot/subsystems/IndexerHighSubsystem.java index 470478e..fe63441 100644 --- a/src/main/java/frc/robot/subsystems/IndexerHighSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IndexerHighSubsystem.java @@ -15,12 +15,11 @@ import edu.wpi.first.wpilibj2.command.Commands; public class IndexerHighSubsystem extends SubsystemBase { - private final TalonFX indexerHighMotor = new TalonFX(Constants.IndexerConstants.IndexerHighMotorID); + private final TalonFX indexerHighMotor = new TalonFX(Constants.IndexerConstants.HighIndexerMotorID, "Upper"); /** Creates a new Intake. */ public IndexerHighSubsystem() {} public void set(double speed){ - //System.out.print(speed); indexerHighMotor.set(speed); } diff --git a/src/main/java/frc/robot/subsystems/IndexerLowSubsystem.java b/src/main/java/frc/robot/subsystems/IndexerLowSubsystem.java index 6713cf0..78676d0 100644 --- a/src/main/java/frc/robot/subsystems/IndexerLowSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IndexerLowSubsystem.java @@ -15,12 +15,11 @@ import edu.wpi.first.wpilibj2.command.Commands; public class IndexerLowSubsystem extends SubsystemBase { - private final TalonFX indexerLowMotor = new TalonFX(Constants.IndexerConstants.IndexerLowMotorID); + private final TalonFX indexerLowMotor = new TalonFX(Constants.IndexerConstants.LowIndexerMotorID, "Upper"); /** Creates a new Intake. */ public IndexerLowSubsystem() {} public void set(double speed){ - //System.out.print(speed); indexerLowMotor.set(speed); } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 245bfc3..2e1fbd0 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -10,23 +10,24 @@ import edu.wpi.first.wpilibj.motorcontrol.MotorController; import edu.wpi.first.wpilibj.motorcontrol.Talon; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; + public class IntakeSubsystem extends SubsystemBase { - private final TalonFX intakeMotor = new TalonFX(Constants.IntakeConstants.IntakeMotorID); - private final TalonFX intakeFollowerMotor = new TalonFX(Constants.IntakeConstants.IntakeFollowerReversedMotorID); - /** Creates a new Intake. */ + private final TalonFX intakeMotor = new TalonFX(Constants.IntakeConstants.IntakeMotorID, "Upper"); + private final TalonFX intakeFollowerMotor = new TalonFX(Constants.IntakeConstants.IntakeFollowerReversedMotorID, "Upper"); + public IntakeSubsystem() { intakeFollowerMotor.setControl(new Follower(Constants.IntakeConstants.IntakeMotorID, MotorAlignmentValue.Opposed)); } - public void set(double speed){ - //System.out.print(speed); intakeMotor.set(speed); + SmartDashboard.putBoolean("ranSetMethod", true); //return Commands.run(() -> intakeMotor.set(speed)); } @@ -38,4 +39,4 @@ public void stop(){ public void periodic() { // This method will be called once per scheduler run } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java deleted file mode 100644 index 308b9fd..0000000 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ /dev/null @@ -1,270 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import static edu.wpi.first.units.Units.*; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; - -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -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.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.math.system.plant.DCMotor; -import frc.robot.Constants; -import frc.robot.handlers.IntakeHandler; -import frc.robot.handlers.Superstructure.SuperstructureState; -import yams.gearing.GearBox; -import yams.gearing.MechanismGearing; -import yams.mechanisms.SmartMechanism; -import yams.motorcontrollers.SmartMotorControllerConfig; -import edu.wpi.first.units.measure.AngularVelocity; -import yams.motorcontrollers.SmartMotorController; -import yams.mechanisms.config.FlyWheelConfig; -import yams.mechanisms.velocity.FlyWheel; -import yams.motorcontrollers.SmartMotorControllerConfig.ControlMode; -import yams.motorcontrollers.SmartMotorControllerConfig.MotorMode; -import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity; -import yams.motorcontrollers.remote.TalonFXWrapper; -import com.ctre.phoenix6.sim.TalonFXSimState; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.StrictFollower; -import com.ctre.phoenix6.controls.VelocityVoltage; - -import edu.wpi.first.wpilibj.smartdashboard.*; -import com.ctre.phoenix6.signals.MotorAlignmentValue; - - -public class ShooterSubsystem extends SubsystemBase { - - - private SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(this) - .withControlMode(ControlMode.CLOSED_LOOP) - // Feedback Constants (PID Constants) - .withClosedLoopController(0.48, 0, 0, DegreesPerSecond.of(180), DegreesPerSecondPerSecond.of(45)) - .withSimClosedLoopController(0.48, 0, 0, DegreesPerSecond.of(180), DegreesPerSecondPerSecond.of(45)) - // Feedforward Constants - .withFeedforward(new SimpleMotorFeedforward(0, 0.19, 1.13)) - .withSimFeedforward(new SimpleMotorFeedforward(0, 0.19, 1.13)) - // Telemetry name and verbosity level - .withTelemetry("ShooterMotor", TelemetryVerbosity.HIGH) - // Gearing from the motor rotor to final shaft. - // In this example GearBox.fromReductionStages(3,4) is the same as GearBox.fromStages("3:1","4:1") which corresponds to the gearbox attached to your motor. - // You could also use .withGearing(12) which does the same thing. - .withGearing(new MechanismGearing(GearBox.fromReductionStages(1))) - // Motor properties to prevent over currenting. - .withMotorInverted(false) - .withIdleMode(MotorMode.COAST) - .withStatorCurrentLimit(Amps.of(40)); - - // Vendor motor controller object - private TalonFX shooterLeader = new TalonFX(Constants.ShooterConstants.ShooterLeaderID); - private TalonFX shooterFollower = new TalonFX(Constants.ShooterConstants.ShooterFollowerID); - private TalonFX shooterFollowerReversed = new TalonFX(Constants.ShooterConstants.ShooterFollowerReversedID); - private TalonFX shooterFollowerReversed2 = new TalonFX(Constants.ShooterConstants.ShooterFollowerReversed2ID); - - private TalonFXSimState shooterMotorSim = shooterLeader.getSimState(); - - // Create our SmartMotorController from our Spark and config with the NEO. - private SmartMotorController SmartMotorController = new TalonFXWrapper(shooterLeader, DCMotor.getKrakenX60(4), smcConfig); - - - private final FlyWheelConfig shooterConfig = new FlyWheelConfig(SmartMotorController) - // Diameter of the flywheel. - .withDiameter(Inches.of(4)) - // Mass of the flywheel. - .withMass(Pounds.of(5.2)) //CHangle later maybe - // Maximum speed of the shooter. - .withUpperSoftLimit(RPM.of(3200)) - // Telemetry name and verbosity for the arm. - .withTelemetry("ShooterMech", TelemetryVerbosity.HIGH); - - // Shooter Mechanism - private FlyWheel shooter = new FlyWheel(shooterConfig); - - - AngularVelocity setVelo = RPM.of(0); - /** - * Gets the current velocity of the shooter. - * - * @return Shooter velocity. - */ - public AngularVelocity getVelocity() { - return shooter.getSpeed();} - - public Command stop() { - setVelo = RPM.of(0); - return shooter.set(0);} - - /** - * Set the shooter velocity WITHOUT creating a command. - * Use this for continuous updates (like distance-based shooting). - * - * @param speed Speed to set. - */ - public void setVelocityDirect(AngularVelocity speed) { - if (speed.gte(Constants.ShooterConstants.SetRPMHardStop)) {speed = Constants.ShooterConstants.SetRPMHardStop;} - System.out.println("new speed" + speed); - setVelo = speed; - } - - /** - * Set the shooter velocity. - * - * @param speed Speed to set. - * @return {@link edu.wpi.first.wpilibj2.command.RunCommand} - */ - public Command setVelocity(AngularVelocity speed) { - if (speed.gte(Constants.ShooterConstants.SetRPMHardStop)){ speed = Constants.ShooterConstants.SetRPMHardStop;} - setVelo = speed; - return shooter.setSpeed(speed);} - - - /** - * Set the dutycycle of the shooter. - * - * @param dutyCycle DutyCycle to set. - * @return {@link edu.wpi.first.wpilibj2.command.RunCommand} - */ - public Command set(double dutyCycle) { return shooter.set(dutyCycle);} - - private CommandSwerveDrivetrain drivetrain; - - public ShooterSubsystem() { - - //-------------COMMENT ALL THIS OUT IF TESTING WITH PHYNEX TUNER----------------- - shooterFollower.setControl(new Follower(Constants.ShooterConstants.ShooterLeaderID, MotorAlignmentValue.Aligned)); - - // 2. Reversed Followers (Opposite direction of leader) - shooterFollowerReversed.setControl(new Follower(Constants.ShooterConstants.ShooterLeaderID, MotorAlignmentValue.Opposed)); - shooterFollowerReversed2.setControl(new Follower(Constants.ShooterConstants.ShooterLeaderID, MotorAlignmentValue.Opposed)); - } - - /** Creates a new ShooterSubsystem. with drivetrain perameters */ - public ShooterSubsystem(CommandSwerveDrivetrain drivetrin) { - //-------------COMMENT ALL THIS OUT IF TESTING WITH PHYNEX TUNER----------------- - shooterFollower.setControl(new Follower(Constants.ShooterConstants.ShooterLeaderID, MotorAlignmentValue.Aligned)); - - // 2. Reversed Followers (Opposite direction of leader) - // Use 'true' for the opposeLeader parameter - shooterFollowerReversed.setControl(new Follower(Constants.ShooterConstants.ShooterLeaderID, MotorAlignmentValue.Opposed)); - shooterFollowerReversed2.setControl(new Follower(Constants.ShooterConstants.ShooterLeaderID, MotorAlignmentValue.Opposed)); - } - private static ShooterSubsystem instance; - public static ShooterSubsystem getInstance(){ - if (instance == null){ - instance = new ShooterSubsystem(); - } - return instance; - } - - public void initialize(CommandSwerveDrivetrain drivetrain) { - this.drivetrain = drivetrain; - } - - //Checking if shooter RPM is at threashold (waiting for it to spinup) - public boolean isAtTargetVelo(){ - double currentRPM = getVelocity().in(RPM); - double targetRPM = setVelo.in(RPM); - - SmartDashboard.putBoolean("Shooter/At Target", currentRPM >= targetRPM); - - return currentRPM >= targetRPM; - } - - public AngularVelocity getCalcedRPM(double DistMeters){ - - //double distanceMeters = drivetrain.getDistance(); - double distanceInches = DistMeters * 39.3701; - - AngularVelocity velo = RPM.of(-0.0312466 * Math.pow(distanceInches, 2) + 29.07009 * distanceInches + 828.29202); - - if (velo.gte(Constants.ShooterConstants.SetRPMHardStop)){ velo = Constants.ShooterConstants.SetRPMHardStop;} - setVelo = velo; - return velo; -} - - public Command setIdleMode(Pose2d RobotPose){ - Pose2d currentPose = RobotPose; - double robotX = currentPose.getX(); - - // Define field zones (adjust these values based on your field layout) - // Assuming field is ~16.5 meters long (54 feet) - double ourZoneMax = 5.5; // meters - our third of the field - double middleZoneMax = 11.0; // meters - middle third - // Anything beyond middleZoneMax is "their" zone - - // Determine which zone we're in and set appropriate state - if (robotX < ourZoneMax) { - return setVelocity(Constants.ShooterConstants.SlowShot); - } else if (robotX < middleZoneMax) { - return stop(); - } else { - return stop(); - } -} - -public boolean isReadyToShoot() { - // Check if at target velocity and drivetrain is aimed - return isAtTargetVelo() && drivetrain != null && isAimedAtTarget(); -} - -public boolean isAimedAtTarget() { - if (drivetrain == null) return false; - - Pose2d robotPose = drivetrain.getPose(); - Pose2d targetPose = CommandSwerveDrivetrain.getHubPose().toPose2d(); - - // Calculate angle error - Translation2d toTarget = targetPose.getTranslation().minus(robotPose.getTranslation()); - Rotation2d targetAngle = toTarget.getAngle().rotateBy(Rotation2d.k180deg); - Rotation2d currentAngle = robotPose.getRotation(); - - double errorDegrees = Math.abs(targetAngle.minus(currentAngle).getDegrees()); - - SmartDashboard.putNumber("Shooter/Aim Error (deg)", errorDegrees); - - return errorDegrees < 2.0; // Within 2 degrees -} - - - - @Override - public void periodic() { - - SmartDashboard.putNumber("Request RPM", setVelo.in(RPM)); - SmartDashboard.putNumber("Real RPM", getVelocity().in(RPM)); - - // This method will be called once per scheduler run - shooter.updateTelemetry(); - } - -@Override -public void simulationPeriodic() { - // This method will be called once per scheduler run during simulation - shooter.simIterate(); - //SmartDashboard.putNumber("Request RPM", setVelo.in(RPM)); - //SmartDashboard.putNumber("Real RPM", getVelocity().in(RPM)); - - - - //System.out.println("Sim periodic running - Leader voltage: " + shooterLeader.getSimState().getMotorVoltage()); - // Log voltages for all motors - double leaderVoltage = shooterLeader.getSimState().getMotorVoltage(); - shooterFollower.getSimState().setSupplyVoltage(leaderVoltage); - shooterFollowerReversed.getSimState().setSupplyVoltage(leaderVoltage); - shooterFollowerReversed2.getSimState().setSupplyVoltage(leaderVoltage); - - SmartDashboard.putNumber("Shooter/Leader Voltage", shooterLeader.getSimState().getMotorVoltage()); - SmartDashboard.putNumber("Shooter/Follower Voltage", shooterFollower.getSimState().getMotorVoltage()); - SmartDashboard.putNumber("Shooter/FollowerReversed Voltage", shooterFollowerReversed.getSimState().getMotorVoltage()); - SmartDashboard.putNumber("Shooter/FollowerReversed2 Voltage", shooterFollowerReversed2.getSimState().getMotorVoltage()); - } -} diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 31ce073..cd33b7b 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -18,14 +18,20 @@ public Vision(CommandSwerveDrivetrain drivetrain) { * Updates pose estimator with Limelight vision measurements * Called automatically in periodic(). */ - - private final String PosLimelight = "limelight-front"; - public void updateVisionMeasurements() { + + // All Pose estimating limelights names + private final String[] PosLimelights = { + "limelight-fl", + "limelight-fr" + // add more as needed + }; + + public void updateVisionMeasurements(String LLName) { boolean doRejectUpdate = false; if (!useMegaTag2) { // MegaTag1 mode - LimelightHelpers.PoseEstimate mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(PosLimelight); + LimelightHelpers.PoseEstimate mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(LLName); if (mt1 == null){ return; @@ -58,12 +64,12 @@ public void updateVisionMeasurements() { // MegaTag2 mode // Set robot orientation for MegaTag2 LimelightHelpers.SetRobotOrientation( - PosLimelight, + LLName, drivetrain.getState().Pose.getRotation().getDegrees(), 0, 0, 0, 0, 0 ); - LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(PosLimelight); + LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(LLName); if (mt2 == null){ return; @@ -80,9 +86,6 @@ public void updateVisionMeasurements() { } if (!doRejectUpdate) { - /* - drivetrain.setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.9));//9999999 to 0.9 - drivetrain.addVisionMeasurement(mt2.pose, mt2.timestampSeconds); */ // Add vision measurement to drivetrain drivetrain.addVisionMeasurement( mt2.pose, @@ -93,8 +96,11 @@ public void updateVisionMeasurements() { } } + @Override public void periodic() { - updateVisionMeasurements(); + for (String limeLight : PosLimelights) { + updateVisionMeasurements(limeLight); + } } } \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/HubShiftUtil.java b/src/main/java/frc/robot/utils/HubShiftUtil.java new file mode 100644 index 0000000..fd04a20 --- /dev/null +++ b/src/main/java/frc/robot/utils/HubShiftUtil.java @@ -0,0 +1,190 @@ +package frc.robot.utils; + +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import java.util.Optional; +import java.util.function.Supplier; +import frc.robot.Constants; + +public class HubShiftUtil { + public enum ShiftEnum { + TRANSITION, + + SHIFT1, + SHIFT2, + SHIFT3, + SHIFT4, + ENDGAME, + AUTO, + DISABLED; + } + + public record ShiftInfo( + ShiftEnum currentShift, double elapsedTime, double remainingTime, boolean active) {} + + private static Timer shiftTimer = new Timer(); + private static final ShiftEnum[] shiftsEnums = ShiftEnum.values(); + + private static final double[] shiftStartTimes = {0.0, 10.0, 35.0, 60.0, 85.0, 110.0}; + private static final double[] shiftEndTimes = {10.0, 35.0, 60.0, 85.0, 110.0, 140.0}; + + private static final double minFuelCountDelay = 1.0; + private static final double maxFuelCountDelay = 2.0; + private static final double minTimeOfFlight = 1.0; //made ts up + private static final double maxTimeOfFlight = 3.0; //made ts up + private static final double shiftEndFuelCountExtension = 3.0; + private static final double approachingActiveFudge = -1 * (minTimeOfFlight + minFuelCountDelay); + private static final double endingActiveFudge = shiftEndFuelCountExtension + -1 * (maxTimeOfFlight + maxFuelCountDelay); + + public static final double autoEndTime = 20.0; + public static final double teleopDuration = 140.0; + private static final boolean[] activeSchedule = {true, true, false, true, false, true}; + private static final boolean[] inactiveSchedule = {true, false, true, false, true, true}; + + + private static Supplier> allianceWinOverride = () -> Optional.empty(); + + public static void setAllianceWinOverride(Supplier> override) { + allianceWinOverride = override; + } + + public static Optional getAllianceWinOverride() { + return allianceWinOverride.get(); + } + + public static Alliance getFirstActiveAlliance() { + var alliance = DriverStation.getAlliance().orElse(Alliance.Blue); + + var winOverride = getAllianceWinOverride(); + if (!winOverride.isEmpty()) { + return winOverride.get() + ? (alliance == Alliance.Blue ? Alliance.Red : Alliance.Blue) + : (alliance == Alliance.Blue ? Alliance.Blue : Alliance.Red); + } + + String message = DriverStation.getGameSpecificMessage(); + if (message.length() > 0) { + char character = message.charAt(0); + if (character == 'R') { + return Alliance.Blue; + } else if (character == 'B') { + return Alliance.Red; + } + } + + return alliance == Alliance.Blue ? Alliance.Red : Alliance.Blue; + } + + public static void initialize() { + shiftTimer.restart(); + } + + private static boolean[] getSchedule() { + boolean[] currentSchedule; + Alliance startAlliance = getFirstActiveAlliance(); + currentSchedule = + startAlliance == DriverStation.getAlliance().orElse(Alliance.Blue) + ? activeSchedule + : inactiveSchedule; + return currentSchedule; + } + + private static ShiftInfo getShiftInfo( + boolean[] currentSchedule, double[] shiftStartTimes, double[] shiftEndTimes) { + double currentTime = shiftTimer.get(); + double stateTimeElapsed = shiftTimer.get(); + double stateTimeRemaining = 0.0; + boolean active = false; + ShiftEnum currentShift = ShiftEnum.DISABLED; + + if (DriverStation.isAutonomousEnabled()) { + stateTimeElapsed = currentTime; + stateTimeRemaining = autoEndTime - currentTime; + active = true; + currentShift = ShiftEnum.AUTO; + } else if (DriverStation.isEnabled() || RobotBase.isSimulation()) { + int currentShiftIndex = -1; + for (int i = 0; i < shiftStartTimes.length; i++) { + if (currentTime >= shiftStartTimes[i] && currentTime < shiftEndTimes[i]) { + currentShiftIndex = i; + break; + } + } + if (currentShiftIndex < 0) { + currentShiftIndex = shiftStartTimes.length - 1; + } + + stateTimeElapsed = currentTime - shiftStartTimes[currentShiftIndex]; + stateTimeRemaining = shiftEndTimes[currentShiftIndex] - currentTime; + + if (currentShiftIndex > 0) { + if (currentSchedule[currentShiftIndex] == currentSchedule[currentShiftIndex - 1]) { + stateTimeElapsed = currentTime - shiftStartTimes[currentShiftIndex - 1]; + } + } + + if (currentShiftIndex < shiftEndTimes.length - 1) { + if (currentSchedule[currentShiftIndex] == currentSchedule[currentShiftIndex + 1]) { + stateTimeRemaining = shiftEndTimes[currentShiftIndex + 1] - currentTime; + } + } + + active = currentSchedule[currentShiftIndex]; + currentShift = shiftsEnums[currentShiftIndex]; + } + ShiftInfo shiftInfo = new ShiftInfo(currentShift, stateTimeElapsed, stateTimeRemaining, active); + return shiftInfo; + } + + public static ShiftInfo getOfficialShiftInfo() { + return getShiftInfo(getSchedule(), shiftStartTimes, shiftEndTimes); + } + + public static ShiftInfo getShiftedShiftInfo() { + boolean[] shiftSchedule = getSchedule(); + if (shiftSchedule[1] == true) { + double[] shiftedShiftStartTimes = { + 0.0, + 10.0, + 35.0 + endingActiveFudge, + 60.0 + approachingActiveFudge, + 85.0 + endingActiveFudge, + 110.0 + approachingActiveFudge + }; + double[] shiftedShiftEndTimes = { + 10.0, + 35.0 + endingActiveFudge, + 60.0 + approachingActiveFudge, + 85.0 + endingActiveFudge, + 110.0 + approachingActiveFudge, + 140.0 + }; + return getShiftInfo(shiftSchedule, shiftedShiftStartTimes, shiftedShiftEndTimes); + } + double[] shiftedShiftStartTimes = { + 0.0, + 10.0 + endingActiveFudge, + 35.0 + approachingActiveFudge, + 60.0 + endingActiveFudge, + 85.0 + approachingActiveFudge, + 110.0 + }; + double[] shiftedShiftEndTimes = { + 10.0 + endingActiveFudge, + 35.0 + approachingActiveFudge, + 60.0 + endingActiveFudge, + 85.0 + approachingActiveFudge, + 110.0, + 140.0 + }; + return getShiftInfo(shiftSchedule, shiftedShiftStartTimes, shiftedShiftEndTimes); + } + + public static double getMatchTime(){ + return 140.0-shiftTimer.get(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/RumbleUtils.java b/src/main/java/frc/robot/utils/RumbleUtils.java new file mode 100644 index 0000000..208eac0 --- /dev/null +++ b/src/main/java/frc/robot/utils/RumbleUtils.java @@ -0,0 +1,24 @@ +package frc.robot.utils; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; + +public class RumbleUtils { + + /** + * Creates a command that rumbles a controller for a set duration. + * + * @param controller The controller to rumble + * @param strength Rumble strength from 0.0 to 1.0 + * @param seconds How long to rumble in seconds + * @return Command that can be scheduled + */ + public static Command rumble(CommandXboxController controller, double strength, double seconds) { + return Commands.startEnd( + () -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, strength), + () -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0.0) + ).withTimeout(seconds); + } +} \ No newline at end of file