Merge branch 'main' into auto-structure-rework

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<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() {
@@ -62,6 +118,6 @@ public void setUpPathPlanner(CommandSwerveDrivetrain drivetrain) {
             },
             drivetrain
         );
-    }   
+    }
 }
 

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

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() {