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 00000000..a8dba64f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 9.647717313452645, + "y": 4.335765788648695 + }, + "prevControl": null, + "nextControl": { + "x": 10.647717313452642, + "y": 4.335765788648695 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.0, + "y": 6.0 + }, + "prevControl": { + "x": 3.0, + "y": 6.0 + }, + "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": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 75705c5e..1a5859f0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -150,9 +150,6 @@ public static class AutonomousConstants { public static final PathConstraints PATH_CONSTRAINTS = new PathConstraints(2.0, 1, 1.0, 0.5); // TODO get // constants - // For Pathfinding - // TODO get real poses to pathfind to - public static final Pose2d TARGET_POSE = new Pose2d(new Translation2d(0, 0), new Rotation2d(0d)); } @@ -338,6 +335,11 @@ public class Pipelines { // TODO get real } } + public class PathFindingConstants { + public static final Pose2d TEST_POSE = new Pose2d(9, 4, new Rotation2d(90)); + public static final Translation2d RED_ORIGIN = new Translation2d(16.4592, 0); + } + public class CollisionConstants { public static final double TIP_DEADZONE = 2d; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 07d1e086..a890fecf 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,6 +4,8 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj.DriverStation; @@ -19,6 +21,7 @@ import frc.robot.Constants.ControllerConstants; import frc.robot.Constants.DrivetrainConstants; import frc.robot.Constants.IndexerConstants; +import frc.robot.Constants.PathFindingConstants; import frc.robot.Constants.IndexerConstants.PieceState; import frc.robot.Constants.LEDsConstants.LED_STATES; import frc.robot.Constants.TunerConstants; @@ -29,6 +32,8 @@ import frc.robot.command.CollisionDetection; import frc.robot.command.Index; import frc.robot.command.MoveToPose; +import frc.robot.command.PathFindToAuton; +import frc.robot.command.PathToPose; import frc.robot.command.PointAtPoint; import frc.robot.command.ManualClimb; import frc.robot.command.PointAtTag; @@ -144,7 +149,7 @@ protected void initializeNamedCommands() { new SmartCollect(() -> .5d, () -> .6d, collector, indexer, pivot, flywheel) .deadlineWith(leds.enableState(LED_STATES.COLLECTING).withTimeout(1))); NamedCommands.registerCommand("Index-Up", new Index(() -> IndexerConstants.INDEXER_DEFAULT_POWER, indexer)); - NamedCommands.registerCommand("PathFind", new MoveToPose(AutonomousConstants.TARGET_POSE, drivetrain)); + NamedCommands.registerCommand("PathFind", new PathToPose(PathFindingConstants.TEST_POSE, drivetrain, driver)); NamedCommands.registerCommand("Collect-And-Go", new CollectAndGo(collector, flywheel, indexer)); // make sure named commands are initialized before autobuilder! @@ -172,9 +177,12 @@ protected void configureButtonBindings() { .onTrue(drivetrain.runOnce(drivetrain::seedFieldRelative)); // makes the robot chase pieces + // new Trigger(driver::getRightBumper) + // .whileTrue(new ChasePieces(drivetrain, collector, indexer, pivot, flywheel, limelights) + // .deadlineWith(leds.enableState(LED_STATES.CHASING))); + new Trigger(driver::getRightBumper) - .whileTrue(new ChasePieces(drivetrain, collector, indexer, pivot, flywheel, limelights) - .deadlineWith(leds.enableState(LED_STATES.CHASING))); + .whileTrue(new PathFindToAuton(PathPlannerPath.fromPathFile("C2-C1"), drivetrain, coPilot)); // parks the robot // new Trigger(driver::getXButton).whileTrue(new InstantCommand(() -> diff --git a/src/main/java/frc/robot/command/PathFindToAuton.java b/src/main/java/frc/robot/command/PathFindToAuton.java new file mode 100644 index 00000000..c28037da --- /dev/null +++ b/src/main/java/frc/robot/command/PathFindToAuton.java @@ -0,0 +1,74 @@ +// 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.command; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.PathPoint; + +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.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.AutonomousConstants; +import frc.robot.Constants.PathFindingConstants; +import frc.robot.subsystems.Swerve; +import frc.thunder.filter.XboxControllerFilter; + +public class PathFindToAuton extends Command { + + private Swerve drivetrain; + private XboxControllerFilter controller; // Driver Controller + private AutoBuilder autoBuilder; + private Command pathFindCommand; + private PathPlannerPath autonPath; + + /* + * Pathfinds to a specific pose given + * @param pathfindingPose The pose to pathfind to + * @param drivetrain The drivetrain subsystem + * @param controller The driver controller + */ + public PathFindToAuton(PathPlannerPath autonPath, Swerve drivetrain, XboxControllerFilter controller) { + this.drivetrain = drivetrain; + this.controller = controller; + this.autonPath = autonPath; + autoBuilder = new AutoBuilder(); + + addRequirements(drivetrain); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + pathFindCommand = autoBuilder.pathfindThenFollowPath( + autonPath, AutonomousConstants.PATH_CONSTRAINTS); + pathFindCommand.schedule(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + pathFindCommand.cancel(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (pathFindCommand.isFinished()) { + return true; + } + return false; + } +} diff --git a/src/main/java/frc/robot/command/PathToPose.java b/src/main/java/frc/robot/command/PathToPose.java new file mode 100644 index 00000000..3a71ed71 --- /dev/null +++ b/src/main/java/frc/robot/command/PathToPose.java @@ -0,0 +1,72 @@ +// 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.command; + +import com.pathplanner.lib.auto.AutoBuilder; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.AutonomousConstants; +import frc.robot.subsystems.Swerve; +import frc.thunder.filter.XboxControllerFilter; + +public class PathToPose extends Command { + + private Swerve drivetrain; + private XboxControllerFilter controller; // Driver Controller + private Pose2d pathfindingPose; + private AutoBuilder autoBuilder; + Command pathFindCommand; + + /* + * Pathfinds to a specific pose given + * @param pathfindingPose The pose to pathfind to + * @param drivetrain The drivetrain subsystem + * @param controller The driver controller + */ + public PathToPose(Pose2d pathfindingPose, Swerve drivetrain, XboxControllerFilter controller) { + this.drivetrain = drivetrain; + this.controller = controller; + this.pathfindingPose = pathfindingPose; + + autoBuilder = new AutoBuilder(); + + addRequirements(drivetrain); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + pathFindCommand = autoBuilder.pathfindToPose( + pathfindingPose, AutonomousConstants.PATH_CONSTRAINTS + ); + pathFindCommand.schedule(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (controller.getYButton()) { + end(true); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + if (interrupted) { + pathFindCommand.cancel(); + } + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (pathFindCommand.isFinished()) { + return true; + } + return false; + } +} diff --git a/src/main/java/frc/thunder b/src/main/java/frc/thunder index 38477370..a736ba11 160000 --- a/src/main/java/frc/thunder +++ b/src/main/java/frc/thunder @@ -1 +1 @@ -Subproject commit 3847737007b7d0a736ccde237955dbbdce92c08b +Subproject commit a736ba11bf1cc9f61630b9eab77b28b636799a42