diff --git a/build.gradle b/build.gradle index 9e5e749c..479ffb42 100644 --- a/build.gradle +++ b/build.gradle @@ -119,6 +119,7 @@ spotless { format "misc", { target "*" targetExclude "simgui*.json", "**/build/**", "**/build-*/**", "**/bin/**", "/.*" + toggleOffOn() indentWithTabs() endWithNewline() trimTrailingWhitespace() diff --git a/src/main/deploy/pathplanner/BumpScoreHighLeaveComScoreHighBlue.path b/src/main/deploy/pathplanner/BumpScoreHighLeaveComScoreHighBlue.path new file mode 100644 index 00000000..94d6819e --- /dev/null +++ b/src/main/deploy/pathplanner/BumpScoreHighLeaveComScoreHighBlue.path @@ -0,0 +1,97 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.8869415516608812, + "y": 1.073401804923269 + }, + "prevControl": null, + "nextControl": { + "x": 3.291499232571115, + "y": 0.7879226014862292 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "ScoreHigh", + "Stow" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 7.0484055498025535, + "y": 0.6965692563863765 + }, + "prevControl": { + "x": 4.855925267406091, + "y": 0.5709584068740797 + }, + "nextControl": { + "x": 4.855925267406091, + "y": 0.5709584068740797 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.7841690384235467, + "y": 0.3083175397120033 + }, + "prevControl": { + "x": 4.199127372967132, + "y": 0.308317539712003 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "ScoreHigh", + "Stow" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "isReversed": null, + "markers": [ + { + "position": 0.6545454545454215, + "names": [ + "WristPrescore", + "IntakeIn" + ] + }, + { + "position": 1.076363636363649, + "names": [ + "Stow" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/BumpScoreHighLeaveComScoreHighRed.path b/src/main/deploy/pathplanner/BumpScoreHighLeaveComScoreHighRed.path new file mode 100644 index 00000000..c8c4109e --- /dev/null +++ b/src/main/deploy/pathplanner/BumpScoreHighLeaveComScoreHighRed.path @@ -0,0 +1,97 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.8869415516608812, + "y": 1.073401804923269 + }, + "prevControl": null, + "nextControl": { + "x": 3.291499232571115, + "y": 0.7879226014862292 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "ScoreHigh", + "Stow" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.9456330365652175, + "y": 0.7650842652112657 + }, + "prevControl": { + "x": 4.753152754168755, + "y": 0.639473415698969 + }, + "nextControl": { + "x": 4.753152754168755, + "y": 0.639473415698969 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.8298457109734734, + "y": 0.35399421226193023 + }, + "prevControl": { + "x": 4.244804045517059, + "y": 0.3539942122619299 + }, + "nextControl": null, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "ScoreHigh", + "Stow" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "isReversed": null, + "markers": [ + { + "position": 0.6545454545454215, + "names": [ + "WristPrescore", + "IntakeIn" + ] + }, + { + "position": 1.076363636363649, + "names": [ + "Stow" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/FlatScoreHighLeaveComScoreHighBlue.path b/src/main/deploy/pathplanner/FlatScoreHighLeaveComScoreHighBlue.path new file mode 100644 index 00000000..2b0d4150 --- /dev/null +++ b/src/main/deploy/pathplanner/FlatScoreHighLeaveComScoreHighBlue.path @@ -0,0 +1,148 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 1.872484505064023, + "y": 4.42702975667234 + }, + "prevControl": null, + "nextControl": { + "x": 4.216451851707123, + "y": 5.092948989316782 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "ScoreHigh", + "Stow" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.335800642560605, + "y": 4.608452855778005 + }, + "prevControl": { + "x": 4.902482564852144, + "y": 4.636363444051799 + }, + "nextControl": { + "x": 6.146291266941508, + "y": 4.556248086855149 + }, + "holonomicAngle": 0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.774345514502994, + "y": 4.430637237342851 + }, + "prevControl": { + "x": 5.565310406248909, + "y": 4.410312015295236 + }, + "nextControl": { + "x": 5.565310406248909, + "y": 4.410312015295236 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": 0.5, + "isLocked": false, + "isStopPoint": true, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 4.010906825232454, + "y": 4.990176476079449 + }, + "prevControl": { + "x": 4.821667762993646, + "y": 4.944499803529522 + }, + "nextControl": { + "x": 2.7901381718127016, + "y": 5.058952174863661 + }, + "holonomicAngle": -180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.7499115340111024, + "y": 5.035853148629374 + }, + "prevControl": { + "x": 3.0402775335465195, + "y": 5.035853148629374 + }, + "nextControl": null, + "holonomicAngle": -180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "ScoreHigh", + "WristIn", + "Stow" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "isReversed": null, + "markers": [ + { + "position": 0.6109090909090871, + "names": [ + "WristPrescore", + "IntakeIn" + ] + }, + { + "position": 2.2400000000000047, + "names": [ + "Stow" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/FlatScoreHighLeaveComScoreHigh.path b/src/main/deploy/pathplanner/FlatScoreHighLeaveComScoreHighRed.path similarity index 90% rename from src/main/deploy/pathplanner/FlatScoreHighLeaveComScoreHigh.path rename to src/main/deploy/pathplanner/FlatScoreHighLeaveComScoreHighRed.path index bab31fa7..dfae5bf8 100644 --- a/src/main/deploy/pathplanner/FlatScoreHighLeaveComScoreHigh.path +++ b/src/main/deploy/pathplanner/FlatScoreHighLeaveComScoreHighRed.path @@ -78,15 +78,15 @@ { "anchorPoint": { "x": 4.010906825232454, - "y": 4.990176476079449 + "y": 4.8874039628421135 }, "prevControl": { - "x": 4.821667762993646, - "y": 4.944499803529522 + "x": 4.821321564400836, + "y": 4.8359490587679295 }, "nextControl": { - "x": 2.7901381718127016, - "y": 5.058952174863661 + "x": 3.291499232571115, + "y": 4.933080635392041 }, "holonomicAngle": -180.0, "isReversal": false, @@ -102,12 +102,12 @@ }, { "anchorPoint": { - "x": 1.73, - "y": 5.0815298211793 + "x": 1.83, + "y": 4.933080635392041 }, "prevControl": { - "x": 3.020365999535417, - "y": 5.0815298211793 + "x": 2.6978567784485996, + "y": 4.933080635392041 }, "nextControl": null, "holonomicAngle": -180.0, @@ -127,7 +127,7 @@ } } ], - "maxVelocity": 4.0, + "maxVelocity": 2.0, "maxAcceleration": 2.0, "isReversed": null, "markers": [ diff --git a/src/main/deploy/pathplanner/FlatScoreMidLeaveComScoreLowScoreLowBackwardsBlue.path b/src/main/deploy/pathplanner/FlatScoreMidLeaveComScoreLowScoreLowBackwardsBlue.path new file mode 100644 index 00000000..9976ec22 --- /dev/null +++ b/src/main/deploy/pathplanner/FlatScoreMidLeaveComScoreLowScoreLowBackwardsBlue.path @@ -0,0 +1,172 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 2.17, + "y": 4.42702975667234 + }, + "prevControl": null, + "nextControl": { + "x": 3.748265958070378, + "y": 4.967338139804485 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "WristShoot", + "IntakeFastOut" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.865698859602848, + "y": 3.9852896799810695 + }, + "prevControl": { + "x": 5.461141178692614, + "y": 4.579086423130116 + }, + "nextControl": { + "x": 5.461141178692614, + "y": 4.579086423130116 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": 0.5, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.521528171261471, + "y": 4.5905055912675925 + }, + "prevControl": { + "x": 5.586752028204912, + "y": 4.853146458429669 + }, + "nextControl": { + "x": 5.586752028204912, + "y": 4.853146458429669 + }, + "holonomicAngle": 0.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "IntakeFastOut", + "Stow" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.591638824303289, + "y": 3.11743290153247 + }, + "prevControl": { + "x": 5.735201213992172, + "y": 4.316445555968033 + }, + "nextControl": { + "x": 5.735201213992172, + "y": 4.316445555968033 + }, + "holonomicAngle": -35.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.567204843811397, + "y": 4.076643025080922 + }, + "prevControl": { + "x": 5.289853656630389, + "y": 4.910242299117077 + }, + "nextControl": null, + "holonomicAngle": 19.502373594688603, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "isReversed": null, + "markers": [ + { + "position": 0.61, + "names": [ + "WristPrescore", + "IntakeIn" + ] + }, + { + "position": 1.2400000000000002, + "names": [ + "Stow" + ] + }, + { + "position": 1.9, + "names": [ + "IntakeFastOut" + ] + }, + { + "position": 2.2981818181818277, + "names": [ + "WristPrescore", + "IntakeIn" + ] + }, + { + "position": 3.3, + "names": [ + "Stow" + ] + }, + { + "position": 3.8499999999999996, + "names": [ + "IntakeFastOut" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/FlatScoreMidLeaveComScoreLowScoreLowBackwardsInside.path b/src/main/deploy/pathplanner/FlatScoreMidLeaveComScoreLowScoreLowBackwardsInside.path new file mode 100644 index 00000000..5692e5a2 --- /dev/null +++ b/src/main/deploy/pathplanner/FlatScoreMidLeaveComScoreLowScoreLowBackwardsInside.path @@ -0,0 +1,197 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 2.17, + "y": 4.42702975667234 + }, + "prevControl": null, + "nextControl": { + "x": 4.513967346643099, + "y": 5.092948989316782 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "WristScore", + "IntakeFastOut" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 5.209919479668018, + "y": 4.6 + }, + "prevControl": { + "x": 4.775703453470522, + "y": 4.6 + }, + "nextControl": { + "x": 6.022089652789166, + "y": 4.6 + }, + "holonomicAngle": 0.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.488866311065955, + "y": 4.4077989010678875 + }, + "prevControl": { + "x": 5.1985003115305375, + "y": 5.035853148629374 + }, + "nextControl": { + "x": 5.1985003115305375, + "y": 5.035853148629374 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": 0.5, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.9326182242108072, + "y": 4.533409750580185 + }, + "prevControl": { + "x": 3.3143375688460774, + "y": 4.727535608917372 + }, + "nextControl": { + "x": 3.3143375688460774, + "y": 4.727535608917372 + }, + "holonomicAngle": 5.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "IntakeFastOut", + "Stow" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.534542983615882, + "y": 3.448588777519436 + }, + "prevControl": { + "x": 5.483979514967578, + "y": 4.830118598698736 + }, + "nextControl": { + "x": 5.483979514967578, + "y": 4.830118598698736 + }, + "holonomicAngle": -35.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.8869415516608812, + "y": 4.053804688805959 + }, + "prevControl": { + "x": 2.948924188446668, + "y": 5.058691484904338 + }, + "nextControl": null, + "holonomicAngle": 15.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "isReversed": null, + "markers": [ + { + "position": 0.61, + "names": [ + "WristPrescore", + "IntakeIn" + ] + }, + { + "position": 2.24, + "names": [ + "Stow" + ] + }, + { + "position": 2.75, + "names": [ + "IntakeFastOut" + ] + }, + { + "position": 3.05, + "names": [ + "WristPrescore", + "IntakeIn" + ] + }, + { + "position": 4.3, + "names": [ + "Stow" + ] + }, + { + "position": 4.6, + "names": [ + "IntakeFastOut" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/FlatScoreMidLeaveComScoreLowScoreLowBackwardsRed.path b/src/main/deploy/pathplanner/FlatScoreMidLeaveComScoreLowScoreLowBackwardsRed.path new file mode 100644 index 00000000..1d0df278 --- /dev/null +++ b/src/main/deploy/pathplanner/FlatScoreMidLeaveComScoreLowScoreLowBackwardsRed.path @@ -0,0 +1,172 @@ +{ + "waypoints": [ + { + "anchorPoint": { + "x": 2.17, + "y": 4.42702975667234 + }, + "prevControl": null, + "nextControl": { + "x": 3.491286619811728, + "y": 5.211911374565391 + }, + "holonomicAngle": 180.0, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "WristShoot", + "IntakeFastOut" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.625896328715734, + "y": 4.327864724105517 + }, + "prevControl": { + "x": 4.810248594856164, + "y": 4.339283892242999 + }, + "nextControl": { + "x": 4.810248594856164, + "y": 4.339283892242999 + }, + "holonomicAngle": 0, + "isReversal": true, + "velOverride": 0.5, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.6699773570487313, + "y": 4.761793113329817 + }, + "prevControl": { + "x": 5.803716222817061, + "y": 4.830308122154706 + }, + "nextControl": { + "x": 5.803716222817061, + "y": 4.830308122154706 + }, + "holonomicAngle": 0.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [ + "IntakeFastOut", + "Stow" + ], + "executionBehavior": "sequential", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 6.785764682640477, + "y": 3.2316245829072856 + }, + "prevControl": { + "x": 5.92932707232936, + "y": 4.430637237342851 + }, + "nextControl": { + "x": 5.92932707232936, + "y": 4.430637237342851 + }, + "holonomicAngle": -35.0, + "isReversal": true, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + }, + { + "anchorPoint": { + "x": 1.6357198526362868, + "y": 4.1109005294933665 + }, + "prevControl": { + "x": 6.066357089979137, + "y": 5.344170688341377 + }, + "nextControl": null, + "holonomicAngle": 19.502373594688603, + "isReversal": false, + "velOverride": null, + "isLocked": false, + "isStopPoint": false, + "stopEvent": { + "names": [], + "executionBehavior": "parallel", + "waitBehavior": "none", + "waitTime": 0 + } + } + ], + "maxVelocity": 2.0, + "maxAcceleration": 2.0, + "isReversed": null, + "markers": [ + { + "position": 0.61, + "names": [ + "WristPrescore", + "IntakeIn" + ] + }, + { + "position": 1.2400000000000002, + "names": [ + "Stow" + ] + }, + { + "position": 1.95, + "names": [ + "IntakeFastOut" + ] + }, + { + "position": 2.385454545454558, + "names": [ + "WristPrescore", + "IntakeIn" + ] + }, + { + "position": 3.3, + "names": [ + "Stow" + ] + }, + { + "position": 3.95, + "names": [ + "IntakeFastOut" + ] + } + ] +} \ No newline at end of file diff --git a/src/main/java/frc/team2412/robot/Robot.java b/src/main/java/frc/team2412/robot/Robot.java index 9f74c241..053e6b3a 100644 --- a/src/main/java/frc/team2412/robot/Robot.java +++ b/src/main/java/frc/team2412/robot/Robot.java @@ -4,24 +4,20 @@ import com.pathplanner.lib.auto.SwerveAutoBuilder; import com.pathplanner.lib.server.PathPlannerServer; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.DataLogManager; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.PowerDistribution; +import edu.wpi.first.wpilibj.*; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.livewindow.LiveWindow; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; 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.CommandScheduler; -import frc.team2412.robot.commands.arm.ManualArmOverrideOffCommand; import frc.team2412.robot.commands.intake.IntakeDefaultCommand; import frc.team2412.robot.sim.PhysicsSim; import frc.team2412.robot.util.MACAddress; import frc.team2412.robot.util.auto.AutonomousChooser; import java.util.HashMap; +import java.util.Optional; public class Robot extends TimedRobot { /** Singleton Stuff */ @@ -154,7 +150,7 @@ public void autonomousInit() { Shuffleboard.startRecording(); if (subsystems.armSubsystem != null) { // if manual arm control is enabled in auto, the arm will never move - new ManualArmOverrideOffCommand(subsystems.armSubsystem).schedule(); + subsystems.armSubsystem.setManualOverride(false); } if (subsystems.intakeSubsystem != null) { new IntakeDefaultCommand(subsystems.intakeSubsystem).schedule(); @@ -200,7 +196,12 @@ public void autonomousExit() { @Override public void teleopExit() { CommandScheduler.getInstance().cancelAll(); - subsystems.drivebaseSubsystem.stopAllMotors(); + if (subsystems.drivebaseSubsystem != null) { + subsystems.drivebaseSubsystem.stopAllMotors(); + } + if (subsystems.armSubsystem != null) { + subsystems.armSubsystem.resetToLow(); + } } @Override @@ -230,29 +231,50 @@ public boolean isCompetition() { private boolean wasArmButtonPressed = false; private boolean isArmCoast = false; + private Optional wasAlignmentCorrect = Optional.empty(); + @Override public void disabledInit() { Shuffleboard.stopRecording(); - wasArmButtonPressed = subsystems.armSubsystem.isIdleModeTogglePressed(); + if (subsystems.armSubsystem != null) { + wasArmButtonPressed = subsystems.armSubsystem.isIdleModeTogglePressed(); + } + wasAlignmentCorrect = Optional.empty(); } @Override public void disabledPeriodic() { - boolean isArmButtonPressed = subsystems.armSubsystem.isIdleModeTogglePressed(); - if (!wasArmButtonPressed && isArmButtonPressed) { - if (isArmCoast) { - subsystems.armSubsystem.setBrake(); - } else { - subsystems.armSubsystem.setCoast(); + if (subsystems.armSubsystem != null) { + boolean isArmButtonPressed = subsystems.armSubsystem.isIdleModeTogglePressed(); + if (!wasArmButtonPressed && isArmButtonPressed) { + if (isArmCoast) { + subsystems.armSubsystem.setBrake(); + } else { + subsystems.armSubsystem.setCoast(); + } + isArmCoast = !isArmCoast; + } + wasArmButtonPressed = isArmButtonPressed; + } + + if ((subsystems.visionSubsystem != null) && (subsystems.armLedSubsystem != null)) { + boolean isAlignmentCorrect = subsystems.visionSubsystem.isYawAlignedToGrid(); + if (wasAlignmentCorrect.isEmpty() || (wasAlignmentCorrect.get() != isAlignmentCorrect)) { + if (isAlignmentCorrect) { + subsystems.armLedSubsystem.setLEDCorrectAlignment(); + } else { + subsystems.armLedSubsystem.setLEDIncorrectAlignment(); + } + wasAlignmentCorrect = Optional.of(isAlignmentCorrect); } - isArmCoast = !isArmCoast; } - wasArmButtonPressed = isArmButtonPressed; } @Override public void disabledExit() { - subsystems.armSubsystem.setBrake(); + if (subsystems.armSubsystem != null) { + subsystems.armSubsystem.setBrake(); + } isArmCoast = false; } } diff --git a/src/main/java/frc/team2412/robot/commands/arm/SetFullArmCommand.java b/src/main/java/frc/team2412/robot/commands/arm/SetFullArmCommand.java index c42aa77a..1d3a075c 100644 --- a/src/main/java/frc/team2412/robot/commands/arm/SetFullArmCommand.java +++ b/src/main/java/frc/team2412/robot/commands/arm/SetFullArmCommand.java @@ -19,5 +19,13 @@ public SetFullArmCommand( addCommands( new SetArmCommand(armSubsystem, positionType, armTolerance), new SetWristCommand(armSubsystem, wristPosition)); + setName( + "SetFullArmCommand(arm, " + + positionType + + ", " + + wristPosition + + ", " + + armTolerance + + ")"); } } diff --git a/src/main/java/frc/team2412/robot/commands/arm/SetWristAngleCommand.java b/src/main/java/frc/team2412/robot/commands/arm/SetWristAngleCommand.java new file mode 100644 index 00000000..2ea9cc7d --- /dev/null +++ b/src/main/java/frc/team2412/robot/commands/arm/SetWristAngleCommand.java @@ -0,0 +1,25 @@ +package frc.team2412.robot.commands.arm; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.team2412.robot.subsystems.ArmSubsystem; + +public class SetWristAngleCommand extends CommandBase { + private final ArmSubsystem armSubsystem; + private final double wristAngle; + + public SetWristAngleCommand(ArmSubsystem armSubsystem, double wristAngle) { + this.armSubsystem = armSubsystem; + this.wristAngle = wristAngle; + addRequirements(armSubsystem); + } + + @Override + public void initialize() { + armSubsystem.setWristGoal(wristAngle); + } + + @Override + public boolean isFinished() { + return Math.abs(armSubsystem.getWristPosition() - wristAngle) < 0.05; + } +} diff --git a/src/main/java/frc/team2412/robot/subsystems/ArmLEDSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/ArmLEDSubsystem.java index 7891e5d7..b471d77c 100644 --- a/src/main/java/frc/team2412/robot/subsystems/ArmLEDSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/ArmLEDSubsystem.java @@ -202,6 +202,22 @@ public void setLEDAlliance() { updateLED(); } + /** Called when robot has correct alignment during autonomous setup */ + public void setLEDCorrectAlignment() { + color1 = ColorSelector.GREEN; + effectIndex = 0; + + updateLED(); + } + + /** Called when robot has incorrect alignment during autonomous setup */ + public void setLEDIncorrectAlignment() { + color1 = ColorSelector.RED; + effectIndex = 0; + + updateLED(); + } + /** Potentially called during Teleop after alliance selection to represent overall team colors */ public void setLEDCustom() { effectIndex = 2; diff --git a/src/main/java/frc/team2412/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/ArmSubsystem.java index 8fff251e..da6b1f82 100644 --- a/src/main/java/frc/team2412/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/ArmSubsystem.java @@ -119,7 +119,7 @@ public static class ArmConstants { * Scoring Wrist Angle */ public static enum PositionType { - UNKNOWN_POSITION(0.212, 0.08, 0.46, 0.46), + UNKNOWN_POSITION(0.212, 0.08, 0.46, 0.35), ARM_LOW_POSITION(0.212, 0.08, 0.46, 0.35), ARM_MIDDLE_POSITION(0.415, 0.08, 0.42, 0.5), ARM_HIGH_POSITION(0.6546, 0.08, 0.465, 0.473), @@ -376,6 +376,14 @@ public void setWristMotor(double percentOutput) { percentOutput, CANSparkMax.ControlType.kDutyCycle, 0); // calculateWristFeedforward()); } + public void resetToLow() { + setPosition(PositionType.ARM_LOW_POSITION); + setArmGoal(PositionType.ARM_LOW_POSITION.armAngle); + setWristGoal(PositionType.ARM_LOW_POSITION.retractedWristAngle); + updateArmMotorOutput(); + updateWristMotorOutput(); + } + /** Sets current position of the arm. Used for condition checking, nothing really else. */ public void setPosition(PositionType position) { currentPosition = position; diff --git a/src/main/java/frc/team2412/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/VisionSubsystem.java index cb398d40..9811b797 100644 --- a/src/main/java/frc/team2412/robot/subsystems/VisionSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/VisionSubsystem.java @@ -2,6 +2,7 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; @@ -179,6 +180,10 @@ public VisionSubsystem(SwerveDrivePoseEstimator initialPoseEstimator, Field2d fi .addBoolean("Corner too close to edge", () -> tooCloseToEdge) .withPosition(1, 3) .withSize(1, 1); + visionTab + .addBoolean("Is aligned to grid", this::isYawAlignedToGrid) + .withPosition(4, 0) + .withSize(1, 1); ShuffleboardUtil.addPose3dLayout(visionTab, "Robot pose", this::getRobotPose, 2, 0); } @@ -266,6 +271,16 @@ public double getLastTimestampSeconds() { return lastTimestampSeconds; } + public boolean isYawAlignedToGrid() { + if (!hasTargets()) { + return false; + } + double yaw = Math.toDegrees(getRobotPose().getRotation().getZ()); + // Wrapping between -90 and 90 maps 180 to 0 + yaw = MathUtil.inputModulus(yaw, -90, 90); + return Math.abs(yaw) < 1; + } + public void setAlliance(DriverStation.Alliance alliance) { this.alliance = alliance; } diff --git a/src/main/java/frc/team2412/robot/util/auto/AutonomousChooser.java b/src/main/java/frc/team2412/robot/util/auto/AutonomousChooser.java index ad814628..013a9684 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutonomousChooser.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutonomousChooser.java @@ -55,8 +55,38 @@ public AutonomousChooser() { () -> AutonomousTrajectories.getAutoPathByName("ChargeScoreHighCharge")); autonomousModeChooser.addOption( - "FlatScoreHighLeaveComScoreHigh", - () -> AutonomousTrajectories.getAutoPathByName("FlatScoreHighLeaveComScoreHigh")); + "FlatScoreHighLeaveComScoreHighRed", + () -> AutonomousTrajectories.getAutoPathByName("FlatScoreHighLeaveComScoreHighRed")); + + autonomousModeChooser.addOption( + "FlatScoreHighLeaveComScoreHighBlue", + () -> AutonomousTrajectories.getAutoPathByName("FlatScoreHighLeaveComScoreHighBlue")); + + autonomousModeChooser.addOption( + "BumpScoreHighLeaveComScoreHighRed", + () -> AutonomousTrajectories.getAutoPathByName("BumpScoreHighLeaveComScoreHighRed")); + + autonomousModeChooser.addOption( + "BumpScoreHighLeaveComScoreHighBlue", + () -> AutonomousTrajectories.getAutoPathByName("BumpScoreHighLeaveComScoreHighBlue")); + + autonomousModeChooser.addOption( + "FlatScoreMidLeaveComScoreLowScoreLowBackwardsRed", + () -> + AutonomousTrajectories.getAutoPathByName( + "FlatScoreMidLeaveComScoreLowScoreLowBackwardsRed")); + + autonomousModeChooser.addOption( + "FlatScoreMidLeaveComScoreLowScoreLowBackwardsBlue", + () -> + AutonomousTrajectories.getAutoPathByName( + "FlatScoreMidLeaveComScoreLowScoreLowBackwardsBlue")); + + autonomousModeChooser.addOption( + "DONOTUSEFlatScoreMidLeaveComScoreLowScoreLowBackwardsInside", + () -> + AutonomousTrajectories.getAutoPathByName( + "FlatScoreMidLeaveComScoreLowScoreLowBackwardsInside")); autonomousModeChooser.addOption( "ChargeShootChargeShoot", diff --git a/src/main/java/frc/team2412/robot/util/auto/AutonomousTrajectories.java b/src/main/java/frc/team2412/robot/util/auto/AutonomousTrajectories.java index eeb3fecd..848f2f86 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutonomousTrajectories.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutonomousTrajectories.java @@ -12,6 +12,7 @@ import frc.team2412.robot.Robot; import frc.team2412.robot.Subsystems; import frc.team2412.robot.commands.arm.SetFullArmCommand; +import frc.team2412.robot.commands.arm.SetWristAngleCommand; import frc.team2412.robot.commands.arm.SetWristCommand; import frc.team2412.robot.commands.arm.SetWristCommand.WristPosition; import frc.team2412.robot.commands.autonomous.AutoBalanceCommand; @@ -31,14 +32,14 @@ public static Command getAutoPathByName(String name) { eventMap.put( "AutoBalance", new AutoBalanceCommand(Robot.getInstance().subsystems.drivebaseSubsystem)); if (!(s.intakeSubsystem == null) && !(s.armSubsystem == null)) { - SetWristCommand wristOut = new SetWristCommand(s.armSubsystem, WristPosition.WRIST_SCORE); - SetWristCommand wristPrescore = - new SetWristCommand(s.armSubsystem, WristPosition.WRIST_PRESCORE); + Command wristOut = new SetWristCommand(s.armSubsystem, WristPosition.WRIST_SCORE); + Command wristPrescore = new SetWristCommand(s.armSubsystem, WristPosition.WRIST_PRESCORE); + Command wristShoot = new SetWristAngleCommand(s.armSubsystem, 0.34); Command intakeOut = new IntakeSetOutCommand(s.intakeSubsystem).withTimeout(0.5); - Command intakeFastOut = new IntakeSetFastOutCommand(s.intakeSubsystem).withTimeout(0.5); - Command intakeIn = new IntakeSetInCommand(s.intakeSubsystem).withTimeout(0.5); - SetWristCommand wristIn = new SetWristCommand(s.armSubsystem, WristPosition.WRIST_RETRACT); - SequentialCommandGroup scoreBottom = + Command intakeFastOut = new IntakeSetFastOutCommand(s.intakeSubsystem).withTimeout(0.1); + Command intakeIn = new IntakeSetInCommand(s.intakeSubsystem).withTimeout(0.2); + Command wristIn = new SetWristCommand(s.armSubsystem, WristPosition.WRIST_RETRACT); + Command scoreBottom = new SequentialCommandGroup(intakeIn, wristPrescore, intakeOut.withTimeout(1.5), wristIn); Command armLow = new SetFullArmCommand(s.armSubsystem, ARM_LOW_POSITION, WRIST_PRESCORE); Command armMid = new SetFullArmCommand(s.armSubsystem, ARM_MIDDLE_POSITION, WRIST_PRESCORE); @@ -46,7 +47,7 @@ public static Command getAutoPathByName(String name) { Command armSubstation = new SetFullArmCommand(s.armSubsystem, ARM_SUBSTATION_POSITION, WRIST_PRESCORE); Command stow = - new SetFullArmCommand(s.armSubsystem, ARM_LOW_POSITION, WristPosition.WRIST_RETRACT, 0.3); + new SetFullArmCommand(s.armSubsystem, ARM_LOW_POSITION, WristPosition.WRIST_RETRACT, 0.5); Command scoreHigh = new SequentialCommandGroup( new SetFullArmCommand(s.armSubsystem, ARM_HIGH_POSITION, WristPosition.WRIST_SCORE), @@ -61,11 +62,12 @@ public static Command getAutoPathByName(String name) { eventMap.put("WristRetract", wristIn); eventMap.put("WristPrescore", wristPrescore); + eventMap.put("WristScore", wristOut); + eventMap.put("WristShoot", wristShoot); eventMap.put("IntakeOut", intakeOut); eventMap.put("IntakeFastOut", intakeFastOut); eventMap.put("IntakeIn", intakeIn); eventMap.put("ArmHigh", armHigh); - eventMap.put("WristScore", wristOut); eventMap.put("ArmLow", armLow); eventMap.put("ArmMid", armMid); eventMap.put("ArmSubstation", armSubstation); diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index fab5e5b8..9e913435 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2023.4.3", + "version": "2023.4.4", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "mavenUrls": [ "https://3015rangerrobotics.github.io/pathplannerlib/repo" @@ -11,7 +11,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2023.4.3" + "version": "2023.4.4" } ], "jniDependencies": [], @@ -19,7 +19,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2023.4.3", + "version": "2023.4.4", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false,