diff --git a/src/main/deploy/pathplanner/autos/Test1.auto b/src/main/deploy/pathplanner/autos/Test1.auto new file mode 100644 index 0000000..6cd4ec6 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Test1.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Test1" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Test2.auto b/src/main/deploy/pathplanner/autos/Test2.auto new file mode 100644 index 0000000..440a1ea --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Test2.auto @@ -0,0 +1,12 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [] + } + }, + "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/New Path.path deleted file mode 100644 index b030c88..0000000 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ /dev/null @@ -1,86 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.884630681818182, - "y": 6.921747159090909 - }, - "prevControl": null, - "nextControl": { - "x": 2.134630681818182, - "y": 6.921747159090909 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.029, - "y": 6.922 - }, - "prevControl": { - "x": 3.779, - "y": 6.922 - }, - "nextControl": { - "x": 4.279, - "y": 6.922 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 4.029, - "y": 5.645383522727273 - }, - "prevControl": { - "x": 3.779, - "y": 5.645383522727273 - }, - "nextControl": { - "x": 4.279, - "y": 5.645383522727273 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 1.884630681818182, - "y": 5.645383522727273 - }, - "prevControl": { - "x": 1.6346306818181817, - "y": 5.645383522727273 - }, - "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 diff --git a/src/main/deploy/pathplanner/paths/Score1.path b/src/main/deploy/pathplanner/paths/Score1.path new file mode 100644 index 0000000..15cc789 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Score1.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.16, + "y": 3.830553977272727 + }, + "prevControl": null, + "nextControl": { + "x": 7.41, + "y": 3.830553977272727 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.88, + "y": 3.830553977272727 + }, + "prevControl": { + "x": 5.63, + "y": 3.830553977272727 + }, + "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": 180.0 + }, + "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/Test1.path b/src/main/deploy/pathplanner/paths/Test1.path new file mode 100644 index 0000000..6245f03 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Test1.path @@ -0,0 +1,190 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.1, + "y": 1.7464914772727262 + }, + "prevControl": null, + "nextControl": { + "x": 6.033916289273189, + "y": 1.649596167268574 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.0, + "y": 2.7 + }, + "prevControl": { + "x": 5.223277321612469, + "y": 2.5875400620062314 + }, + "nextControl": { + "x": 4.776722678387531, + "y": 2.812459937993769 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.666704545454546, + "y": 2.1054687499999987 + }, + "prevControl": { + "x": 5.396009858797951, + "y": 2.5773225696812925 + }, + "nextControl": { + "x": 3.9373992321111384, + "y": 1.633614930318703 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.6453125, + "y": 0.65 + }, + "prevControl": { + "x": 1.7571158988749893, + "y": 0.8736067977499792 + }, + "nextControl": { + "x": 1.5335091011250106, + "y": 0.426393202250021 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.878948863636363, + "y": 2.8034801136363625 + }, + "prevControl": { + "x": 3.710734216820903, + "y": 2.5632447010820343 + }, + "nextControl": { + "x": 4.022342972724125, + "y": 3.0082681247086107 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.3361931818181818, + "y": 0.8091619318181813 + }, + "prevControl": { + "x": 1.2564204545454543, + "y": 0.5598721590909065 + }, + "nextControl": { + "x": 1.4123871135941277, + "y": 1.0472679686180126 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.5997443181818185, + "y": 2.963025568181818 + }, + "prevControl": { + "x": 3.549025212233497, + "y": 2.718224480986922 + }, + "nextControl": { + "x": 3.65046342413014, + "y": 3.207826655376714 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.4458806818181815, + "y": 0.958735795454545 + }, + "prevControl": { + "x": 1.5515352472533563, + "y": 1.1853127422137075 + }, + "nextControl": { + "x": 1.3402261163830067, + "y": 0.7321588486953825 + }, + "isLocked": true, + "linkedName": null + }, + { + "anchor": { + "x": 3.091, + "y": 3.98 + }, + "prevControl": { + "x": 3.0475879555832677, + "y": 3.733798061746948 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.976063829787228, + "rotationDegrees": 115.438429497609 + }, + { + "waypointRelativePos": 2.9, + "rotationDegrees": 46.59192238122859 + }, + { + "waypointRelativePos": 5.961436170212785, + "rotationDegrees": 52.1285759762159 + }, + { + "waypointRelativePos": 6.976063829787137, + "rotationDegrees": 52.25426954444807 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": null + } + ], + "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": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Test2.path b/src/main/deploy/pathplanner/paths/Test2.path new file mode 100644 index 0000000..0bf1d58 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Test2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.109744318181819, + "y": 6.2 + }, + "prevControl": null, + "nextControl": { + "x": 6.09292862387953, + "y": 6.261535626920918 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.414573863636364, + "y": 5.136832386363635 + }, + "prevControl": { + "x": 5.220522783519216, + "y": 4.8353394365031255 + }, + "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.0, + "rotation": -126.19320730556458 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -178.89829388479356 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ba1826e..7eba0ca 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,6 +22,7 @@ import frc.robot.commands.RunAlgaeIntake; import frc.robot.commands.RunCoralOuttake; import frc.robot.commands.SwerveCharacterization; +import frc.robot.commands.TestAuto1; import frc.robot.constants.ROBOT; import frc.robot.constants.SWERVE; import frc.robot.constants.SWERVE.ROUTINE_TYPE; @@ -99,6 +100,7 @@ private void initAutoChooser() { m_chooser.setDefaultOption("Do Nothing", new WaitCommand(0)); m_chooser.addOption("DriveForward", new DriveForward(m_swerveDrive)); + m_chooser.addOption("TestAuto1", new TestAuto1(m_swerveDrive)); } private void initSmartDashboard() { diff --git a/src/main/java/frc/robot/commands/TestAuto1.java b/src/main/java/frc/robot/commands/TestAuto1.java new file mode 100644 index 0000000..afd9981 --- /dev/null +++ b/src/main/java/frc/robot/commands/TestAuto1.java @@ -0,0 +1,46 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; + +import com.ctre.phoenix6.swerve.SwerveRequest; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathPlannerPath; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.subsystems.CommandSwerveDrivetrain; + +public class TestAuto1 extends SequentialCommandGroup { + /** Creates a new DriveForward. */ + public TestAuto1(CommandSwerveDrivetrain swerveDrive /* TODO: Add field sim */) { + try { + PathPlannerPath path = PathPlannerPath.fromPathFile("Test1"); + + var m_ppCommand = AutoBuilder.followPath(path); + + var point = new SwerveRequest.PointWheelsAt(); + var stopRequest = new SwerveRequest.ApplyRobotSpeeds(); + + // Will throw an exception if the starting pose is not present + var starting_pose = path.getStartingHolonomicPose().orElseThrow(); + + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + addCommands( + new InstantCommand( // Reset the pose of the robot to the starting pose of the path + () -> swerveDrive.resetPose(starting_pose)), + new InstantCommand( + () -> swerveDrive.applyRequest(() -> point.withModuleDirection(new Rotation2d())), + swerveDrive) + .alongWith(new WaitCommand(1)), + m_ppCommand.andThen(() -> swerveDrive.setControl(stopRequest))); + } catch (Exception e) { + DriverStation.reportError("Failed to load path for DriveForward", e.getStackTrace()); + addCommands(new WaitCommand(0)); + } + } +}