diff --git a/src/main/java/frc/robot/Auto.java b/src/main/java/frc/robot/Auto.java index 0d399364..95f2dacc 100644 --- a/src/main/java/frc/robot/Auto.java +++ b/src/main/java/frc/robot/Auto.java @@ -1,30 +1,86 @@ package frc.robot; -import java.io.IOException; +import java.util.ArrayList; +import java.util.List; import java.util.Optional; -import org.json.simple.parser.ParseException; -import org.littletonrobotics.junction.Logger; 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 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 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 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() { @@ -62,6 +118,6 @@ public void setUpPathPlanner(CommandSwerveDrivetrain drivetrain) { }, drivetrain ); - } + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 386c043a..4a9d73b0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -5,16 +5,13 @@ package frc.robot; -import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.NT4Publisher; -import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; -import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -33,7 +30,8 @@ public Robot() { Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables PowerDistribution distribution = new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging } else { - setUseTiming(false); // Run as fast as possible + setUseTiming(true); // Run as fast as possible + Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables } Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may be added. @@ -42,8 +40,8 @@ public Robot() { @Override public void robotPeriodic() { - CommandScheduler.getInstance().run(); - + CommandScheduler.getInstance().run(); + m_robotContainer.auto.logAutoInformation(); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 70870486..5c1f83f0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -34,14 +34,17 @@ public class RobotContainer { public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); - private Auto auto = new Auto(drivetrain); + public Auto auto = new Auto(drivetrain); // Use open-loop control for drive motors private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); public RobotContainer() { - configureBindings(); + configureBindings(); + + drivetrain.setUpPathPlanner(); + // Establish the "Trajectory Field" Field2d into the dashboard } private void configureBindings() {