From 52d7659f3e0b619ccc2b9fa0051dd5e4474918d7 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Mon, 20 Jan 2025 15:37:37 -0700 Subject: [PATCH 1/4] make autos --- .../deploy/pathplanner/autos/New Auto.auto | 12 +++++ .../pathplanner/paths/Example Path.path | 20 +++---- .../deploy/pathplanner/paths/New Path.path | 54 +++++++++++++++++++ src/main/java/frc4388/robot/Constants.java | 4 +- .../java/frc4388/robot/RobotContainer.java | 5 ++ 5 files changed, 84 insertions(+), 11 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/New Path.path diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto index 70b7ab2..5a4618b 100644 --- a/src/main/deploy/pathplanner/autos/New Auto.auto +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -9,6 +9,18 @@ "data": { "pathName": "Example Path" } + }, + { + "type": "named", + "data": { + "name": "AutoGotoPosition" + } + }, + { + "type": "path", + "data": { + "pathName": "New Path" + } } ] } diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path index 946269c..4bd51e2 100644 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.415049342105263, - "y": 4.785115131578947 + "x": 15.106960227272728, + "y": 2.120426136363636 }, "prevControl": null, "nextControl": { - "x": 2.905756578947368, - "y": 4.794736842105262 + "x": 15.101585096858368, + "y": 2.396686756913594 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.8294407894736837, - "y": 5.862746710526316 + "x": 15.106960227272728, + "y": 4.005056818181817 }, "prevControl": { - "x": 3.0693256578947365, - "y": 5.872368421052631 + "x": 15.10799254912145, + "y": 3.6525397317875603 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": -179.09061955080082 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -2.4366482468102095 + "rotation": 178.99491399474587 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 0000000..c729144 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 14.398977272727272, + "y": 4.015028409090909 + }, + "prevControl": null, + "nextControl": { + "x": 13.651107954545454, + "y": 4.005056818181817 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 15.525767045454545, + "y": 5.271448863636363 + }, + "prevControl": { + "x": 14.857670454545453, + "y": 5.690255681818181 + }, + "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": 177.06432655357875 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -178.8308606720925 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 25d3767..6027489 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -354,7 +354,9 @@ public final class Constants { public static final class FieldConstants { public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); - public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); + public static final double HORISONTAL_SCORING_POSITION_OFFSET = 0; + // public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // Positive is Right + public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(20); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e211436..2f9d7cc 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -35,6 +35,8 @@ import frc4388.utility.controller.VirtualController; import frc4388.robot.commands.GotoPositionCommand; import frc4388.robot.commands.Swerve.neoJoystickPlayback; import frc4388.robot.commands.Swerve.neoJoystickRecorder; + +import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; // Subsystems // import frc4388.robot.subsystems.LED; @@ -92,12 +94,15 @@ public class RobotContainer { () -> autoplaybackName.get(), // lastAutoName new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, true, false); + private Command AutoGotoPosition = new GotoPositionCommand(m_robotSwerveDrive, m_vision); /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + NamedCommands.registerCommand("AutoGotoPosition", AutoGotoPosition); + configureButtonBindings(); configureVirtualButtonBindings(); new DeferredBlock(() -> m_robotSwerveDrive.resetGyro()); From 4a4524382818cacb467032af9e7195ace9bd0e5e Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Thu, 23 Jan 2025 19:32:12 -0700 Subject: [PATCH 2/4] make those autos --- .../pathplanner/autos/Blue Bottom Cage.auto | 61 ++++++++++++++++ .../pathplanner/autos/Blue Center Cage.auto | 61 ++++++++++++++++ .../pathplanner/autos/Blue Top Cage.auto | 61 ++++++++++++++++ .../autos/Center Barge Bottom.auto | 61 ++++++++++++++++ .../pathplanner/autos/Center Barge Top.auto | 61 ++++++++++++++++ .../deploy/pathplanner/autos/New Auto.auto | 31 -------- .../pathplanner/autos/Red Bottom Cage.auto | 61 ++++++++++++++++ .../pathplanner/autos/Red Center Cage.auto | 61 ++++++++++++++++ .../pathplanner/autos/Red Top Cage.auto | 61 ++++++++++++++++ ...ath.path => Bottom Blue Cage to Reef.path} | 20 +++--- .../paths/Bottom Feed to Reef.path | 54 ++++++++++++++ .../paths/Bottom Red Cage to Reef.path | 54 ++++++++++++++ .../paths/Bottom Reef to Feed.path | 54 ++++++++++++++ ...le Path.path => Center Barge to Reef.path} | 0 .../paths/Center Blue Cage to Reef.path | 54 ++++++++++++++ .../paths/Center Red Cage to Reef.path | 54 ++++++++++++++ .../paths/Center Reef to Bottom Feed.path | 54 ++++++++++++++ .../paths/Center Reef to Feed.path | 70 +++++++++++++++++++ .../paths/Top Blue Cage to Reef.path | 54 ++++++++++++++ .../pathplanner/paths/Top Feed to Reef.path | 54 ++++++++++++++ .../paths/Top Red Cage to Reef.path | 54 ++++++++++++++ .../pathplanner/paths/Top Reef to Feed.path | 54 ++++++++++++++ src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/Robot.java | 1 + .../java/frc4388/robot/RobotContainer.java | 25 ++++--- .../frc4388/robot/subsystems/SwerveDrive.java | 14 ++-- 26 files changed, 1137 insertions(+), 54 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Blue Bottom Cage.auto create mode 100644 src/main/deploy/pathplanner/autos/Blue Center Cage.auto create mode 100644 src/main/deploy/pathplanner/autos/Blue Top Cage.auto create mode 100644 src/main/deploy/pathplanner/autos/Center Barge Bottom.auto create mode 100644 src/main/deploy/pathplanner/autos/Center Barge Top.auto delete mode 100644 src/main/deploy/pathplanner/autos/New Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/Red Bottom Cage.auto create mode 100644 src/main/deploy/pathplanner/autos/Red Center Cage.auto create mode 100644 src/main/deploy/pathplanner/autos/Red Top Cage.auto rename src/main/deploy/pathplanner/paths/{New Path.path => Bottom Blue Cage to Reef.path} (71%) create mode 100644 src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path create mode 100644 src/main/deploy/pathplanner/paths/Bottom Red Cage to Reef.path create mode 100644 src/main/deploy/pathplanner/paths/Bottom Reef to Feed.path rename src/main/deploy/pathplanner/paths/{Example Path.path => Center Barge to Reef.path} (100%) create mode 100644 src/main/deploy/pathplanner/paths/Center Blue Cage to Reef.path create mode 100644 src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path create mode 100644 src/main/deploy/pathplanner/paths/Center Reef to Bottom Feed.path create mode 100644 src/main/deploy/pathplanner/paths/Center Reef to Feed.path create mode 100644 src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path create mode 100644 src/main/deploy/pathplanner/paths/Top Feed to Reef.path create mode 100644 src/main/deploy/pathplanner/paths/Top Red Cage to Reef.path create mode 100644 src/main/deploy/pathplanner/paths/Top Reef to Feed.path diff --git a/src/main/deploy/pathplanner/autos/Blue Bottom Cage.auto b/src/main/deploy/pathplanner/autos/Blue Bottom Cage.auto new file mode 100644 index 0000000..98b8016 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Blue Bottom Cage.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Bottom Blue Cage to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Reef to Feed" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Feed to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Blue Center Cage.auto b/src/main/deploy/pathplanner/autos/Blue Center Cage.auto new file mode 100644 index 0000000..a7a32d6 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Blue Center Cage.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Blue Cage to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Reef to Feed" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Feed to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Blue Top Cage.auto b/src/main/deploy/pathplanner/autos/Blue Top Cage.auto new file mode 100644 index 0000000..3acc852 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Blue Top Cage.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Top Blue Cage to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Reef to Feed" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Feed to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Center Barge Bottom.auto b/src/main/deploy/pathplanner/autos/Center Barge Bottom.auto new file mode 100644 index 0000000..70a439a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center Barge Bottom.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Barge to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Reef to Bottom Feed" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Center Barge Top.auto b/src/main/deploy/pathplanner/autos/Center Barge Top.auto new file mode 100644 index 0000000..a5f3397 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Center Barge Top.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Barge to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Reef to Feed" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Feed to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto deleted file mode 100644 index 5a4618b..0000000 --- a/src/main/deploy/pathplanner/autos/New Auto.auto +++ /dev/null @@ -1,31 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Example Path" - } - }, - { - "type": "named", - "data": { - "name": "AutoGotoPosition" - } - }, - { - "type": "path", - "data": { - "pathName": "New Path" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Red Bottom Cage.auto b/src/main/deploy/pathplanner/autos/Red Bottom Cage.auto new file mode 100644 index 0000000..658c0b4 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Red Bottom Cage.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Bottom Red Cage to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Reef to Feed" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Red Center Cage.auto b/src/main/deploy/pathplanner/autos/Red Center Cage.auto new file mode 100644 index 0000000..47338f7 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Red Center Cage.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Center Red Cage to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Reef to Feed" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Red Top Cage.auto b/src/main/deploy/pathplanner/autos/Red Top Cage.auto new file mode 100644 index 0000000..429e075 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Red Top Cage.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Top Red Cage to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Reef to Feed" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Bottom Feed to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/Bottom Blue Cage to Reef.path similarity index 71% rename from src/main/deploy/pathplanner/paths/New Path.path rename to src/main/deploy/pathplanner/paths/Bottom Blue Cage to Reef.path index c729144..a5ccc16 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/Bottom Blue Cage to Reef.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 14.398977272727272, - "y": 4.015028409090909 + "x": 7.616287878787879, + "y": 5.092234848484848 }, "prevControl": null, "nextControl": { - "x": 13.651107954545454, - "y": 4.005056818181817 + "x": 8.269913357759823, + "y": 5.035328772529629 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 15.525767045454545, - "y": 5.271448863636363 + "x": 5.370012626262627, + "y": 5.346338383838384 }, "prevControl": { - "x": 14.857670454545453, - "y": 5.690255681818181 + "x": 4.997649306769947, + "y": 5.369630479499745 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 177.06432655357875 + "rotation": -122.12499844038754 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -178.8308606720925 + "rotation": 178.97696981133208 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path new file mode 100644 index 0000000..1d37d81 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.3145202020202018, + "y": 0.9655934343434341 + }, + "prevControl": null, + "nextControl": { + "x": 2.115768324814807, + "y": 1.5107373181346522 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.692929292929293, + "y": 2.6833333333333327 + }, + "prevControl": { + "x": 3.11332763609877, + "y": 2.3131602797293294 + }, + "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": 59.82647997035557 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 54.32359177813805 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Red Cage to Reef.path b/src/main/deploy/pathplanner/paths/Bottom Red Cage to Reef.path new file mode 100644 index 0000000..6b2c3d3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bottom Red Cage to Reef.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.565467171717171, + "y": 0.8334595959595947 + }, + "prevControl": null, + "nextControl": { + "x": 8.123574009981496, + "y": 0.3592362071562901 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.3903409090909085, + "y": 2.6833333333333327 + }, + "prevControl": { + "x": 5.06636861749931, + "y": 2.9361778277713553 + }, + "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": 121.75948008481289 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 179.30130561701642 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Reef to Feed.path b/src/main/deploy/pathplanner/paths/Bottom Reef to Feed.path new file mode 100644 index 0000000..117ae7d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Bottom Reef to Feed.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.1565656565656575, + "y": 2.886616161616162 + }, + "prevControl": null, + "nextControl": { + "x": 5.532638888888889, + "y": 2.530871212121211 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.1823863636363636, + "y": 1.026578282828283 + }, + "prevControl": { + "x": 0.9229528762543462, + "y": 0.9200437072077402 + }, + "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": 54.0394828033551 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 119.16761337957772 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Center Barge to Reef.path similarity index 100% rename from src/main/deploy/pathplanner/paths/Example Path.path rename to src/main/deploy/pathplanner/paths/Center Barge to Reef.path diff --git a/src/main/deploy/pathplanner/paths/Center Blue Cage to Reef.path b/src/main/deploy/pathplanner/paths/Center Blue Cage to Reef.path new file mode 100644 index 0000000..05a8393 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Blue Cage to Reef.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.575631313127491, + "y": 6.139141414136251 + }, + "prevControl": null, + "nextControl": { + "x": 6.3210457316672, + "y": 6.004686892287159 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.197222222222222, + "y": 5.52929292929293 + }, + "prevControl": { + "x": 6.234912180385907, + "y": 5.781923450986739 + }, + "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": -121.60750224624891 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 179.44374777291927 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path b/src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path new file mode 100644 index 0000000..6a9c102 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.585795454545455, + "y": 1.9413510101010092 + }, + "prevControl": null, + "nextControl": { + "x": 6.5546611575005915, + "y": 2.13657272729177 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.319191919191919, + "y": 2.6833333333333327 + }, + "prevControl": { + "x": 5.563570773660507, + "y": 2.6306173444056773 + }, + "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": 118.92642583525368 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Reef to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Center Reef to Bottom Feed.path new file mode 100644 index 0000000..ad0027f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Reef to Bottom Feed.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.786742424242425, + "y": 4.035164141414142 + }, + "prevControl": null, + "nextControl": { + "x": 7.026767676767678, + "y": 2.185290404040403 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3755050505050503, + "y": 0.8842803030303026 + }, + "prevControl": { + "x": 1.5707222528658078, + "y": 1.040454064918909 + }, + "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": 50.964487101253106 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -178.99491399474581 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Reef to Feed.path b/src/main/deploy/pathplanner/paths/Center Reef to Feed.path new file mode 100644 index 0000000..ce674ff --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Center Reef to Feed.path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.817234848484849, + "y": 4.045328282828282 + }, + "prevControl": null, + "nextControl": { + "x": 6.62020202020202, + "y": 4.797474747474748 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.2175505050505055, + "y": 5.45814393939394 + }, + "prevControl": { + "x": 5.45268897997343, + "y": 5.373232823449551 + }, + "nextControl": { + "x": 3.388005050505051, + "y": 6.11881313131313 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.121401515151515, + "y": 7.003093434343434 + }, + "prevControl": { + "x": 0.32627122293847843, + "y": 7.229156472141377 + }, + "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": -52.30575953331084 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -177.95459151111288 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path b/src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path new file mode 100644 index 0000000..2a1b6ba --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.575631313131314, + "y": 7.287689393939393 + }, + "prevControl": null, + "nextControl": { + "x": 7.270707070707072, + "y": 7.013257575757576 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.309027777777779, + "y": 5.3768308080808085 + }, + "prevControl": { + "x": 4.945156854248459, + "y": 5.08761998976538 + }, + "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": -120.0183674276091 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 179.30972280213487 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Feed to Reef.path b/src/main/deploy/pathplanner/paths/Top Feed to Reef.path new file mode 100644 index 0000000..3c7f204 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Top Feed to Reef.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.263699494949495, + "y": 7.135227272727272 + }, + "prevControl": null, + "nextControl": { + "x": 1.6862557056350642, + "y": 6.8478211564289 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.6217803030303033, + "y": 5.417487373737374 + }, + "prevControl": { + "x": 3.111746968518841, + "y": 5.776730048021274 + }, + "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": -60.228308784269046 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -55.124671655397854 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Red Cage to Reef.path b/src/main/deploy/pathplanner/paths/Top Red Cage to Reef.path new file mode 100644 index 0000000..fe0d749 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Top Red Cage to Reef.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.595959595959596, + "y": 3.0289141414141416 + }, + "prevControl": null, + "nextControl": { + "x": 8.242449416720314, + "y": 3.1104783377417067 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.349684343434343, + "y": 2.6934974747474745 + }, + "prevControl": { + "x": 4.985594317577342, + "y": 2.6542548990156853 + }, + "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": 120.25643716352924 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -179.06080905426438 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Reef to Feed.path b/src/main/deploy/pathplanner/paths/Top Reef to Feed.path new file mode 100644 index 0000000..32f6914 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Top Reef to Feed.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.17689393939394, + "y": 5.224368686868687 + }, + "prevControl": null, + "nextControl": { + "x": 5.9669821266547345, + "y": 4.892012503312241 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.192550505050505, + "y": 7.0234217171717175 + }, + "prevControl": { + "x": 0.9300747161944047, + "y": 7.106516384157039 + }, + "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": -52.56142842766699 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -121.75948008481281 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 6027489..5be92c8 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -341,7 +341,7 @@ public final class Constants { public static final class VisionConstants { public static final String CAMERA_NAME = "Camera_Module_v1"; - public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413*0, .2794), new Rotation3d(0,0.52333,Math.PI)); + public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413, .2794), new Rotation3d(0,0.52333,Math.PI)); public static final double MIN_ESTIMATION_DISTANCE = 1; // Meters diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 18e54f5..0281e90 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -148,6 +148,7 @@ public class Robot extends TimedRobot { // continue until interrupted by another command, remove // this line or comment it out. if (m_autonomousCommand != null) { + CommandScheduler.getInstance().cancel(m_autonomousCommand); m_autonomousCommand.cancel(); } m_robotTime.startMatchTime(); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2f9d7cc..f705983 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -16,6 +16,8 @@ import java.util.List; import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc4388.utility.controller.XboxController; import frc4388.utility.controller.DeadbandedXboxController; import frc4388.robot.Constants.OIConstants; @@ -36,6 +38,8 @@ import frc4388.robot.commands.GotoPositionCommand; import frc4388.robot.commands.Swerve.neoJoystickPlayback; import frc4388.robot.commands.Swerve.neoJoystickRecorder; +import com.pathplanner.lib.auto.AutoBuilder; + import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; // Subsystems @@ -89,6 +93,7 @@ public class RobotContainer { // ! /* Autos */ private String lastAutoName = "defualt.auto"; + private final SendableChooser autoChooser; private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName); private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive, () -> autoplaybackName.get(), // lastAutoName @@ -120,6 +125,8 @@ public class RobotContainer { .withName("SwerveDrive DefaultCommand")); m_robotSwerveDrive.setToSlow(); + autoChooser = AutoBuilder.buildAutoChooser(); + SmartDashboard.putData("Auto Chooser", autoChooser); // this.subsystems.add(m_robotSwerveDrive); // this.subsystems.add(m_robotMap.leftFront); // this.subsystems.add(m_robotMap.rightFront); @@ -270,15 +277,17 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { + //return autoPlayback; - //return new GotoPositionCommand(m_robotSwerveDrive, m_vision); - try{ - // Load the path you want to follow using its name in the GUI - return new PathPlannerAuto("New Auto"); - } catch (Exception e) { - DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace()); - return Commands.none(); - } + //return new GotoPositionCommand(m_robotSwerveDrive, m_vision) + return autoChooser.getSelected(); + // try{ + // // Load the path you want to follow using its name in the GUI + // return new PathPlannerAuto("New Auto"); + // } catch (Exception e) { + // DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace()); + // return Commands.none(); + // } // zach told me to do the below comment //return new GotoPositionCommand(m_robotSwerveDrive, m_vision); // return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index f0410e9..0820891 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -32,6 +32,7 @@ import frc4388.utility.Status; import frc4388.utility.Subsystem; import frc4388.utility.Status.ReportLevel; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.config.PIDConstants; @@ -52,6 +53,8 @@ public class SwerveDrive extends Subsystem { public double autoSpeedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * 0.25; // cap auto performance to // 25% + public Pose2d initalPose2d = null; + public double rotTarget = 0.0; public Rotation2d orientRotTarget = new Rotation2d(); public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); @@ -75,11 +78,9 @@ public class SwerveDrive extends Subsystem { // DoubleSupplier a = () -> 1.d; AutoBuilder.configure( () -> { - var pose = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(new Pose2d()); - // pose = new Pose2d(pose.getX(), pose.getY(), pose.getRotation().times(-1)); - return pose;// getRotation().times(-1) + return swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()).orElse(initalPose2d); }, // Robot pose supplier - swerveDriveTrain::resetPose, // Method to reset odometry (will be called if your auto has a starting + this::setOdoPose, // Method to reset odometry (will be called if your auto has a starting // pose) () -> swerveDriveTrain.getState().Speeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE (speeds, feedforwards) -> swerveDriveTrain.setControl(new SwerveRequest.ApplyRobotSpeeds() @@ -108,6 +109,11 @@ public class SwerveDrive extends Subsystem { } + public void setOdoPose(Pose2d pose) { + initalPose2d = pose; + swerveDriveTrain.resetPose(pose); + } + // public void oneModuleTest(SwerveModule module, Translation2d leftStick, // Translation2d rightStick){ // // double ang = Math.atan2(rightStick.getY(), rightStick.getX()); From a490b253113f2b0d8ca8abb016c3850991ae9d9d Mon Sep 17 00:00:00 2001 From: BallisticCrusader <121452254+BallisticCrusader@users.noreply.github.com> Date: Fri, 24 Jan 2025 10:56:55 -0800 Subject: [PATCH 3/4] Added Taxi Auto Created a taxi path and auto. --- src/main/deploy/pathplanner/autos/Taxi.auto | 19 ++++++++ src/main/deploy/pathplanner/paths/Taxi.path | 54 +++++++++++++++++++++ 2 files changed, 73 insertions(+) create mode 100644 src/main/deploy/pathplanner/autos/Taxi.auto create mode 100644 src/main/deploy/pathplanner/paths/Taxi.path diff --git a/src/main/deploy/pathplanner/autos/Taxi.auto b/src/main/deploy/pathplanner/autos/Taxi.auto new file mode 100644 index 0000000..db06ab4 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Taxi.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Taxi" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Taxi.path b/src/main/deploy/pathplanner/paths/Taxi.path new file mode 100644 index 0000000..f5b4725 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Taxi.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.576229508196722, + "y": 7.273668032786885 + }, + "prevControl": null, + "nextControl": { + "x": 8.050548646741815, + "y": 7.2681295199500715 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.137704918032787, + "y": 7.273668032786885 + }, + "prevControl": { + "x": 5.88773585280021, + "y": 7.277600770605731 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file From 5ed7e59ccb6da75396c04c94e5ccf611533e0d01 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Fri, 24 Jan 2025 20:41:09 -0700 Subject: [PATCH 4/4] Created New Autos --- .../deploy/pathplanner/autos/Mira's Auto.auto | 61 +++++++++++++++++++ .../paths/Center Barge to Reef.path | 20 +++--- src/main/deploy/pathplanner/paths/Taxi.path | 20 +++--- .../java/frc4388/robot/RobotContainer.java | 20 ++++++ 4 files changed, 101 insertions(+), 20 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Mira's Auto.auto diff --git a/src/main/deploy/pathplanner/autos/Mira's Auto.auto b/src/main/deploy/pathplanner/autos/Mira's Auto.auto new file mode 100644 index 0000000..3acc852 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Mira's Auto.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Top Blue Cage to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Reef to Feed" + } + }, + { + "type": "named", + "data": { + "name": "grab-coral" + } + }, + { + "type": "path", + "data": { + "pathName": "Top Feed to Reef" + } + }, + { + "type": "named", + "data": { + "name": "april-allign" + } + }, + { + "type": "named", + "data": { + "name": "place-coral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Barge to Reef.path b/src/main/deploy/pathplanner/paths/Center Barge to Reef.path index 4bd51e2..9e36d9d 100644 --- a/src/main/deploy/pathplanner/paths/Center Barge to Reef.path +++ b/src/main/deploy/pathplanner/paths/Center Barge to Reef.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 15.106960227272728, - "y": 2.120426136363636 + "x": 7.565467171717171, + "y": 4.045328282828282 }, "prevControl": null, "nextControl": { - "x": 15.101585096858368, - "y": 2.396686756913594 + "x": 8.114457611671098, + "y": 4.043616195025507 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 15.106960227272728, - "y": 4.005056818181817 + "x": 5.979861111111112, + "y": 4.045328282828282 }, "prevControl": { - "x": 15.10799254912145, - "y": 3.6525397317875603 + "x": 5.5035379766577375, + "y": 4.0462802990619195 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -179.09061955080082 + "rotation": 179.65844947344297 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 178.99491399474587 + "rotation": 180.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Taxi.path b/src/main/deploy/pathplanner/paths/Taxi.path index f5b4725..8ff7750 100644 --- a/src/main/deploy/pathplanner/paths/Taxi.path +++ b/src/main/deploy/pathplanner/paths/Taxi.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.576229508196722, - "y": 7.273668032786885 + "x": 7.565467171717171, + "y": 7.348674242424242 }, "prevControl": null, "nextControl": { - "x": 8.050548646741815, - "y": 7.2681295199500715 + "x": 7.8906231049309685, + "y": 7.339915637757156 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.137704918032787, - "y": 7.273668032786885 + "x": 6.305113636363636, + "y": 7.348674242424242 }, "prevControl": { - "x": 5.88773585280021, - "y": 7.277600770605731 + "x": 5.928529040722133, + "y": 7.353679406601922 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 179.47479945510887 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 177.6140559696112 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index f705983..7735d99 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -30,6 +30,8 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.Commands; // Autos @@ -100,6 +102,21 @@ public class RobotContainer { new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, true, false); private Command AutoGotoPosition = new GotoPositionCommand(m_robotSwerveDrive, m_vision); + + private Command placeCoral = new SequentialCommandGroup( + new InstantCommand(() -> System.out.println("Placing Some Coral")), + new WaitCommand(3) + ); + + private Command aprilAlign = new SequentialCommandGroup( + new InstantCommand(() -> System.out.println("Aligning...")), + new WaitCommand(1) + ); + + private Command grabCoral = new SequentialCommandGroup( + new InstantCommand(() -> System.out.println("Yoinking some coral...")), + new WaitCommand(2) + ); /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -107,6 +124,9 @@ public class RobotContainer { public RobotContainer() { NamedCommands.registerCommand("AutoGotoPosition", AutoGotoPosition); + NamedCommands.registerCommand("april-allign", aprilAlign); + NamedCommands.registerCommand("place-coral", placeCoral); + NamedCommands.registerCommand("grab-coral", grabCoral); configureButtonBindings(); configureVirtualButtonBindings();