-
-
Notifications
You must be signed in to change notification settings - Fork 1
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Abstract all auto code into Auto.java (#22)
* Abstract all auto code into Auto.java * Renamed new test auto. --------- Co-authored-by: mmilunicmobile <mmilunicmobile@gmail.com>
- Loading branch information
1 parent
a2453db
commit 93c1761
Showing
6 changed files
with
380 additions
and
65 deletions.
There are no files selected for viewing
Large diffs are not rendered by default.
Oops, something went wrong.
19 changes: 19 additions & 0 deletions
19
src/main/deploy/pathplanner/autos/Spinny Difficult Auto Choreo.auto
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,19 @@ | ||
| { | ||
| "version": "2025.0", | ||
| "command": { | ||
| "type": "sequential", | ||
| "data": { | ||
| "commands": [ | ||
| { | ||
| "type": "path", | ||
| "data": { | ||
| "pathName": "Spinny Path" | ||
| } | ||
| } | ||
| ] | ||
| } | ||
| }, | ||
| "resetOdom": true, | ||
| "folder": null, | ||
| "choreoAuto": true | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,123 @@ | ||
| package frc.robot; | ||
|
|
||
| import java.util.ArrayList; | ||
| import java.util.List; | ||
| import java.util.Optional; | ||
|
|
||
| import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; | ||
|
|
||
| import com.pathplanner.lib.auto.AutoBuilder; | ||
| import com.pathplanner.lib.commands.PathPlannerAuto; | ||
| import com.pathplanner.lib.config.PIDConstants; | ||
| import com.pathplanner.lib.config.RobotConfig; | ||
| import com.pathplanner.lib.controllers.PPHolonomicDriveController; | ||
| 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.trajectory.TrajectoryConfig; | ||
| import edu.wpi.first.math.trajectory.TrajectoryGenerator; | ||
| import edu.wpi.first.math.util.Units; | ||
| import edu.wpi.first.wpilibj.DriverStation; | ||
| import edu.wpi.first.wpilibj.DriverStation.Alliance; | ||
| import edu.wpi.first.wpilibj.smartdashboard.Field2d; | ||
| import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; | ||
| import edu.wpi.first.wpilibj2.command.Command; | ||
| import edu.wpi.first.wpilibj2.command.Commands; | ||
| import frc.robot.subsystems.CommandSwerveDrivetrain; | ||
|
|
||
| public class Auto { | ||
| private final LoggedDashboardChooser<Command> autoChooser; | ||
| private Command previousAuto = Commands.none(); | ||
| private RobotConfig config; // PathPlanner robot configuration | ||
|
|
||
| // *NEW | ||
| private final Field2d m_trajectoryField = new Field2d(); | ||
|
|
||
| public Auto(CommandSwerveDrivetrain drivetrain) { | ||
| setUpPathPlanner(drivetrain); | ||
| autoChooser = new LoggedDashboardChooser<>("Auto Routine", AutoBuilder.buildAutoChooser()); | ||
| SmartDashboard.putData("Auto Path", m_trajectoryField); | ||
| } | ||
|
|
||
| public void logAutoInformation() { | ||
| if (previousAuto == autoChooser.get()) { | ||
| return; | ||
| } | ||
|
|
||
| previousAuto = autoChooser.get(); | ||
|
|
||
| Command command = previousAuto; | ||
| { | ||
| try | ||
| { | ||
| var paths = PathPlannerAuto.getPathGroupFromAutoFile(command.getName()); // A list of all paths contained in this auto | ||
| List<Pose2d> poses = new ArrayList<>(); // This will be a list of all points during the auto | ||
|
|
||
| for (PathPlannerPath path : paths) { // For each path assigned, split into segments | ||
| List<PathPoint> points = path.getAllPathPoints(); | ||
| for (PathPoint point : points) { // For each segment, split into points | ||
| Pose2d newPose2d = new Pose2d(point.position, new Rotation2d()); | ||
| poses.add(newPose2d); | ||
| } | ||
| } | ||
|
|
||
| // Generate a trajectory from the "poses" list. This is our entire path | ||
| // "config" is used for unit conversions; Reference Field2d Widget | ||
| var m_trajectory = TrajectoryGenerator.generateTrajectory(poses, new TrajectoryConfig(Units.feetToMeters(3.0), Units.feetToMeters(3.0))); | ||
|
|
||
| // Log the trajectory | ||
| m_trajectoryField.getObject("traj").setTrajectory(m_trajectory); | ||
| // Log the start and end positions | ||
| m_trajectoryField.getObject("start_and_end").setPoses(poses.get(0), poses.get(poses.size() -1)); | ||
| } | ||
| catch (Exception e) | ||
| { | ||
| // Fallback in case the path is set to none, or the path file referenced does not exist | ||
| e.printStackTrace(); | ||
| System.out.println("Pathplanner file not found! Skipping..."); | ||
| m_trajectoryField.getObject("traj").setPoses(); | ||
| m_trajectoryField.getObject("start_and_end").setPoses(); | ||
| } | ||
| } | ||
| } | ||
|
|
||
| public Command getAuto() { | ||
| if (AutoBuilder.isConfigured()) { | ||
| return autoChooser.get(); | ||
| } else { | ||
| return Commands.none(); | ||
| } | ||
| } | ||
|
|
||
| public void setUpPathPlanner(CommandSwerveDrivetrain drivetrain) { | ||
| try { | ||
| config = RobotConfig.fromGUISettings(); | ||
| } catch (Exception e) { | ||
| e.printStackTrace(); | ||
| } | ||
|
|
||
| AutoBuilder.configure( | ||
| drivetrain::getRobotPose, | ||
| drivetrain::resetPose, | ||
| drivetrain::getChassisSpeeds, | ||
| (speeds, feedforwards) -> drivetrain.setControl(drivetrain.driveWithFeedforwards(speeds, feedforwards)), | ||
| new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains | ||
| new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants | ||
| new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants | ||
| ), | ||
| config, | ||
| () -> { | ||
| // Boolean supplier that controls when the path will be mirrored for the red alliance | ||
| Optional<Alliance> alliance = DriverStation.getAlliance(); | ||
| if (alliance.isPresent()) { | ||
| return alliance.get() == DriverStation.Alliance.Red; | ||
| } | ||
| return false; | ||
| }, | ||
| drivetrain | ||
| ); | ||
| } | ||
| } | ||
|
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters