From 58b5686f6dfcf32665cb07c6443d0559a9112734 Mon Sep 17 00:00:00 2001 From: Korozya-bit Date: Thu, 21 Mar 2024 14:21:56 +0200 Subject: [PATCH] final code for DCMP --- .pathplanner/settings.json | 2 +- .../deploy/pathplanner/autos/TwoNotes.auto | 94 +++++++++++++++++++ .../pathplanner/paths/Note1ToPose1.path | 10 +- .../pathplanner/paths/Note1ToPose2.path | 10 +- .../pathplanner/paths/Note2ToNote4.path | 12 +-- .../pathplanner/paths/Note2ToNote5.path | 12 +-- .../pathplanner/paths/Note2ToPose2.path | 6 +- .../pathplanner/paths/Note3ToPose2.path | 6 +- .../deploy/pathplanner/paths/Note4ToAmp.path | 2 +- .../pathplanner/paths/Note4ToNote2.path | 12 +-- .../pathplanner/paths/Note4ToPose1.path | 2 +- .../pathplanner/paths/Note4ToPose2.path | 2 +- .../pathplanner/paths/Note5ToShoot2.path | 8 +- .../pathplanner/paths/Note8ToPose3.path | 8 +- .../paths/{New Path.path => P1.path} | 28 +++--- src/main/deploy/pathplanner/paths/P2.path | 52 ++++++++++ .../deploy/pathplanner/paths/PID Test.path | 2 +- .../pathplanner/paths/Pose1ToNote4.path | 2 +- .../pathplanner/paths/Pose2ToCenter.path | 2 +- .../pathplanner/paths/Pose2ToNote2.path | 4 +- .../pathplanner/paths/Pose2ToNote3.path | 6 +- .../pathplanner/paths/Pose2ToNote4.path | 2 +- .../pathplanner/paths/Pose2ToNote5.path | 2 +- .../deploy/pathplanner/paths/Pose3Leave.path | 2 +- .../pathplanner/paths/Pose3ToNote2.path | 2 +- .../pathplanner/paths/Pose3ToNote8.path | 2 +- .../deploy/pathplanner/paths/SpeakerTop.path | 2 +- .../pathplanner/paths/note7ToPose3.path | 2 +- .../pathplanner/paths/pose3ToNote7.path | 2 +- src/main/java/frc/robot/Constants.java | 12 +-- src/main/java/frc/robot/RobotContainer.java | 31 ++++-- .../frc/robot/subsystems/swerve/Swerve.java | 13 ++- .../robot/subsystems/swerve/SwerveModule.java | 11 ++- 33 files changed, 266 insertions(+), 99 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/TwoNotes.auto rename src/main/deploy/pathplanner/paths/{New Path.path => P1.path} (61%) create mode 100644 src/main/deploy/pathplanner/paths/P2.path diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index dc16ce5..14edf73 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -10,6 +10,6 @@ "defaultMaxVel": 5.5, "defaultMaxAccel": 3.0, "defaultMaxAngVel": 290.0, - "defaultMaxAngAccel": 250.0, + "defaultMaxAngAccel": 180.0, "maxModuleSpeed": 5.88 } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TwoNotes.auto b/src/main/deploy/pathplanner/autos/TwoNotes.auto new file mode 100644 index 0000000..a3d0d34 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/TwoNotes.auto @@ -0,0 +1,94 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.3, + "y": 5.57 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "prepShooterCommand" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "shootToSpeakerCommand" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeFromGround" + } + }, + { + "type": "path", + "data": { + "pathName": "P1" + } + } + ] + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "P2" + } + }, + { + "type": "named", + "data": { + "name": "closeIntake" + } + }, + { + "type": "named", + "data": { + "name": "prepShooterCommand" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "shootToSpeakerCommand" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Note1ToPose1.path b/src/main/deploy/pathplanner/paths/Note1ToPose1.path index 025e211..8e456b1 100644 --- a/src/main/deploy/pathplanner/paths/Note1ToPose1.path +++ b/src/main/deploy/pathplanner/paths/Note1ToPose1.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.5079619390008028, - "y": 6.916726910376898 + "x": 2.537562767433229, + "y": 6.798323596647194 }, "prevControl": null, "nextControl": { - "x": 2.079829971625757, - "y": 6.739208777562855 + "x": 2.109430800058183, + "y": 6.62080546383315 }, "isLocked": false, "linkedName": "note1" @@ -35,7 +35,7 @@ "maxVelocity": 5.5, "maxAcceleration": 3.0, "maxAngularVelocity": 290.0, - "maxAngularAcceleration": 250.0 + "maxAngularAcceleration": 180.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Note1ToPose2.path b/src/main/deploy/pathplanner/paths/Note1ToPose2.path index 4f0ed23..26aed31 100644 --- a/src/main/deploy/pathplanner/paths/Note1ToPose2.path +++ b/src/main/deploy/pathplanner/paths/Note1ToPose2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.5079619390008028, - "y": 6.916726910376898 + "x": 2.537562767433229, + "y": 6.798323596647194 }, "prevControl": null, "nextControl": { - "x": 2.2218205974873504, - "y": 6.37404505578242 + "x": 2.2514214259197765, + "y": 6.255641742052716 }, "isLocked": false, "linkedName": "note1" @@ -35,7 +35,7 @@ "maxVelocity": 5.5, "maxAcceleration": 3.0, "maxAngularVelocity": 290.0, - "maxAngularAcceleration": 250.0 + "maxAngularAcceleration": 180.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Note2ToNote4.path b/src/main/deploy/pathplanner/paths/Note2ToNote4.path index 9bed5bd..95b06b3 100644 --- a/src/main/deploy/pathplanner/paths/Note2ToNote4.path +++ b/src/main/deploy/pathplanner/paths/Note2ToNote4.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 2.4330426438630655, + "x": 2.557296653054846, "y": 5.5665622442356 }, "prevControl": null, "nextControl": { - "x": 3.55533671803809, + "x": 3.6795907272298707, "y": 5.5665622442356 }, "isLocked": false, @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.934780484945588, - "y": 7.3114046228092455 + "x": 7.9446474277563945, + "y": 7.419940993728142 }, "prevControl": { - "x": 7.1138378174192285, - "y": 7.313054540638996 + "x": 7.123704760230035, + "y": 7.421590911557892 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Note2ToNote5.path b/src/main/deploy/pathplanner/paths/Note2ToNote5.path index a818c08..8c74886 100644 --- a/src/main/deploy/pathplanner/paths/Note2ToNote5.path +++ b/src/main/deploy/pathplanner/paths/Note2ToNote5.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 2.4330426438630655, + "x": 2.557296653054846, "y": 5.5665622442356 }, "prevControl": null, "nextControl": { - "x": 3.7807975615951244, + "x": 3.905051570786905, "y": 6.235907856431097 }, "isLocked": false, @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 7.855844942459117, - "y": 5.732693773079855 + "x": 7.806510228405074, + "y": 5.762294601512281 }, "prevControl": { - "x": 6.000859694027081, - "y": 6.650319454485064 + "x": 5.951524979973038, + "y": 6.67992028291749 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Note2ToPose2.path b/src/main/deploy/pathplanner/paths/Note2ToPose2.path index 2108c48..ee967b7 100644 --- a/src/main/deploy/pathplanner/paths/Note2ToPose2.path +++ b/src/main/deploy/pathplanner/paths/Note2ToPose2.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 2.4330426438630655, + "x": 2.557296653054846, "y": 5.5665622442356 }, "prevControl": null, "nextControl": { - "x": 1.3783243970795378, + "x": 1.5025784062713186, "y": 5.5665622442356 }, "isLocked": false, @@ -35,7 +35,7 @@ "maxVelocity": 5.5, "maxAcceleration": 3.0, "maxAngularVelocity": 290.0, - "maxAngularAcceleration": 250.0 + "maxAngularAcceleration": 180.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Note3ToPose2.path b/src/main/deploy/pathplanner/paths/Note3ToPose2.path index b39fc56..5838fd2 100644 --- a/src/main/deploy/pathplanner/paths/Note3ToPose2.path +++ b/src/main/deploy/pathplanner/paths/Note3ToPose2.path @@ -4,12 +4,12 @@ { "anchor": { "x": 2.596764424298081, - "y": 4.430257322053106 + "y": 4.33 }, "prevControl": null, "nextControl": { "x": 2.359957796838672, - "y": 4.894003634161114 + "y": 4.793746312108008 }, "isLocked": false, "linkedName": "note3" @@ -41,7 +41,7 @@ "maxVelocity": 5.5, "maxAcceleration": 3.0, "maxAngularVelocity": 290.0, - "maxAngularAcceleration": 250.0 + "maxAngularAcceleration": 180.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Note4ToAmp.path b/src/main/deploy/pathplanner/paths/Note4ToAmp.path index 615d4a6..1b07512 100644 --- a/src/main/deploy/pathplanner/paths/Note4ToAmp.path +++ b/src/main/deploy/pathplanner/paths/Note4ToAmp.path @@ -35,7 +35,7 @@ "maxVelocity": 5.5, "maxAcceleration": 3.0, "maxAngularVelocity": 290.0, - "maxAngularAcceleration": 250.0 + "maxAngularAcceleration": 180.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Note4ToNote2.path b/src/main/deploy/pathplanner/paths/Note4ToNote2.path index 0265e2b..634567b 100644 --- a/src/main/deploy/pathplanner/paths/Note4ToNote2.path +++ b/src/main/deploy/pathplanner/paths/Note4ToNote2.path @@ -3,24 +3,24 @@ "waypoints": [ { "anchor": { - "x": 7.934780484945588, - "y": 7.3114046228092455 + "x": 7.9446474277563945, + "y": 7.419940993728142 }, "prevControl": null, "nextControl": { - "x": 5.773920009378484, - "y": 6.640452511674253 + "x": 5.783786952189291, + "y": 6.748988882593149 }, "isLocked": false, "linkedName": "note4" }, { "anchor": { - "x": 2.4330426438630655, + "x": 2.557296653054846, "y": 5.5665622442356 }, "prevControl": { - "x": 3.187126924808923, + "x": 3.3113809340007037, "y": 5.5665622442356 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Note4ToPose1.path b/src/main/deploy/pathplanner/paths/Note4ToPose1.path index 52db03f..ece0228 100644 --- a/src/main/deploy/pathplanner/paths/Note4ToPose1.path +++ b/src/main/deploy/pathplanner/paths/Note4ToPose1.path @@ -35,7 +35,7 @@ "maxVelocity": 5.5, "maxAcceleration": 3.0, "maxAngularVelocity": 290.0, - "maxAngularAcceleration": 250.0 + "maxAngularAcceleration": 180.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Note4ToPose2.path b/src/main/deploy/pathplanner/paths/Note4ToPose2.path index 00de9c1..64283a1 100644 --- a/src/main/deploy/pathplanner/paths/Note4ToPose2.path +++ b/src/main/deploy/pathplanner/paths/Note4ToPose2.path @@ -35,7 +35,7 @@ "maxVelocity": 5.5, "maxAcceleration": 3.0, "maxAngularVelocity": 290.0, - "maxAngularAcceleration": 250.0 + "maxAngularAcceleration": 180.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Note5ToShoot2.path b/src/main/deploy/pathplanner/paths/Note5ToShoot2.path index 0f03c8d..5f7121f 100644 --- a/src/main/deploy/pathplanner/paths/Note5ToShoot2.path +++ b/src/main/deploy/pathplanner/paths/Note5ToShoot2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.855844942459117, - "y": 5.732693773079855 + "x": 7.806510228405074, + "y": 5.762294601512281 }, "prevControl": null, "nextControl": { - "x": 5.211504269162385, - "y": 7.05486410972822 + "x": 5.162169555108342, + "y": 7.084464938160646 }, "isLocked": false, "linkedName": "note5" diff --git a/src/main/deploy/pathplanner/paths/Note8ToPose3.path b/src/main/deploy/pathplanner/paths/Note8ToPose3.path index c6d6b70..ef915df 100644 --- a/src/main/deploy/pathplanner/paths/Note8ToPose3.path +++ b/src/main/deploy/pathplanner/paths/Note8ToPose3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 7.924913542134778, - "y": 0.9669603954592523 + "x": 7.915046599323969, + "y": 0.7498876536214618 }, "prevControl": null, "nextControl": { - "x": 5.1424356694867255, - "y": 1.085363709188957 + "x": 5.132568726675917, + "y": 0.8682909673511665 }, "isLocked": false, "linkedName": "pose8" diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/P1.path similarity index 61% rename from src/main/deploy/pathplanner/paths/New Path.path rename to src/main/deploy/pathplanner/paths/P1.path index d1784b8..417fa33 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/P1.path @@ -3,39 +3,39 @@ "waypoints": [ { "anchor": { - "x": 15.8, - "y": 4.48 + "x": 1.3, + "y": 5.5665622442356 }, "prevControl": null, "nextControl": { - "x": 16.8, - "y": 4.48 + "x": 2.3, + "y": 5.5665622442356 }, "isLocked": false, - "linkedName": null + "linkedName": "pose2" }, { "anchor": { - "x": 4.0, - "y": 6.0 + "x": 2.557296653054846, + "y": 5.5665622442356 }, "prevControl": { - "x": 3.0, - "y": 6.0 + "x": 1.5572966530548462, + "y": 5.5665622442356 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "note2" } ], "rotationTargets": [], "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 5.5, - "maxAcceleration": 3.0, + "maxVelocity": 2.0, + "maxAcceleration": 1.0, "maxAngularVelocity": 290.0, - "maxAngularAcceleration": 250.0 + "maxAngularAcceleration": 180.0 }, "goalEndState": { "velocity": 0, @@ -48,5 +48,5 @@ "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/P2.path b/src/main/deploy/pathplanner/paths/P2.path new file mode 100644 index 0000000..b9db91e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/P2.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.557296653054846, + "y": 5.5665622442356 + }, + "prevControl": null, + "nextControl": { + "x": 2.1231481970691677, + "y": 5.5665622442356 + }, + "isLocked": false, + "linkedName": "note2" + }, + { + "anchor": { + "x": 1.3, + "y": 5.5665622442356 + }, + "prevControl": { + "x": 1.817341894067714, + "y": 5.5665622442356 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "pose2" + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 2.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 290.0, + "maxAngularAcceleration": 180.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/PID Test.path b/src/main/deploy/pathplanner/paths/PID Test.path index a87a063..1e2ea6b 100644 --- a/src/main/deploy/pathplanner/paths/PID Test.path +++ b/src/main/deploy/pathplanner/paths/PID Test.path @@ -35,7 +35,7 @@ "maxVelocity": 5.5, "maxAcceleration": 3.0, "maxAngularVelocity": 290.0, - "maxAngularAcceleration": 250.0 + "maxAngularAcceleration": 180.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Pose1ToNote4.path b/src/main/deploy/pathplanner/paths/Pose1ToNote4.path index 318cd8f..e1cb1b0 100644 --- a/src/main/deploy/pathplanner/paths/Pose1ToNote4.path +++ b/src/main/deploy/pathplanner/paths/Pose1ToNote4.path @@ -35,7 +35,7 @@ "maxVelocity": 5.5, "maxAcceleration": 3.0, "maxAngularVelocity": 290.0, - "maxAngularAcceleration": 250.0 + "maxAngularAcceleration": 180.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Pose2ToCenter.path b/src/main/deploy/pathplanner/paths/Pose2ToCenter.path index 744f814..99b34d1 100644 --- a/src/main/deploy/pathplanner/paths/Pose2ToCenter.path +++ b/src/main/deploy/pathplanner/paths/Pose2ToCenter.path @@ -35,7 +35,7 @@ "maxVelocity": 5.5, "maxAcceleration": 3.0, "maxAngularVelocity": 290.0, - "maxAngularAcceleration": 250.0 + "maxAngularAcceleration": 180.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Pose2ToNote2.path b/src/main/deploy/pathplanner/paths/Pose2ToNote2.path index db1ef50..65a5823 100644 --- a/src/main/deploy/pathplanner/paths/Pose2ToNote2.path +++ b/src/main/deploy/pathplanner/paths/Pose2ToNote2.path @@ -16,11 +16,11 @@ }, { "anchor": { - "x": 2.4330426438630655, + "x": 2.557296653054846, "y": 5.5665622442356 }, "prevControl": { - "x": 2.0675641351282703, + "x": 2.191818144320051, "y": 5.5665622442356 }, "nextControl": null, diff --git a/src/main/deploy/pathplanner/paths/Pose2ToNote3.path b/src/main/deploy/pathplanner/paths/Pose2ToNote3.path index 875c620..04a3088 100644 --- a/src/main/deploy/pathplanner/paths/Pose2ToNote3.path +++ b/src/main/deploy/pathplanner/paths/Pose2ToNote3.path @@ -17,11 +17,11 @@ { "anchor": { "x": 2.596764424298081, - "y": 4.430257322053106 + "y": 4.33 }, "prevControl": { "x": 2.1527519978116896, - "y": 4.903870576971923 + "y": 4.803613254918817 }, "nextControl": null, "isLocked": false, @@ -35,7 +35,7 @@ "maxVelocity": 5.5, "maxAcceleration": 3.0, "maxAngularVelocity": 290.0, - "maxAngularAcceleration": 250.0 + "maxAngularAcceleration": 180.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Pose2ToNote4.path b/src/main/deploy/pathplanner/paths/Pose2ToNote4.path index d015293..4ad694e 100644 --- a/src/main/deploy/pathplanner/paths/Pose2ToNote4.path +++ b/src/main/deploy/pathplanner/paths/Pose2ToNote4.path @@ -35,7 +35,7 @@ "maxVelocity": 5.5, "maxAcceleration": 3.0, "maxAngularVelocity": 290.0, - "maxAngularAcceleration": 250.0 + "maxAngularAcceleration": 180.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Pose2ToNote5.path b/src/main/deploy/pathplanner/paths/Pose2ToNote5.path index ea576cc..dc6478c 100644 --- a/src/main/deploy/pathplanner/paths/Pose2ToNote5.path +++ b/src/main/deploy/pathplanner/paths/Pose2ToNote5.path @@ -35,7 +35,7 @@ "maxVelocity": 5.5, "maxAcceleration": 3.0, "maxAngularVelocity": 290.0, - "maxAngularAcceleration": 250.0 + "maxAngularAcceleration": 180.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Pose3Leave.path b/src/main/deploy/pathplanner/paths/Pose3Leave.path index b1b8388..4bb8d87 100644 --- a/src/main/deploy/pathplanner/paths/Pose3Leave.path +++ b/src/main/deploy/pathplanner/paths/Pose3Leave.path @@ -35,7 +35,7 @@ "maxVelocity": 5.5, "maxAcceleration": 3.0, "maxAngularVelocity": 290.0, - "maxAngularAcceleration": 250.0 + "maxAngularAcceleration": 180.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Pose3ToNote2.path b/src/main/deploy/pathplanner/paths/Pose3ToNote2.path index 0eaea0d..87eaa0e 100644 --- a/src/main/deploy/pathplanner/paths/Pose3ToNote2.path +++ b/src/main/deploy/pathplanner/paths/Pose3ToNote2.path @@ -35,7 +35,7 @@ "maxVelocity": 5.5, "maxAcceleration": 3.0, "maxAngularVelocity": 290.0, - "maxAngularAcceleration": 250.0 + "maxAngularAcceleration": 180.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/Pose3ToNote8.path b/src/main/deploy/pathplanner/paths/Pose3ToNote8.path index a5413c7..1e19a05 100644 --- a/src/main/deploy/pathplanner/paths/Pose3ToNote8.path +++ b/src/main/deploy/pathplanner/paths/Pose3ToNote8.path @@ -35,7 +35,7 @@ "maxVelocity": 5.5, "maxAcceleration": 3.0, "maxAngularVelocity": 290.0, - "maxAngularAcceleration": 250.0 + "maxAngularAcceleration": 180.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/SpeakerTop.path b/src/main/deploy/pathplanner/paths/SpeakerTop.path index b0a6eea..c739f62 100644 --- a/src/main/deploy/pathplanner/paths/SpeakerTop.path +++ b/src/main/deploy/pathplanner/paths/SpeakerTop.path @@ -35,7 +35,7 @@ "maxVelocity": 5.5, "maxAcceleration": 3.0, "maxAngularVelocity": 290.0, - "maxAngularAcceleration": 250.0 + "maxAngularAcceleration": 180.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/note7ToPose3.path b/src/main/deploy/pathplanner/paths/note7ToPose3.path index f07a51a..bce294c 100644 --- a/src/main/deploy/pathplanner/paths/note7ToPose3.path +++ b/src/main/deploy/pathplanner/paths/note7ToPose3.path @@ -35,7 +35,7 @@ "maxVelocity": 5.5, "maxAcceleration": 3.0, "maxAngularVelocity": 290.0, - "maxAngularAcceleration": 250.0 + "maxAngularAcceleration": 180.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/deploy/pathplanner/paths/pose3ToNote7.path b/src/main/deploy/pathplanner/paths/pose3ToNote7.path index 7c9d6aa..2405edb 100644 --- a/src/main/deploy/pathplanner/paths/pose3ToNote7.path +++ b/src/main/deploy/pathplanner/paths/pose3ToNote7.path @@ -35,7 +35,7 @@ "maxVelocity": 5.5, "maxAcceleration": 3.0, "maxAngularVelocity": 290.0, - "maxAngularAcceleration": 250.0 + "maxAngularAcceleration": 180.0 }, "goalEndState": { "velocity": 0, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 697234e..0c11a3a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -28,7 +28,7 @@ public static final class SwerveConstants { public enum Modules { // drive ID, spin ID, abs encoder channel, offset angle, drive reversed, angle reversed FL(18, 17, 2, 0.045, true, true), - FR(12, 11, 3, 0.071, true, true), + FR(12, 11, 8, 0.071, true, true), BL(16, 15, 4, 0.595, true, true), BR(14, 13, 1, 0.116, true, true); @@ -98,7 +98,7 @@ public static final class ModuleConstants { public static final double kTurningEncoderRPMToRadiansPerSec = kTurningEncoderRotationsToRadians / 60; public static final Gains MODULE_ANGLE_GAINS = new Gains(0.785, 0, 0); - public static final double TOLERANCE = 0.2; + public static final double TOLERANCE = 0.35; public static final int DRIVE_CURRENT_LIMIT = 60; public static final int ANGLE_CURRENT_LIMIT = 25; @@ -113,8 +113,8 @@ public static final class ShooterConstants { public static final Gains UPPER_GAINS = new Gains(new Gains(0.00028558, 0, 0), new Gains(14.5152, 0, 0.11033, 0)); public static final Gains LOWER_GAINS = new Gains(new Gains(0.0006, 0, 0), new Gains(15.5466, 0, 0.1059890436, 0)); - public static final double AMP_UPPER_SHOOTER_RPM = 1125; //1000 - public static final double AMP_LOWER_SHOOTER_RPM = 2500; //2250 + public static final double AMP_UPPER_SHOOTER_RPM = 1125; + public static final double AMP_LOWER_SHOOTER_RPM = 2500; public static final double SPEAKER_SHOOTER_RPM = 5300; @@ -138,12 +138,12 @@ public static final class IntakeConstants { public static final int ENCODER_PORT = 6; public static final int BEAMBREAK_PORT = 0; - public static final Gains INTAKE_GAINS = new Gains(0.058037 * 2 , 0.0, 0.011109 / 4,0.038684, 0.12578, 0.022038, 54.356); + public static final Gains INTAKE_GAINS = new Gains(0.116074, 0.0, 0.00277725,0.038684, 0.12578, 0.022038, 54.356); public static final double INTAKE_TOLERANCE = 7.5; public static final double ANGLE_MOTOR_CONVERSION_FACTOR = (1 / 10.0) * (1 / 3.0) * (16.0 / 40.0); - public static final double INTAKE_ENCODER_OFFSET = 0.235; + public static final double INTAKE_ENCODER_OFFSET = 0.4016 - 0.006; public static final int INTAKE_READING_OFFSET = 50; // deg public static final double AMP_SHOOTER_SPEED = -1; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e36f44c..c561f58 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -55,6 +55,7 @@ public class RobotContainer implements Logged { private static EventLoop climberLoop = new EventLoop(); // swerve final Translation2d emptyPose = new Translation2d(); + private boolean climberMode = false; private boolean farShooter = false; @@ -68,11 +69,27 @@ public class RobotContainer implements Logged { if (climberMode) { cs.setActiveButtonLoop(climberLoop); swerve.maxSpeed.setDouble(20); + leds.setPattern(SOLID, CYAN.color).schedule(); } else { cs.setActiveButtonLoop(cs.getDefaultButtonLoop()); swerve.maxSpeed.setDouble(Constants.SwerveConstants.DRIVE_SPEED_PERCENTAGE); + leds.setPattern(TRAIN, TEAM_BLUE.color, TEAM_GOLD.color).schedule(); + } + }).withName("ClimberMode").ignoringDisable(true); + + + private final Command shooterDistanceCommand = new InstantCommand(() -> { + farShooter = !farShooter; + + if (farShooter) { + shooter.upperSpeed.setDouble(80); + shooter.lowerSpeed.setDouble(100); + } else { + shooter.upperSpeed.setDouble(SPEAKER_DC * 100); + shooter.lowerSpeed.setDouble(SPEAKER_DC * 100); } - }).withName("ClimberMode"); + }).ignoringDisable(true).withName("Delivery"); + Command intakeVibrate = vibrateControllerCommand(50, 0.25); @@ -134,7 +151,9 @@ private void configureBindings() { new ProxyCommand(() -> swerve.pathFindToPose(AMPLIFIER.pose.get(), new PathConstraints(1.5, 2, Math.PI, Math.PI), 0)), scoreNoteCommand(shooter.shootToAmpCommand(), driver.R1(), true))); - driver.povDown().onTrue(shooter.forceShootCommand().alongWith(new WaitCommand(0.75).andThen(intake.forceTransport(()-> false)))); + driver.povDown().onTrue(shooter.forceShootCommand().alongWith(new WaitCommand(0.75).andThen(intake.forceTransport(() -> false)))); + + driver.create().onTrue(intake.shootToAmpCommand()); //up - full field shot to speaker //right - drive to amp @@ -152,7 +171,7 @@ private Command scoreNoteCommand(Command shooterCommand, Trigger release, boolea new ParallelDeadlineGroup( intake.transportToShooterCommand(() -> toAmp), leds.deadLineLEDcommand(leds.setPattern(SOLID, GREEN.color).withTimeout(0.25)) - ))); + ))); } private Command systemTesterCommand() { @@ -191,7 +210,7 @@ private void init() { NamedCommands.registerCommand("setSwerveIdle", swerve.setDriveIdleMode(CANSparkBase.IdleMode.kCoast)); - NamedCommands.registerCommand("forceShoot", shooter.forceShootCommand().alongWith(new WaitCommand(0.75).andThen(intake.forceTransport(()-> false)))); + NamedCommands.registerCommand("forceShoot", shooter.forceShootCommand().alongWith(new WaitCommand(0.75).andThen(intake.forceTransport(() -> false)))); NamedCommands.registerCommand("farShooter", scoreNoteCommand(shooter.manualShooter(1, 0.6), new Trigger(() -> true), false)); NamedCommands.registerCommand("prepFarShooter", shooter.prepFarShooter(() -> swerve.getDistanceFromPose(SPEAKER.pose.get()))); @@ -203,7 +222,7 @@ private void init() { pitTab.add("System tester", systemTesterCommand().withName("SystemTest")).withSize(2, 2); matchTab.addBoolean("intake note", intake.hasNoteTrigger).withPosition(19, 0).withSize(4, 3); - matchTab.add("pumpNote", intake.pumpNoteCommand().withName("PumpNote")).withPosition(16, 1).withSize(3, 2); + matchTab.add("delivery", shooterDistanceCommand).withPosition(16, 1).withSize(3, 2); matchTab.add("climberMode", climberModeCommand).withPosition(16, 3).withSize(3, 2); autoChooser.setDefaultOption("none", new InstantCommand(() -> { @@ -214,7 +233,7 @@ private void init() { autoChooser.addOption("3214", swerve.runAuto("3214")); autoChooser.addOption("73", swerve.runAuto("73")); - autoChooser.addOption("shoot", swerve.runAuto("shoot")); + autoChooser.addOption("shoot", shooter.forceShootCommand().alongWith(new WaitCommand(0.75).andThen(intake.forceTransport(() -> false)))); autoChooser.addOption("leaveFromBottom", swerve.runAuto("shootAndLeave")); Shuffleboard.getTab("Auto").add(autoChooser); diff --git a/src/main/java/frc/robot/subsystems/swerve/Swerve.java b/src/main/java/frc/robot/subsystems/swerve/Swerve.java index ffd1661..f721386 100644 --- a/src/main/java/frc/robot/subsystems/swerve/Swerve.java +++ b/src/main/java/frc/robot/subsystems/swerve/Swerve.java @@ -10,7 +10,6 @@ import com.pathplanner.lib.util.PIDConstants; import com.pathplanner.lib.util.ReplanningConfig; import com.revrobotics.CANSparkBase; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; @@ -197,7 +196,7 @@ public Command driveSwerveCommand( BooleanSupplier boost, Supplier turnToTranslation ) { - return straightenModulesCommand().andThen(this.run( + return this.run( () -> { int allianceMultiplier = AllianceUtils.isRedAlliance() ? -1 : 1; double speedLimit = boost.getAsBoolean() ? BOOST_SPEED_PERCENTAGE : maxSpeed.getDouble(DRIVE_SPEED_PERCENTAGE); @@ -222,7 +221,7 @@ public Command driveSwerveCommand( //apply the array to the swerve modules of the robot setModulesStates(moduleStates); } - )); + ); } public Command driveSwerveCommand( @@ -251,8 +250,7 @@ public Command straightenModulesCommand() { swerveModules[FRONT_LEFT].isReset .and(swerveModules[FRONT_RIGHT].isReset) .and(swerveModules[BACK_LEFT].isReset) - .and(swerveModules[BACK_RIGHT].isReset) - .or(new Trigger(() -> manualStraighten)), + .and(swerveModules[BACK_RIGHT].isReset), this); } @@ -306,7 +304,7 @@ public Command turnToAngleCommand(double degrees) { // other methods public void resetAngleEncoders() { - foreachModule(SwerveModule::resetEncoders); + foreachModule(SwerveModule::resetRelative); } private void stopModules() { @@ -397,7 +395,6 @@ public Command pathFindThenFollowPath(String pathName) { public Command runAuto(String autoName) { return new SequentialCommandGroup( - straightenModulesCommand(), setOdometryPositionCommand(PathPlannerAuto.getStaringPoseFromAutoFile(autoName)), new PathPlannerAuto(autoName)); } @@ -417,6 +414,8 @@ private void initShuffleboardData() { .withPosition(0, 2).withSize(4, 4); swerveTab.add("Field2d", field).withSize(9, 5).withPosition(12, 0); swerveTab.add("manual straighten", new InstantCommand(() -> manualStraighten = !manualStraighten)); + + swerveTab.add(new InstantCommand(()-> foreachModule((SwerveModule::resetAbs)))); } private void initAutoBuilder() { diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index ec68974..65dc880 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -74,7 +74,7 @@ public SwerveModule( new Thread(() -> { try { Thread.sleep(2_000); - resetEncoders(); + resetRelative(); } catch (Exception ignored) {} }).start(); } @@ -98,9 +98,12 @@ public double getAbsEncoderRad() { return angle; } - public void resetEncoders() { -// _driveEncoder.setPosition(0); - _angleMotor.setPosition(getAbsEncoderRad()); + public void resetRelative() { + _angleMotor.setPosition(0); + } + + public void resetAbs() { + _angleMotor.setPosition(getResetRad()); } public SwerveModuleState getState() {