From cc41ea3f1b29d64e615a9648509d974c2d5e49a6 Mon Sep 17 00:00:00 2001 From: Mehooliu Date: Thu, 5 Mar 2026 10:09:30 -0500 Subject: [PATCH] Auto climb, auto aim, shooter tuned, intake rotation tuned --- .../pathplanner/autos/HP Auto + Climb.auto | 26 ++++++- src/main/deploy/pathplanner/navgrid.json | 2 +- .../deploy/pathplanner/paths/HP to Hub.path | 2 +- .../pathplanner/paths/Hub to Shoot.path | 54 ++++++++++++++ .../paths/Hub to side of Climb.path | 14 ++-- .../pathplanner/paths/Side into Climb.path | 54 ++++++++++++++ .../pathplanner/paths/Starting to HP.path | 10 +-- .../pathplanner/paths/test_1_Meter.path | 20 +++--- src/main/deploy/pathplanner/settings.json | 5 +- .../neo/modules/physicalproperties.json | 2 +- src/main/java/frc/robot/Constants.java | 58 +++++++++------ src/main/java/frc/robot/Robot.java | 12 +++- src/main/java/frc/robot/RobotContainer.java | 51 ++++++++----- .../frc/robot/subsystems/IntakeSubsystem.java | 54 +++++++++----- .../robot/subsystems/ShooterSubsystem.java | 12 ++-- .../robot/subsystems/TargetingSubsystems.java | 72 ++++++++++++------- .../swervedrive/SwerveSubsystem.java | 33 +++++++++ .../robot/subsystems/swervedrive/Vision.java | 7 +- 18 files changed, 369 insertions(+), 119 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/Hub to Shoot.path create mode 100644 src/main/deploy/pathplanner/paths/Side into Climb.path diff --git a/src/main/deploy/pathplanner/autos/HP Auto + Climb.auto b/src/main/deploy/pathplanner/autos/HP Auto + Climb.auto index d8ff8c6..83f6076 100644 --- a/src/main/deploy/pathplanner/autos/HP Auto + Climb.auto +++ b/src/main/deploy/pathplanner/autos/HP Auto + Climb.auto @@ -19,7 +19,7 @@ { "type": "wait", "data": { - "waitTime": 3.0 + "waitTime": 1.5 } }, { @@ -34,6 +34,12 @@ "name": "Shoot_Fuel_Command" } }, + { + "type": "named", + "data": { + "name": "Assist_Shooter" + } + }, { "type": "wait", "data": { @@ -51,6 +57,24 @@ "data": { "pathName": "Hub to side of Climb" } + }, + { + "type": "path", + "data": { + "pathName": "Side into Climb" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "Lift_Robot" + } } ] } diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json index ac5f521..33c18ce 100644 --- a/src/main/deploy/pathplanner/navgrid.json +++ b/src/main/deploy/pathplanner/navgrid.json @@ -1 +1 @@ -{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file +{"field_size":{"x":16.54,"y":8.07},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/HP to Hub.path b/src/main/deploy/pathplanner/paths/HP to Hub.path index 74ab6bc..ffc5e11 100644 --- a/src/main/deploy/pathplanner/paths/HP to Hub.path +++ b/src/main/deploy/pathplanner/paths/HP to Hub.path @@ -45,7 +45,7 @@ "rotation": 38.99099404250546 }, "reversed": false, - "folder": null, + "folder": "Start to HP to Hub to Climb", "idealStartingState": { "velocity": 0, "rotation": 180.0 diff --git a/src/main/deploy/pathplanner/paths/Hub to Shoot.path b/src/main/deploy/pathplanner/paths/Hub to Shoot.path new file mode 100644 index 0000000..055f568 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Hub to Shoot.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5185568181818176, + "y": 4.024693181818182 + }, + "prevControl": null, + "nextControl": { + "x": 2.879534090909091, + "y": 4.024693181818182 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.281738636363636, + "y": 4.024693181818182 + }, + "prevControl": { + "x": 2.8279999999999994, + "y": 4.024693181818182 + }, + "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": "Middle Shoot to Climb", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Hub to side of Climb.path b/src/main/deploy/pathplanner/paths/Hub to side of Climb.path index 17d6206..1cc25eb 100644 --- a/src/main/deploy/pathplanner/paths/Hub to side of Climb.path +++ b/src/main/deploy/pathplanner/paths/Hub to side of Climb.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 0.8, - "y": 2.0 + "x": 1.08, + "y": 2.0457840909090907 }, "prevControl": { - "x": 1.2698463103929543, - "y": 2.1710100716628347 + "x": 1.5498463103929543, + "y": 2.2167941625719254 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -80.0 + "rotation": -99.66394495351781 }, "reversed": false, - "folder": null, + "folder": "Start to HP to Hub to Climb", "idealStartingState": { "velocity": 0, - "rotation": 45.0 + "rotation": 22.013232308372665 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Side into Climb.path b/src/main/deploy/pathplanner/paths/Side into Climb.path new file mode 100644 index 0000000..75e2d29 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Side into Climb.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.08, + "y": 2.046 + }, + "prevControl": null, + "nextControl": { + "x": 1.0861477272727271, + "y": 2.5817386363636365 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.25, + "y": 3.5 + }, + "prevControl": { + "x": 1.256147727272727, + "y": 3.190795454545455 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "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": -99.664 + }, + "reversed": false, + "folder": "Start to HP to Hub to Climb", + "idealStartingState": { + "velocity": 0, + "rotation": -99.664 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Starting to HP.path b/src/main/deploy/pathplanner/paths/Starting to HP.path index e1878fe..ea68986 100644 --- a/src/main/deploy/pathplanner/paths/Starting to HP.path +++ b/src/main/deploy/pathplanner/paths/Starting to HP.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 0.5, - "y": 0.65 + "x": 0.4471249999999998, + "y": 0.63375 }, "prevControl": { - "x": 1.0519022424324624, - "y": 0.6499999999999999 + "x": 0.999027242432462, + "y": 0.6337499999999999 }, "nextControl": null, "isLocked": false, @@ -45,7 +45,7 @@ "rotation": 180.0 }, "reversed": false, - "folder": null, + "folder": "Start to HP to Hub to Climb", "idealStartingState": { "velocity": 0, "rotation": 180.0 diff --git a/src/main/deploy/pathplanner/paths/test_1_Meter.path b/src/main/deploy/pathplanner/paths/test_1_Meter.path index 69305a4..d799800 100644 --- a/src/main/deploy/pathplanner/paths/test_1_Meter.path +++ b/src/main/deploy/pathplanner/paths/test_1_Meter.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.0, - "y": 7.0 + "x": 15.474, + "y": 3.437204545454546 }, "prevControl": null, "nextControl": { - "x": 2.0, - "y": 7.0 + "x": 16.473999999999986, + "y": 3.437204545454546 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.0, - "y": 7.0 + "x": 1.066, + "y": 1.87 }, "prevControl": { - "x": 0.8572206587126847, - "y": 7.0 + "x": -0.07677934128731523, + "y": 1.87 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 90.0 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 90.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 2ca3c4c..2dc50a1 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -2,7 +2,10 @@ "robotWidth": 0.9, "robotLength": 0.9, "holonomicMode": true, - "pathFolders": [], + "pathFolders": [ + "Middle Shoot to Climb", + "Start to HP to Hub to Climb" + ], "autoFolders": [], "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, diff --git a/src/main/deploy/swerve/neo/modules/physicalproperties.json b/src/main/deploy/swerve/neo/modules/physicalproperties.json index 63dee33..fa0c1f3 100644 --- a/src/main/deploy/swerve/neo/modules/physicalproperties.json +++ b/src/main/deploy/swerve/neo/modules/physicalproperties.json @@ -1,6 +1,6 @@ { "optimalVoltage": 12, - "robotMass": 140, + "robotMass": 51.75, "wheelGripCoefficientOfFriction": 1.85, "currentLimit": { "drive": 40, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9c643fa..8213d76 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -38,10 +38,10 @@ public final class Constants { private static ShuffleboardTab programmingTab = Shuffleboard.getTab("Programming"); - public static final double ROBOT_MASS = 140 * 0.453592; // 32lbs * kg per pound + public static final double ROBOT_MASS = 115 * 0.453592; // 32lbs * kg per pound public static final Matter CHASSIS = new Matter(new Translation3d(0, 0, Units.inchesToMeters(8)), ROBOT_MASS); public static final double LOOP_TIME = 0.13; // s, 20ms + 110ms sprk max velocity lag - public static final double MAX_SPEED = Units.feetToMeters(25); + public static final double MAX_SPEED = Units.feetToMeters(14.5); // Maximum speed of the robot in meters per second, used to limit acceleration. // public static final class AutonConstants @@ -68,14 +68,14 @@ public final class Constants { } public static class ShooterConstants { - private static GenericEntry shooterRPM = programmingTab.add("Desired Shooter RPM", -4000) - .withWidget(BuiltInWidgets.kNumberBar).getEntry(); + // private static GenericEntry shooterRPM = programmingTab.add("Desired Shooter RPM", -4000) + // .withWidget(BuiltInWidgets.kNumberBar).getEntry(); - public static double SHOOTER_RPM; + public static double SHOOTER_RPM = -2700; - public static void updateShooterRPM() { - SHOOTER_RPM = shooterRPM.getDouble(-4000); - } + // public static void updateShooterRPM() { + // SHOOTER_RPM = shooterRPM.getDouble(-4000); + // } public static final int CENTER_SHOOTER_MOTOR_ID = 42; public static final int LEFT_SHOOTER_MOTOR_ID = 41; @@ -128,31 +128,45 @@ public final class Constants { public static final int INTAKE_ROTATOR_MOTOR_ID = 51; public static class IntakeRotatorPID { - public static final double INTAKE_ROTATOR_P = .5; - public static final double INTAKE_ROTATOR_I = 0.01; - public static final double INTAKE_ROTATOR_D = 0; + public static final double INTAKE_ROTATOR_P = .1; + public static final double INTAKE_ROTATOR_I = 0.0; + public static final double INTAKE_ROTATOR_D = 0.; } public static final double INTAKE_MOTOR_P = 0.0001; public static final double INTAKE_MOTOR_I = 0; public static final double INTAKE_MOTOR_D = 0; - public static final double INTAKE_COLLECT_ENCODER_VALUE = 0; - public static final double INTAKE_MIDDLE_ENCODER_VALUE = -2.5; - public static final double INTAKE_RETRACT_ENCODER_VALUE = -5; + public static final double INTAKE_COLLECT_ENCODER_VALUE = -0.2; + public static final double INTAKE_MIDDLE_ENCODER_VALUE = -3; + public static final double INTAKE_RETRACT_ENCODER_VALUE = -5.5; public static final double INTAKE_THROUGHBORE_ENCODER_DEPLOY = .13; public static final double INTAKE_THROUGHBORE_ENCODER_RETRACT = .49; public static final double INTAKE_THROUGHBORE_ENCODER_MIDDLE = .389; + public static final double INTAKE_MANUAL_SPEED = .1; } // create object and a new widget under programming tab in Elastic where object // retrieves value from widget public static class TargetingConstants { - public static final Pose2d RIGHT_CLIMB_POSE = new Pose2d(1.075, 4.75, Rotation2d.fromDegrees(90)); - public static final Pose2d LEFT_CLIMB_POSE = new Pose2d(1.075, 2.75, Rotation2d.fromDegrees(-90)); + public static final Pose2d BLUE_LEFT_CLIMB_POSE = new Pose2d(1.066, 4.633, Rotation2d.fromDegrees(90)); + public static final Pose2d BLUE_LEFT_CLIMB_POSE_OFFSETTED = new Pose2d(1.066, 5.133, + Rotation2d.fromDegrees(90)); + + public static final Pose2d BLUE_RIGHT_CLIMB_POSE = new Pose2d(1.066, 2.87, Rotation2d.fromDegrees(-90)); + public static final Pose2d BLUE_RIGHT_CLIMB_POSE_OFFSETTED = new Pose2d(1.066, 2.37, + Rotation2d.fromDegrees(-90)); + + public static final Pose2d RED_RIGHT_CLIMB_POSE = new Pose2d(15.474, 5.22, Rotation2d.fromDegrees(90)); + public static final Pose2d RED_RIGHT_CLIMB_POSE_OFFSETTED = new Pose2d(15.474, 5.72, + Rotation2d.fromDegrees(90)); + + public static final Pose2d RED_LEFT_CLIMB_POSE = new Pose2d(15.474, 3.437, Rotation2d.fromDegrees(-90)); + public static final Pose2d RED_LEFT_CLIMB_POSE_OFFSETTED = new Pose2d(15.474, 2.937, + Rotation2d.fromDegrees(-90)); public static final PhotonCamera ORANGE_PHOTON_CAM = new PhotonCamera("Back Left Camera"); public static final PhotonCamera BLACK_PHOTON_CAM = new PhotonCamera("Back Right Camera"); @@ -165,16 +179,17 @@ public final class Constants { public static final double HUB_X_POSE_BLUE = 4.625; public static final double HUB_Y_POSE_BLUE = 4.03; - public static final double HUB_X_POSE_RED = 4.625; - public static final double HUB_Y_POSE_RED = 0; + public static final double HUB_X_POSE_RED = 11.915; + public static final double HUB_Y_POSE_RED = 4.03; public static final Transform3d ROBOT_TO_BACK_LEFT_CAM = new Transform3d( - new Translation3d(-Units.inchesToMeters(12.75), Units.inchesToMeters(6.25), Units.inchesToMeters(13.1875)), - new Rotation3d(0, 0, Math.toRadians(-205))); + new Translation3d(-Units.inchesToMeters(12.75), Units.inchesToMeters(6.25), + Units.inchesToMeters(13.1875)), + new Rotation3d(0, 0, Math.toRadians(205))); public static final Transform3d ROBOT_TO_BACK_RIGHT_CAM = new Transform3d( new Translation3d(-Units.inchesToMeters(12.75), -Units.inchesToMeters(6.25), Units.inchesToMeters(14)), - new Rotation3d(0, 0, Math.toRadians(200))); + new Rotation3d(0, 0, Math.toRadians(-200))); public static final Transform3d ROBOT_TO_FRONT_LEFT_CAM = new Transform3d( new Translation3d(Units.inchesToMeters(23.375), 0, Units.inchesToMeters(13)), @@ -202,6 +217,5 @@ public final class Constants { public static class LEDConstants { public static final int LED_PWM_PORT = 6; - } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0b128a3..9efa539 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,13 +4,17 @@ package frc.robot; +import java.util.logging.Logger; + import org.photonvision.targeting.PhotonPipelineResult; import com.pathplanner.lib.auto.AutoBuilder; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StructPublisher; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.Timer; @@ -44,6 +48,7 @@ public class Robot extends TimedRobot { private static NetworkTable table; private static GenericEntry distanceFromLimelight; + private static StructPublisher advantageScopePublisher = NetworkTableInstance.getDefault().getStructTopic("Robot Pose hahaha", Pose2d.struct).publish(); public Robot() { instance = this; @@ -99,13 +104,16 @@ public class Robot extends TimedRobot { // robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); + TargetingSubsystems.updateShooterRPM(m_robotContainer.getSwerveDrive().getPose()); + + SmartDashboard.putNumber("Estimated Shooter RPM", Constants.ShooterConstants.SHOOTER_RPM); //Constants.ShooterConstants.getRampAndIndexerMotorSpeed(); //Constants.IntakeConstants.updateIntakeWheelsRPM(); - Constants.ShooterConstants.updateShooterRPM(); + //Constants.ShooterConstants.updateShooterRPM(); TargetingSubsystems.getHubPoseTheta(m_robotContainer.getSwerveDrive()); //Constants.ShooterConstants.updateIndexerAndRampMotorRPM(); - + advantageScopePublisher.set(m_robotContainer.getSwerveDrive().getPose()); } /** diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0a4cf0f..78de8c3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,7 @@ package frc.robot; +import com.fasterxml.jackson.databind.util.Named; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; @@ -22,7 +23,9 @@ import edu.wpi.first.wpilibj.XboxController; 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.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.OperatorConstants; @@ -56,6 +59,8 @@ public class RobotContainer { // Replace with CommandPS4Controller or CommandJoystick if needed final CommandXboxController driverXbox = new CommandXboxController(0); final CommandXboxController operatorXbox = new CommandXboxController(1); + private final static CommandJoystick topButtons = new CommandJoystick(2); + // The robot's subsystems and commands are defined here... private static final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), @@ -66,8 +71,8 @@ public class RobotContainer { private final SendableChooser autoChooser; private final IntakeSubsystem m_IntakeSubsystem = new IntakeSubsystem(); - // private final TargetingSubsystems m_TargetingSubsystems = new - // TargetingSubsystems(); + private final TargetingSubsystems m_TargetingSubsystems = new + TargetingSubsystems(); private final ShooterSubsystem m_ShooterSubsystem = new ShooterSubsystem(); private final ClimberSubsystem m_ClimberSubsystem = new ClimberSubsystem(); /** @@ -139,16 +144,13 @@ public class RobotContainer { NamedCommands.registerCommand("test", Commands.print("I EXIST")); NamedCommands.registerCommand("Shoot_Fuel_Command", m_ShooterSubsystem.shootFuelCommand() - .andThen(m_IntakeSubsystem.startIntakeMotorCommand()) - .andThen(m_IntakeSubsystem.assistFuelIntakeCommand( - Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE, - Constants.IntakeConstants.INTAKE_MIDDLE_ENCODER_VALUE) - .repeatedly() - .withTimeout(2))); - NamedCommands.registerCommand("Deploy_Intake_Command", m_IntakeSubsystem - .goToPositionCommand(Constants.IntakeConstants.INTAKE_COLLECT_ENCODER_VALUE)); + .andThen(m_IntakeSubsystem.startIntakeMotorCommand())); + NamedCommands.registerCommand("Deploy_Intake_Command", m_IntakeSubsystem.deployIntakeCommand()); NamedCommands.registerCommand("Stop_Shooter_Command", m_ShooterSubsystem.stopShooterCommand()); NamedCommands.registerCommand("Lift_Robot_Command", m_ClimberSubsystem.liftRobotCommand()); + NamedCommands.registerCommand("Assist_Shooter", m_IntakeSubsystem.assistFuelIntakeCommand()); + NamedCommands.registerCommand("Lift_Robot", m_ClimberSubsystem.liftRobotCommand()); + // Have the autoChooser pull in all PathPlanner autos as options autoChooser = AutoBuilder.buildAutoChooser(); @@ -197,18 +199,18 @@ public class RobotContainer { // command for // full shooting system including linear actuators driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand()); - - driverXbox.rightBumper().onTrue(m_IntakeSubsystem.assistFuelIntakeCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_MIDDLE, Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY).repeatedly()); + driverXbox.rightBumper().whileTrue(m_IntakeSubsystem.assistFuelIntakeCommand().repeatedly()); + //driverXbox.rightBumper().onTrue(m_IntakeSubsystem.assistFuelIntakeCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_MIDDLE, Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY).repeatedly()); driverXbox.y().onTrue(m_ClimberSubsystem.lowerRobotCommand()); driverXbox.a().onTrue(m_ClimberSubsystem.liftRobotCommand()); - driverXbox.povDown().onTrue(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY)); - driverXbox.povUp().onTrue(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_RETRACT)); + //driverXbox.povDown().onTrue(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY)); + //driverXbox.povUp().onTrue(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_RETRACT)); //driverXbox.povLeft().onTrue(m_IntakeSubsystem.goToPositionCommand(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_MIDDLE)); - //driverXbox.povDown().onTrue(m_IntakeSubsystem.deployIntakeCommand()); - //driverXbox.povUp().onTrue(m_IntakeSubsystem.retractIntakeCommand()); + driverXbox.povDown().onTrue(m_IntakeSubsystem.deployIntakeCommand()); + driverXbox.povUp().onTrue(m_IntakeSubsystem.retractIntakeCommand()); //driverXbox.povRight().whileTrue(m_TargetingSubsystems.aimAtHubPose(drivebase, driverXbox)); // driverXbox.rightTrigger().onTrue(m_ShooterSubsystem.shootFuelCommand()); @@ -228,6 +230,15 @@ public class RobotContainer { operatorXbox.a().whileTrue(m_ShooterSubsystem.setIndexerAndRampMotorRPMCommand()) .onFalse(m_ShooterSubsystem.stopIndexerAndRampMotorCommand()); + topButtons.axisGreaterThan(1, 0.3).toggleOnTrue(m_IntakeSubsystem.rotateIntakeCommand(Constants.IntakeConstants.INTAKE_MANUAL_SPEED * -1)).toggleOnFalse(m_IntakeSubsystem.rotateIntakeCommand(0)); + topButtons.axisGreaterThan(1, -0.3).toggleOnTrue(m_IntakeSubsystem.rotateIntakeCommand(Constants.IntakeConstants.INTAKE_MANUAL_SPEED)).toggleOnFalse(m_IntakeSubsystem.rotateIntakeCommand(0)); + topButtons.button(3).onTrue(killAllCommand()); + topButtons.button(6).whileTrue(m_TargetingSubsystems.aimAtHubPose(drivebase, driverXbox)); + topButtons.button(1).onTrue(drivebase.driveToClimbPose(Constants.TargetingConstants.BLUE_LEFT_CLIMB_POSE_OFFSETTED, Constants.TargetingConstants.RED_LEFT_CLIMB_POSE_OFFSETTED)); + topButtons.button(2).onTrue(drivebase.driveToClimbPose(Constants.TargetingConstants.BLUE_RIGHT_CLIMB_POSE_OFFSETTED, Constants.TargetingConstants.RED_RIGHT_CLIMB_POSE_OFFSETTED)); + + //topButtons.button(1).onTrue(drivebase.driveToPose(Constants.)) + if (RobotBase.isSimulation()) { drivebase.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard); } else { @@ -302,4 +313,12 @@ public class RobotContainer { return drivebase; } + public void killAll() { + CommandScheduler.getInstance().cancelAll(); + } + + public Command killAllCommand(){ + return Commands.runOnce(()->killAll()); + } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 78adf31..2ef8428 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -49,18 +49,28 @@ public class IntakeSubsystem extends SubsystemBase { Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I, Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D, m_Constraints); intakeRotatorProfiledPIDController.setTolerance(0.05); + //intakeRotatorProfiledPIDController.setGoal(Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_RETRACT); intakeRotatorConfig.closedLoop + //Slot 0 .p(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_P) .i(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_I) .d(Constants.IntakeConstants.IntakeRotatorPID.INTAKE_ROTATOR_D) - .p(.13, ClosedLoopSlot.kSlot1) + + //Slot 1 + .p(.05, ClosedLoopSlot.kSlot1) .i(0, ClosedLoopSlot.kSlot1) - .d(0, ClosedLoopSlot.kSlot1); + .d(.05, ClosedLoopSlot.kSlot1) + + //Slot 2 + .p(1,ClosedLoopSlot.kSlot2) + .i(.001, ClosedLoopSlot.kSlot2) + .d(0.1, ClosedLoopSlot.kSlot2); + intakeRotatorMotor.configure(intakeRotatorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, com.revrobotics.PersistMode.kNoPersistParameters); intakeRotatorConfig.smartCurrentLimit(40); - //intakeRotatorPIDController = intakeRotatorMotor.getClosedLoopController(); + intakeRotatorPIDController = intakeRotatorMotor.getClosedLoopController(); intakeWheelsMotorConfig.closedLoop.pid(Constants.IntakeConstants.INTAKE_MOTOR_P, @@ -68,14 +78,29 @@ public class IntakeSubsystem extends SubsystemBase { Constants.IntakeConstants.INTAKE_MOTOR_D); intakeWheelsMotor.configure(intakeWheelsMotorConfig, com.revrobotics.ResetMode.kNoResetSafeParameters, com.revrobotics.PersistMode.kNoPersistParameters); + intakeRotatorMotor.getEncoder().setPosition(-5); intakeWheelsMotorPIDController = intakeWheelsMotor.getClosedLoopController(); } - public void goToPosition(double goalPosition) { - double pidVal = intakeRotatorProfiledPIDController.calculate(encoderValue, goalPosition); - intakeRotatorMotor.setVoltage(-pidVal * 12); + /* public void goToPosition(double goalPosition) { + intakeRotatorProfiledPIDController.setGoal(goalPosition); + //double pidVal = intakeRotatorProfiledPIDController.calculate(encoderValue, goalPosition); + } + + public Command goToPositionCommand(double goalPosition) { + return runOnce(() -> goToPosition(goalPosition)); + } + */ + + public void rotateIntake(double speed) + { + intakeRotatorMotor.set(speed); } + public Command rotateIntakeCommand(double speed) + { + return runOnce(()->intakeRotatorMotor.set(speed)); + } public void startIntakeMotor() { intakeWheelsMotorPIDController.setSetpoint(Constants.IntakeConstants.INTAKE_WHEELS_MOTOR_RPM, @@ -103,14 +128,10 @@ public class IntakeSubsystem extends SubsystemBase { return runOnce(() -> stopIntakeMotor()); } - public Command goToPositionCommand(double goalPosition) { - return runOnce(() -> goToPosition(goalPosition)); - } - - public Command assistFuelIntakeCommand(double deployedPosition, double assistPosition) { + /* public Command assistFuelIntakeCommand(double deployedPosition, double assistPosition) { return runOnce(() -> goToPositionCommand(assistPosition).andThen(new WaitCommand(1.5)) .andThen(goToPositionCommand(deployedPosition)).andThen(new WaitCommand(1.5))); - } + } */ public static void resetIntakeRotationEncoder() { intakeRotatorMotor.getEncoder().setPosition(-5); @@ -137,19 +158,20 @@ public class IntakeSubsystem extends SubsystemBase { public void assistFuelIntake() { intakeRotatorPIDController.setSetpoint(Constants.IntakeConstants. INTAKE_MIDDLE_ENCODER_VALUE, - ControlType.kPosition, ClosedLoopSlot.kSlot1); + ControlType.kPosition, ClosedLoopSlot.kSlot2); } public Command assistFuelIntakeCommand() { return runOnce(() -> assistFuelIntake()).andThen(new - WaitCommand(1.5)).andThen(deployIntakeCommand()) - .andThen(new WaitCommand(1.5)); + WaitCommand(1)).andThen(deployIntakeCommand()) + .andThen(new WaitCommand(1)); } @Override public void periodic() { - encoderValue = intakeRotatorEncoder.get(); + //encoderValue = intakeRotatorEncoder.get(); + //intakeRotatorMotor.setVoltage(intakeRotatorProfiledPIDController.calculate(encoderValue) * -12); SmartDashboard.putNumber("Intake Rotator Motor PID", intakeRotatorMotor.getEncoder().getPosition()); SmartDashboard.putNumber("Intake Rotator Encoder Value", intakeRotatorEncoder.get()); SmartDashboard.putNumber("Voltage Deploy", intakeRotatorProfiledPIDController.calculate(encoderValue, Constants.IntakeConstants.INTAKE_THROUGHBORE_ENCODER_DEPLOY)); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 528068f..60b89ec 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -205,18 +205,18 @@ public class ShooterSubsystem extends SubsystemBase { }*/ - //public Command shootFuelCommand() { - // return runOnce(() -> setShooterMotorsRPM()).andThen(new WaitCommand(2)) - // .andThen(() -> setIndexerAndRampMotorRPM()); - // }; + public Command shootFuelCommand() { + return runOnce(() -> setShooterMotorsRPM()).andThen(new WaitCommand(2)) + .andThen(() -> setIndexerAndRampMotorRPM()); + }; - public Command shootFuelCommand() { + /* public Command shootFuelCommand() { return runOnce(() -> setShooterMotorsRPM()) .andThen(new WaitUntilCommand(() -> { return (getShooterMotorRPM() <= Constants.ShooterConstants.SHOOTER_RPM); })) .andThen(() -> setIndexerAndRampMotorRPM()); - }; + };*/ public void stopShooters() { diff --git a/src/main/java/frc/robot/subsystems/TargetingSubsystems.java b/src/main/java/frc/robot/subsystems/TargetingSubsystems.java index cfe2638..b37119d 100644 --- a/src/main/java/frc/robot/subsystems/TargetingSubsystems.java +++ b/src/main/java/frc/robot/subsystems/TargetingSubsystems.java @@ -53,15 +53,21 @@ import frc.robot.Constants; public class TargetingSubsystems extends SubsystemBase { - PIDController photonAimPIDController = new PIDController(3, 0, 0.001); + PIDController photonAimPIDController = new PIDController(3, 0.01, 0); - static Pose2d allianceHubPose; + static Pose2d allianceHubPose = new Pose2d(); public static Rotation2d hubThetaPose = new Rotation2d(); public static Optional alliance = DriverStation.getAlliance(); + private static ShuffleboardTab cameras; public TargetingSubsystems() { photonAimPIDController.enableContinuousInput(-Math.PI, Math.PI); - + cameras = Shuffleboard.getTab("Vision"); + // cameras.addCamera("Rear Left Camera","Rear Left + // Camera","http://photonvision.local:5800/#/cameras"); + // cameras.addCamera("Rear Right Camera", "Rear Right Camera", + // "http://photonvision.local:5800/#/cameras"); + } Pose2d currentRobotPose; @@ -108,16 +114,12 @@ public class TargetingSubsystems extends SubsystemBase { return new RunCommand(() -> { currentRobotPose = swerveDrive.getPose(); - - // Transform2d errorFromDesiredPose = desiredPose.minus(currentRobotPose); - Rotation2d angleDifference = PhotonUtils.getYawToPose(currentRobotPose, allianceHubPose); - double angleSpeed = photonAimPIDController.calculate(currentRobotPose.getRotation().getRadians(), - angleDifference.getRadians()); + double angleSpeed = photonAimPIDController.calculate(currentRobotPose.getRotation().getRadians(), allianceHubPose.getRotation().getRadians()); angleSpeed = MathUtil.clamp(angleSpeed, -3.0, 3.0); @@ -142,29 +144,48 @@ public class TargetingSubsystems extends SubsystemBase { }, swerveDrive); } + public static void getHubPoseTheta(SwerveSubsystem swerveDrive) { + if (alliance.isPresent()) { + if (alliance.get() == Alliance.Blue) { + hubThetaPose = new Rotation2d( + Math.atan2(Constants.TargetingConstants.HUB_Y_POSE_BLUE - swerveDrive.getPose().getY(), Constants.TargetingConstants.HUB_X_POSE_BLUE - swerveDrive.getPose().getX())); - public static void getHubPoseTheta(SwerveSubsystem swerveDrive) - { - if(alliance.isPresent()){ - if (alliance.get() == Alliance.Blue){ - hubThetaPose = new Rotation2d(Math.atan2(Constants.TargetingConstants.HUB_Y_POSE_BLUE - swerveDrive.getPose().getY(), Constants.TargetingConstants.HUB_X_POSE_BLUE - swerveDrive.getPose().getX())); - - allianceHubPose = new Pose2d(Constants.TargetingConstants.HUB_X_POSE_BLUE, Constants.TargetingConstants.HUB_Y_POSE_BLUE, hubThetaPose); + allianceHubPose = new Pose2d(Constants.TargetingConstants.HUB_X_POSE_BLUE, + Constants.TargetingConstants.HUB_Y_POSE_BLUE, hubThetaPose); } - else{ - hubThetaPose = new Rotation2d(Math.atan2(Constants.TargetingConstants.HUB_Y_POSE_RED - swerveDrive.getPose().getY(), Constants.TargetingConstants.HUB_X_POSE_RED - swerveDrive.getPose().getX())); - allianceHubPose = new Pose2d(Constants.TargetingConstants.HUB_X_POSE_RED, Constants.TargetingConstants.HUB_Y_POSE_RED, hubThetaPose); + else { + hubThetaPose = new Rotation2d( + Math.atan2(Constants.TargetingConstants.HUB_Y_POSE_RED - swerveDrive.getPose().getY(), + Constants.TargetingConstants.HUB_X_POSE_RED - swerveDrive.getPose().getX())); + allianceHubPose = new Pose2d(Constants.TargetingConstants.HUB_X_POSE_RED, + Constants.TargetingConstants.HUB_Y_POSE_RED, hubThetaPose); } - } + } } - + + + public static void updateShooterRPM(Pose2d currentRobotPose) { + double distance = PhotonUtils.getDistanceToPose(currentRobotPose, + allianceHubPose); + Constants.ShooterConstants.SHOOTER_RPM = -300 * Math.pow(distance, 3) + + 1221.475 * Math.pow(distance, 2) + - 1955.00131 * distance + - 1630.07168; + + } + + @Override public void periodic() { - - SmartDashboard.putString("Target Hub Pose", allianceHubPose.getX() + "\n" + allianceHubPose.getY() + "\n" + allianceHubPose.getRotation()) ; - + alliance = DriverStation.getAlliance(); + SmartDashboard.putString("Target Hub Pose", + allianceHubPose.getX() + " " + allianceHubPose.getY() + " " + allianceHubPose.getRotation()); + + SmartDashboard.putString("Hub Pose", "x: " + allianceHubPose.getMeasureX() + " y: " + allianceHubPose.getY() + + " angle: " + allianceHubPose.getRotation()); + /* * Shuffleboard.getTab("Vision").add("Photon Vision Yaw Value", * photonVision.getLatestResult().getBestTarget().getYaw()); @@ -174,10 +195,7 @@ public class TargetingSubsystems extends SubsystemBase { * LimelightHelpers.getTX("limelight")); * Shuffleboard.getTab("Vision").add("Limelight April Tag ID", * LimelightHelpers.getFiducialID("limelight")); - * Shuffleboard.getTab("Vision").addCamera("Limelight", "limelight", null); - * Shuffleboard.getTab("Vision").addCamera("Photon", - * "Arducam_OV9281_USB_Camera", - * "http://photonvision.local:5800"); */ + } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 61ec661..ee84c9b 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -27,6 +27,9 @@ import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +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.SubsystemBase; @@ -36,6 +39,7 @@ import frc.robot.subsystems.TargetingSubsystems; import frc.robot.subsystems.swervedrive.Vision.Cameras; import java.io.File; import java.io.IOException; +import java.lang.reflect.Field; import java.util.Arrays; import java.util.Optional; import java.util.concurrent.atomic.AtomicReference; @@ -70,6 +74,7 @@ public class SwerveSubsystem extends SubsystemBase { */ private Vision vision; + private final Field2d rebuiltField = new Field2d(); /** * Initialize {@link SwerveDrive} with the directory provided. * @@ -118,6 +123,7 @@ public class SwerveSubsystem extends SubsystemBase { swerveDrive.stopOdometryThread(); } setupPathPlanner(); + SmartDashboard.putData("Rebuilt Field", rebuiltField); } /** @@ -149,6 +155,11 @@ public class SwerveSubsystem extends SubsystemBase { swerveDrive.updateOdometry(); vision.updatePoseEstimation(swerveDrive); } + + SmartDashboard.putString("Robot Pose ", "x: " + swerveDrive.getPose().getX() + "\ny: " + + swerveDrive.getPose().getY() + "\nrot: " + swerveDrive.getPose().getRotation()); + + rebuiltField.setRobotPose(getPose()); } @Override @@ -275,6 +286,28 @@ public class SwerveSubsystem extends SubsystemBase { ); } + public Command driveToClimbPose(Pose2d blueAlliancePose, Pose2d redAlliancePose) { + + Pose2d goal; + Optional alliance = DriverStation.getAlliance(); + if (alliance.get() == Alliance.Blue) { + goal = blueAlliancePose; + } else { + goal = redAlliancePose; + } + // Create the constraints to use while pathfinding + PathConstraints constraints = new PathConstraints( + swerveDrive.getMaximumChassisVelocity(), 4.0, + swerveDrive.getMaximumChassisAngularVelocity(), Units.degreesToRadians(720)); + + // Since AutoBuilder is configured, we can use it to build pathfinding commands + return AutoBuilder.pathfindToPose( + goal, + constraints, + edu.wpi.first.units.Units.MetersPerSecond.of(0) // Goal end velocity in meters/sec + ); + } + /** * Drive with {@link SwerveSetpointGenerator} from 254, implemented by * PathPlanner. diff --git a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java index 87947d4..3b4d08d 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/Vision.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/Vision.java @@ -319,14 +319,15 @@ public class Vision { BACK_RIGHT_CAM("Rear Right Camera", Constants.TargetingConstants.ROBOT_TO_BACK_RIGHT_CAM.getRotation(), Constants.TargetingConstants.ROBOT_TO_BACK_RIGHT_CAM.getTranslation(), - VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)), + VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)); /** * Front Left Camera */ - FRONT_LEFT_CAM("Front Left Camera", + /*FRONT_LEFT_CAM("Front Left Camera", Constants.TargetingConstants.ROBOT_TO_FRONT_LEFT_CAM.getRotation(), Constants.TargetingConstants.ROBOT_TO_FRONT_LEFT_CAM.getTranslation(), - VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1)); + VecBuilder.fill(4, 4, 8), VecBuilder.fill(0.5, 0.5, 1));*/ + /** * Front Right Camera