Adds Pathweaver export test

src/main/java/frc4388/robot/Robot.java

@@ -4,11 +4,14 @@
 
 package frc4388.robot;
 
+import java.io.IOException;
+
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc4388.utility.RobotLogger;
 import frc4388.utility.RobotTime;
 
 /**
@@ -64,6 +67,7 @@ public class Robot extends TimedRobot {
   @Override
   public void disabledInit() {
     m_robotTime.endMatchTime();
+    RobotLogger.getInstance().setEnabled(false);
   }
 
   @Override
@@ -96,6 +100,7 @@ public class Robot extends TimedRobot {
       m_autonomousCommand.schedule();
     }
     m_robotTime.startMatchTime();
+    RobotLogger.getInstance().setEnabled(false);
   }
 
   /**
@@ -115,6 +120,7 @@ public class Robot extends TimedRobot {
       m_autonomousCommand.cancel();
     }
     m_robotTime.startMatchTime();
+    RobotLogger.getInstance().setEnabled(true);
   }
 
   /**
@@ -126,6 +132,16 @@ public class Robot extends TimedRobot {
     // m_robotContainer.getOperatorController().updateInput();
   }
 
+  @Override
+  public void testInit() {
+    RobotLogger.getInstance().setEnabled(false);
+    try {
+      RobotLogger.getInstance().exportPath();
+    } catch (IOException e) {
+      e.printStackTrace();
+    }
+  }
+
   /**
    * This function is called periodically during test mode.
    */

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -21,6 +21,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.RobotMap;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.utility.Gains;
+import frc4388.utility.RobotLogger;
 
 public class SwerveDrive extends SubsystemBase {
   // private WPI_TalonFX m_leftFrontSteerMotor;
@@ -142,6 +143,7 @@ public class SwerveDrive extends SubsystemBase {
     updateOdometry();
     // m_gyro.setFusedHeadingToCompass();
     // m_gyro.setYawToCompass();
+    RobotLogger.getInstance().put("poseMeters", m_poseEstimator.getEstimatedPosition());
     super.periodic();
   }
 

src/main/java/frc4388/utility/RobotLogger.java

@@ -0,0 +1,67 @@
+package frc4388.utility;
+
+import java.io.IOException;
+import java.nio.file.Path;
+import java.util.HashMap;
+import java.util.Hashtable;
+import java.util.List;
+import java.util.Map;
+import java.util.Objects;
+import java.util.Map.Entry;
+import java.util.stream.Collectors;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryUtil;
+import edu.wpi.first.wpilibj.Filesystem;
+
+public final class RobotLogger {
+  private static final int SAMPLE_BASE = 15_000; // ms of sampling
+  private static final int SAMPLE_RATE = 20; // ms between samples
+
+  private static RobotLogger instance = null;
+
+  public static RobotLogger getInstance() {
+    return Objects.requireNonNullElseGet(instance, () -> instance = new RobotLogger());
+  }
+
+  private static long getTime() {
+    return RobotTime.getInstance().m_robotTime;
+  }
+
+  private RobotLogger() {
+    data = new HashMap<>();
+  }
+
+  private final Map<String, Map<Long, Object>> data;
+  private boolean enabled = false;
+
+  public void setEnabled(boolean value) {
+    enabled = value;
+  }
+
+  public void put(String key, Object value) {
+    if (enabled)
+      data.compute(key, (k, v) -> {
+        v = v == null ? new Hashtable<>(SAMPLE_BASE / SAMPLE_RATE, 1) : v;
+        v.put(getTime(), value);
+        return v;
+      });
+  }
+
+  public void exportPath() throws IOException {
+    List<Trajectory.State> states = data.get("poseMeters").entrySet().stream().map(entry -> {
+      double timeSeconds = entry.getKey();
+      double velocityMetersPerSecond = 0;
+      double accelerationMetersPerSecondSq = 0;
+      Pose2d poseMeters = (Pose2d) entry.getValue();
+      double curvatureRadPerMeter = 0;
+      return new Trajectory.State(timeSeconds, velocityMetersPerSecond, accelerationMetersPerSecondSq, poseMeters, curvatureRadPerMeter);
+    }).collect(Collectors.toUnmodifiableList());
+    String pathPath = "paths/" + System.nanoTime() + ".wpilib.json";
+    Path outputPath = Filesystem.getDeployDirectory().toPath().resolve(pathPath);
+    outputPath.toFile().createNewFile();
+    TrajectoryUtil.toPathweaverJson(new Trajectory(states), outputPath);
+  }
+}