From 4af6a0d87fab6e63d1cb30c4959e65c908757abf Mon Sep 17 00:00:00 2001 From: James Ding Date: Wed, 6 Mar 2024 15:29:25 -0800 Subject: [PATCH] Add basic autonomous points --- simgui-ds.json | 3 +- src/main/deploy/pathplanner/navgrid.json | 2 +- src/main/deploy/pathplanner/paths/Amp.path | 49 ----------- .../deploy/pathplanner/paths/Playground.path | 12 +-- src/main/java/org/rambots/AutoConstants.kt | 11 --- src/main/java/org/rambots/BuildConstants.java | 10 +-- src/main/java/org/rambots/RobotContainer.kt | 82 +++++++++---------- .../java/org/rambots/auto/AutoConstants.kt | 28 +++++++ .../org/rambots/auto/SourcePosePositions.kt | 9 ++ .../java/org/rambots/commands/AmpScoring.kt | 15 ++-- .../java/org/rambots/commands/SourceIntake.kt | 26 ++++++ .../org/rambots/subsystems/drive/Drive.java | 3 +- 12 files changed, 125 insertions(+), 125 deletions(-) delete mode 100644 src/main/deploy/pathplanner/paths/Amp.path delete mode 100644 src/main/java/org/rambots/AutoConstants.kt create mode 100644 src/main/java/org/rambots/auto/AutoConstants.kt create mode 100644 src/main/java/org/rambots/auto/SourcePosePositions.kt create mode 100644 src/main/java/org/rambots/commands/SourceIntake.kt diff --git a/simgui-ds.json b/simgui-ds.json index c4b7efd..647d10f 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -91,8 +91,7 @@ ], "robotJoysticks": [ { - "guid": "78696e70757401000000000000000000", - "useGamepad": true + "guid": "030000004c050000e60c000000000000" } ] } diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json index e74ee28..e9b200c 100644 --- a/src/main/deploy/pathplanner/navgrid.json +++ b/src/main/deploy/pathplanner/navgrid.json @@ -1 +1 @@ -{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,true,true,true,true,true,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,true,true,true,true,true,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,true,true,true,true,true,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,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,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,true,true,true,true,true,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,true,true,true,true,true,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,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,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,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,true,true,true,true,true,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,true,true,true,true,true,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,true,true,true,true,true,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,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false]]} \ No newline at end of file +{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,true,true,true,true,true,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,true,true,true,true,true,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,true,true,true,true,true,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,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,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,true,true,true,true,true,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,true,true,true,true,true,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,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,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,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,true,true,true,true,true,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,true,true,true,true,true,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,true,true,true,true,true,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,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,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,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false],[false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Amp.path b/src/main/deploy/pathplanner/paths/Amp.path deleted file mode 100644 index 5b74233..0000000 --- a/src/main/deploy/pathplanner/paths/Amp.path +++ /dev/null @@ -1,49 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 2.888049050119454, - "y": 6.856133569918385 - }, - "prevControl": null, - "nextControl": { - "x": 2.526699430623062, - "y": 6.856133569918385 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.8230185926564035, - "y": 7.246010790953966 - }, - "prevControl": { - "x": 2.0702578059960404, - "y": 6.818096767866134 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 90.0, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": null, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Playground.path b/src/main/deploy/pathplanner/paths/Playground.path index 3787a86..a93c000 100644 --- a/src/main/deploy/pathplanner/paths/Playground.path +++ b/src/main/deploy/pathplanner/paths/Playground.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 4.0, - "y": 6.0 + "x": 15.916953193479339, + "y": 1.2159744963458083 }, "prevControl": { - "x": 3.0, - "y": 6.0 + "x": 14.916953193479339, + "y": 1.2159744963458083 }, "nextControl": null, "isLocked": false, @@ -39,13 +39,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": 120.0, "rotateFast": false }, "reversed": false, "folder": null, "previewStartingState": { - "rotation": -59.9781887324857, + "rotation": -90.0, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/java/org/rambots/AutoConstants.kt b/src/main/java/org/rambots/AutoConstants.kt deleted file mode 100644 index d7e394f..0000000 --- a/src/main/java/org/rambots/AutoConstants.kt +++ /dev/null @@ -1,11 +0,0 @@ -package org.rambots - -import edu.wpi.first.math.geometry.Pose2d -import edu.wpi.first.math.geometry.Rotation2d -import edu.wpi.first.math.geometry.Translation2d - -object AutoConstants { - - val ampScorePose = Pose2d(Translation2d(1.82, 7.7), Rotation2d.fromDegrees(-90.0)) - -} \ No newline at end of file diff --git a/src/main/java/org/rambots/BuildConstants.java b/src/main/java/org/rambots/BuildConstants.java index e9ee355..041ca90 100644 --- a/src/main/java/org/rambots/BuildConstants.java +++ b/src/main/java/org/rambots/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "frc-2024"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 8; - public static final String GIT_SHA = "cd46b92a3c9db0369f3c1f26dd9d6a937b0f47bd"; - public static final String GIT_DATE = "2024-03-05 09:15:24 PST"; + public static final int GIT_REVISION = 9; + public static final String GIT_SHA = "e235e0327a4c8b1b2be006c6080e1000250747ba"; + public static final String GIT_DATE = "2024-03-05 18:20:38 PST"; public static final String GIT_BRANCH = "james/swerve"; - public static final String BUILD_DATE = "2024-03-05 16:08:22 PST"; - public static final long BUILD_UNIX_TIME = 1709683702503L; + public static final String BUILD_DATE = "2024-03-06 15:22:24 PST"; + public static final long BUILD_UNIX_TIME = 1709767344100L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/org/rambots/RobotContainer.kt b/src/main/java/org/rambots/RobotContainer.kt index 4f14b62..c7fd0f2 100644 --- a/src/main/java/org/rambots/RobotContainer.kt +++ b/src/main/java/org/rambots/RobotContainer.kt @@ -14,14 +14,14 @@ package org.rambots import com.pathplanner.lib.auto.AutoBuilder import edu.wpi.first.math.geometry.* +import edu.wpi.first.wpilibj.PS5Controller import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands +import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller import edu.wpi.first.wpilibj2.command.button.CommandXboxController import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine import org.littletonrobotics.junction.networktables.LoggedDashboardChooser -import org.rambots.commands.DriveCommands -import org.rambots.commands.DriveToPoint -import org.rambots.commands.PathFinderAndFollow +import org.rambots.commands.* import org.rambots.subsystems.drive.* import org.rambots.subsystems.drive.DriveConstants.moduleConfigs import org.rambots.subsystems.vision.AprilTagVision @@ -46,7 +46,7 @@ object RobotContainer { private val driveController = DriveController() // Controller - private val controller = CommandXboxController(0) + private val controller = CommandPS5Controller(0) // Dashboard inputs private val autoChooser: LoggedDashboardChooser @@ -147,11 +147,9 @@ object RobotContainer { // flywheel.sysIdDynamic(SysIdRoutine.Direction.kReverse)); // Configure the button bindings - aprilTagVision.setDataInterfaces(Consumer { visionData: List? -> - drive.addVisionData( - visionData - ) - }) + aprilTagVision.setDataInterfaces { visionData: List? -> + drive.addVisionData(visionData) + } driveController.setPoseSupplier { drive.pose } driveController.disableHeadingControl() configureButtonBindings() @@ -162,41 +160,37 @@ object RobotContainer { * instantiating a [GenericHID] or one of its subclasses ([ ] or [XboxController]), and then passing it to a [ ]. */ private fun configureButtonBindings() { - drive.defaultCommand = DriveCommands.joystickDrive( - drive, - driveController, - { -controller.leftY }, - { -controller.leftX }, - { -controller.rightX }) - - controller.leftBumper().whileTrue(Commands.runOnce({ driveController.toggleDriveMode() })) - - controller.a().whileTrue(PathFinderAndFollow(driveController.driveModeType)) - - controller - .b() - .whileTrue( - Commands.startEnd( - { driveController.enableHeadingControl() }, { driveController.disableHeadingControl() }) - ) - - controller - .y() - .whileTrue( - Commands.runOnce( - { - drive.setAutoStartPose( - Pose2d(Translation2d(4.0, 5.0), Rotation2d.fromDegrees(0.0)) - ) - }) - ) - controller - .povDown() - .whileTrue( - DriveToPoint( - drive, Pose2d(Translation2d(2.954, 3.621), Rotation2d.fromRadians(2.617)) - ) - ) + drive.defaultCommand = DriveCommands.joystickDrive(drive, driveController, { -controller.leftY }, { -controller.leftX }, { -controller.rightX }) + + controller.L1().whileTrue(AmpScoring()) + controller.R2().whileTrue(SourceIntake()) + +// controller.a().whileTrue(PathFinderAndFollow(driveController.driveModeType)) + +// controller +// .b() +// .whileTrue( +// Commands.startEnd( +// { driveController.enableHeadingControl() }, { driveController.disableHeadingControl() }) +// ) +// +// controller +// .y() +// .whileTrue( +// Commands.runOnce( +// { +// drive.setAutoStartPose( +// Pose2d(Translation2d(4.0, 5.0), Rotation2d.fromDegrees(0.0)) +// ) +// }) +// ) +// controller +// .povDown() +// .whileTrue( +// DriveToPoint( +// drive, Pose2d(Translation2d(2.954, 3.621), Rotation2d.fromRadians(2.617)) +// ) +// ) // // controller // .povUp() diff --git a/src/main/java/org/rambots/auto/AutoConstants.kt b/src/main/java/org/rambots/auto/AutoConstants.kt new file mode 100644 index 0000000..4ead5c9 --- /dev/null +++ b/src/main/java/org/rambots/auto/AutoConstants.kt @@ -0,0 +1,28 @@ +package org.rambots.auto + +import com.pathplanner.lib.path.PathConstraints +import edu.wpi.first.math.geometry.Pose2d +import edu.wpi.first.math.geometry.Rotation2d +import edu.wpi.first.math.geometry.Translation2d +import edu.wpi.first.math.util.Units +import org.rambots.util.AllianceFlipUtil + +object AutoConstants { + + val pathConstraints = PathConstraints(4.33, 8.0, Units.degreesToRadians(540.0), Units.degreesToRadians(720.0)) + + val ampScorePose: Pose2d get() = AllianceFlipUtil.apply(Pose2d(Translation2d(1.82, 7.7), Rotation2d.fromDegrees(-90.0))) + + val sourceDirectIntake get() = SourcePosePositions( + AllianceFlipUtil.apply(Pose2d(Translation2d(15.92, 1.22), Rotation2d.fromDegrees(120.0))), + AllianceFlipUtil.apply(Pose2d(Translation2d(15.42, 0.93), Rotation2d.fromDegrees(120.0))), + AllianceFlipUtil.apply(Pose2d(Translation2d(14.95, 0.64), Rotation2d.fromDegrees(120.0))) + ) + + val sourceGroundIntake get() = SourcePosePositions( + AllianceFlipUtil.apply(Pose2d(Translation2d(1.82, 7.7), Rotation2d.fromDegrees(120.0))), + AllianceFlipUtil.apply(Pose2d(Translation2d(1.82, 7.7), Rotation2d.fromDegrees(120.0))), + AllianceFlipUtil.apply(Pose2d(Translation2d(1.82, 7.7), Rotation2d.fromDegrees(120.0))) + ) + +} \ No newline at end of file diff --git a/src/main/java/org/rambots/auto/SourcePosePositions.kt b/src/main/java/org/rambots/auto/SourcePosePositions.kt new file mode 100644 index 0000000..e546d43 --- /dev/null +++ b/src/main/java/org/rambots/auto/SourcePosePositions.kt @@ -0,0 +1,9 @@ +package org.rambots.auto + +import edu.wpi.first.math.geometry.Pose2d + +data class SourcePosePositions( + val left: Pose2d, + val center: Pose2d, + val right: Pose2d +) diff --git a/src/main/java/org/rambots/commands/AmpScoring.kt b/src/main/java/org/rambots/commands/AmpScoring.kt index e55019d..ab95ebb 100644 --- a/src/main/java/org/rambots/commands/AmpScoring.kt +++ b/src/main/java/org/rambots/commands/AmpScoring.kt @@ -1,23 +1,28 @@ package org.rambots.commands +import com.pathplanner.lib.auto.AutoBuilder import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.Commands +import org.rambots.auto.AutoConstants class AmpScoring: Command() { + private val pathRun get() = AutoBuilder.pathfindToPose(AutoConstants.ampScorePose, AutoConstants.pathConstraints) + private var scoreCommand = Commands.sequence(pathRun) + override fun initialize() { super.initialize() - } - - override fun execute() { - super.execute() + scoreCommand = Commands.sequence(pathRun) + scoreCommand.schedule() } override fun end(interrupted: Boolean) { super.end(interrupted) + scoreCommand.cancel() } override fun isFinished(): Boolean { - return super.isFinished() + return scoreCommand.isFinished } } \ No newline at end of file diff --git a/src/main/java/org/rambots/commands/SourceIntake.kt b/src/main/java/org/rambots/commands/SourceIntake.kt new file mode 100644 index 0000000..bbe0040 --- /dev/null +++ b/src/main/java/org/rambots/commands/SourceIntake.kt @@ -0,0 +1,26 @@ +package org.rambots.commands + +import com.pathplanner.lib.auto.AutoBuilder +import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.Commands +import org.rambots.auto.AutoConstants + +class SourceIntake: Command() { + private val pathRun get() = AutoBuilder.pathfindToPose(AutoConstants.sourceDirectIntake.left, AutoConstants.pathConstraints) + private var scoreCommand = Commands.sequence(pathRun) + + override fun initialize() { + super.initialize() + scoreCommand = Commands.sequence(pathRun) + scoreCommand.schedule() + } + + override fun end(interrupted: Boolean) { + super.end(interrupted) + scoreCommand.cancel() + } + + override fun isFinished(): Boolean { + return scoreCommand.isFinished + } +} \ No newline at end of file diff --git a/src/main/java/org/rambots/subsystems/drive/Drive.java b/src/main/java/org/rambots/subsystems/drive/Drive.java index 6d0692f..c8f3c3d 100644 --- a/src/main/java/org/rambots/subsystems/drive/Drive.java +++ b/src/main/java/org/rambots/subsystems/drive/Drive.java @@ -340,8 +340,7 @@ public void setAutoStartPose(Pose2d pose) { * @param visionPose The pose of the robot as measured by the vision camera. * @param timestamp The timestamp of the vision measurement in seconds. */ - public void addVisionMeasurement( - Pose2d visionPose, double timestamp, Matrix visionMeasurementStdDevs) { + public void addVisionMeasurement(Pose2d visionPose, double timestamp, Matrix visionMeasurementStdDevs) { poseEstimator.addVisionMeasurement(visionPose, timestamp, visionMeasurementStdDevs); }