Add auto path preloading

- Refactor recording to use commands

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

@@ -5,22 +5,23 @@
 package frc4388.robot;
 
 import java.io.File;
-import java.io.IOException;
-
+import java.nio.file.Files;
+import java.nio.file.Path;
 import java.util.logging.Level;
 import java.util.logging.Logger;
+import java.util.stream.StreamSupport;
+
+import com.diffplug.common.base.Errors;
 
-import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import frc4388.utility.PathPlannerUtil;
-import frc4388.utility.RobotLogger;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
 import frc4388.utility.RobotTime;
-import frc4388.utility.PathPlannerUtil.Path;
 
 /**
  * The VM is configured to automatically run this class, and to call the
@@ -61,6 +62,33 @@ public class Robot extends TimedRobot {
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
+    // addPeriodic(m_robotContainer::recordPeriodic, kDefaultPeriod);
+    SmartDashboard.putData(CommandScheduler.getInstance());
+    SmartDashboard.putData("JVM Memory", new RunCommand(() -> {}) {
+      @Override public boolean runsWhenDisabled() { return true; }
+      @Override public String getName() {
+        if (isScheduled()) {
+          Runtime runtime = Runtime.getRuntime();
+          long totalMemory = runtime.totalMemory() / 1_000_000;
+          long freeMemory = runtime.freeMemory() / 1_000_000;
+          long maxMemory = runtime.maxMemory() / 1_000_000;
+          return totalMemory - freeMemory + " MB / " + totalMemory + " MB / " + maxMemory + " MB";
+        }
+        return "Not Running";
+      }
+    });
+    SmartDashboard.putData("Usable Deploy Space", new RunCommand(() -> {}) {
+      @Override public boolean runsWhenDisabled() { return true; }
+      @Override public String getName() {
+        if (isScheduled()) {
+          File deploy = Filesystem.getDeployDirectory();
+          long usedSpace = Errors.suppress().getWithDefault(() -> Files.walk(deploy.toPath()).map(Path::toFile).filter(File::isFile).mapToLong(File::length).sum(), 0l) / 1_000_000;
+          long usableSpace = deploy.getUsableSpace() / 1_000_000;
+          return usedSpace + " MB / " + usableSpace + " MB";
+        }
+        return "Not Running";
+      }
+    });
   }
 
   /**
@@ -94,26 +122,13 @@ public class Robot extends TimedRobot {
     LOGGER.fine("disabledInit()");
     m_robotTime.endMatchTime();
     if (isTest()) {
-      try {
-        // IMPORTANT: Had to chown the pathplanner folder in order to save autos.
-        Path path = RobotLogger.getInstance().createPath(0.1, 0.1, false);
-        String pathPath = "recording." + System.currentTimeMillis() + ".path";
-        File outputFile = Filesystem.getDeployDirectory().toPath().resolve("pathplanner").resolve(pathPath).toFile();
-        outputFile.createNewFile();
-        path.write(outputFile);
-        LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
-        LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
-        LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
-        LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
-        LOGGER.log(Level.WARNING, "Recorded path to {0} in the deploy directory on the RoboRIO", pathPath);
-        LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
-        LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
-        LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
-        LOGGER.log(Level.WARNING, "----------------------------------------------------------------------");
-        // LOGGER.finest(path::toString);
-      } catch (IOException e) {
-        e.printStackTrace();
-      }
+      // IMPORTANT: Had to chown the pathplanner folder in order to save autos.
+      File outputFile = Filesystem.getDeployDirectory().toPath().resolve("pathplanner").resolve("recording." + System.currentTimeMillis() + ".path").toFile();
+      if (Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) {
+        m_robotContainer.createPath(null, null, false).write(outputFile);
+        LOGGER.log(Level.SEVERE, "Recorded path to {0}.", outputFile.getPath());
+      } else
+        LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath());
     }
   }
 
@@ -130,12 +145,7 @@ public class Robot extends TimedRobot {
   @Override
   public void autonomousInit() {
     LOGGER.fine("autonomousInit()");
-    try {
-      m_autonomousCommand = m_robotContainer.getAutonomousCommand();
-    } catch (IOException e) {
-      // TODO Auto-generated catch block
-      e.printStackTrace();
-    }
+    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
     // schedule the autonomous command (example)
     if (m_autonomousCommand != null) {
@@ -176,6 +186,7 @@ public class Robot extends TimedRobot {
 
   @Override
   public void testInit() {
+    CommandScheduler.getInstance().cancelAll();
   }
 
   /**

src/main/java/frc4388/robot/RobotContainer.java

@@ -6,14 +6,27 @@ package frc4388.robot;
 
 import java.io.File;
 import java.io.IOException;
+import java.nio.file.FileSystem;
+import java.nio.file.FileSystems;
+import java.nio.file.Files;
+import java.nio.file.Path;
+import java.nio.file.StandardWatchEventKinds;
+import java.nio.file.WatchEvent;
+import java.nio.file.WatchKey;
+import java.nio.file.WatchService;
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.Comparator;
+import java.util.List;
 import java.util.Optional;
+import java.util.function.Function;
+import java.util.logging.Level;
 import java.util.logging.Logger;
+import java.util.regex.Matcher;
+import java.util.regex.Pattern;
 
-import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
-import com.ctre.phoenix.motorcontrol.can.TalonFX;
+import com.diffplug.common.base.Errors;
+import com.diffplug.common.base.Errors.Plugins.Log;
 import com.pathplanner.lib.PathPlanner;
 import com.pathplanner.lib.PathPlannerTrajectory;
 import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState;
@@ -23,12 +36,23 @@ import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 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.networktables.NTSendableBuilder;
+import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj.command.PrintCommand;
+import edu.wpi.first.wpilibj.shuffleboard.EventImportance;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.NotifierCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
@@ -38,6 +62,9 @@ import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.utility.LEDPatterns;
+import frc4388.utility.PathPlannerUtil;
+import frc4388.utility.SendableRunChooser;
+import frc4388.utility.PathPlannerUtil.Path.Waypoint;
 import frc4388.utility.controller.DeadbandedXboxController;
 
 /**
@@ -48,6 +75,7 @@ import frc4388.utility.controller.DeadbandedXboxController;
  * commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
+  private static final Logger LOGGER = Logger.getLogger(RobotContainer.class.getName());
   /* RobotMap */
   private final RobotMap m_robotMap = new RobotMap();
 
@@ -55,17 +83,28 @@ public class RobotContainer {
   private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(
       m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
 
-  private final TalonFX m_testMotor = new TalonFX(23);
-
   private final LED m_robotLED = new LED(m_robotMap.LEDController);
 
   /* Controllers */
   private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
   private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
 
+  private String loadedPathName = "";
+  private PathPlannerTrajectory loadedPathTrajectory = null;
+  private static final Function<CharSequence, String> pathExtensionRemover = ((Function<CharSequence, Matcher>) Pattern
+      .compile(".path")::matcher).andThen(m -> m.replaceFirst(""));
+
+  private final SendableRunChooser<File> autoChooser = new SendableRunChooser<>(selectedAuto -> {
+    if (selectedAuto != null && !selectedAuto.equals(loadedPathName)) {
+      loadedPathTrajectory = PathPlanner.loadPath(pathExtensionRemover.apply(selectedAuto), 5.5, 50);
+      loadedPathName = selectedAuto;
+    }
+  });
+
   /**
    * The container for the robot. Contains subsystems, OI devices, and commands.
    */
+  @SuppressWarnings("unchecked")
   public RobotContainer() {
     configureButtonBindings();
     /* Default Commands */
@@ -77,14 +116,67 @@ public class RobotContainer {
             getDriverController().getRightX(),
             getDriverController().getRightY(),
             true),
-            m_robotSwerveDrive));
+            m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
 
     // continually sends updates to the Blinkin LED controller to keep the lights on
-    m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
+    m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED).withName("LED update defaultCommand"));
+    try (WatchService watcher = FileSystems.getDefault().newWatchService()) {
+      WatchKey watchKey = Filesystem.getDeployDirectory().toPath().resolve("pathplanner").register(
+          FileSystems.getDefault().newWatchService(),
+          StandardWatchEventKinds.ENTRY_CREATE, StandardWatchEventKinds.ENTRY_MODIFY,
+          StandardWatchEventKinds.ENTRY_DELETE);
+      new NotifierCommand(() -> {}, TimedRobot.kDefaultPeriod) {
+        @Override
+        public void execute() {
+          var selectedAuto = autoChooser.getSelected();
+          if (selectedAuto != null && !selectedAuto.getName().equals(loadedPathName)) {
+            setName("Path Watcher: Loading Path");
+            // loadedPathTrajectory = PathPlanner.loadPath(pathExtensionRemover.apply(selectedAuto.getName()), 5.5, 50);
+            loadedPathName = selectedAuto.getName();
+          }
+          if (!watchKey.pollEvents().isEmpty()) {
+            updateAutoChooser();
+            LOGGER.info("Updated autonomous chooser.");
+          }
+          if (!watchKey.reset())
+            LOGGER.severe("File watch key invalid.");
+          setName("Path Watcher: Waiting");
+        }
 
-    m_testMotor.set(TalonFXControlMode.PercentOutput, 0);
+        @Override
+        public boolean runsWhenDisabled() {
+          return true;
+        }
+      }.schedule();
+      pathLoaderCommand.schedule();
+    } catch (IOException exception) {
+      LOGGER.log(Level.SEVERE, "Exception with path file watcher.", exception);
+    }
+    updateAutoChooser();
+    recordInit();
   }
 
+  private Command pathLoaderCommand = new CommandBase() {
+    @Override
+    public void execute() {
+      var selectedAuto = autoChooser.getSelected();
+      if (selectedAuto != null && !selectedAuto.getName().equals(loadedPathName)) {
+        setName("LOADING PATH");
+        Thread thread = new Thread(() -> {
+          loadedPathTrajectory = PathPlanner.loadPath(pathExtensionRemover.apply(selectedAuto.getName()), 5.5, 50);
+          setName("Path loader waiting");
+        }, "Path Loader Thread");
+        thread.start();
+      }
+    }
+
+    @Override
+    public boolean runsWhenDisabled() {
+      return true;
+    }
+  }.withName("Path loader");
+  private Thread pathLoaderThread = new Thread();
+
   /**
    * Use this method to define your button->command mappings. Buttons can be
    * created by instantiating a {@link GenericHID} or one of its subclasses
@@ -108,7 +200,7 @@ public class RobotContainer {
         .whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
 
     new JoystickButton(getDriverController(), XboxController.Button.kA.value)
-      .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
+        .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
 
     /* Operator Buttons */
     // activates "Lit Mode"
@@ -118,59 +210,39 @@ public class RobotContainer {
         .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
   }
 
-  public Command[] buildAuto(double maxVel, double maxAccel, Object... inputs) {
-    ArrayList<Command> commands = new ArrayList<>();
-    commands.add(new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()));
-
-    PIDController xController = SwerveDriveConstants.X_CONTROLLER;
-    PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
-    ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
-    thetaController.enableContinuousInput(-Math.PI, Math.PI);
-
-    // parse input
-    for (int i = 0; i < inputs.length; i++) {
-      if (inputs[i] instanceof String) {
-        PathPlannerTrajectory traj = PathPlanner.loadPath(inputs[i].toString(), maxVel, maxAccel);
-        PathPlannerState initState = (PathPlannerState) traj.sample(0);
-
-        commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation))));
-        commands.add(new PPSwerveControllerCommand(traj, m_robotSwerveDrive::getOdometry, m_robotSwerveDrive.m_kinematics, xController, yController, thetaController, m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive));
-      } else if (inputs[i] instanceof Command)
-        commands.add((Command) inputs[i]);
-    }
-
-    commands.add(new InstantCommand(m_robotSwerveDrive::stopModules));
-    Command[] ret = new Command[commands.size()];
-    ret = commands.toArray(ret);
-    return ret;
-  }
-
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *
    * @return the command to run in autonomous
-   * @throws IOException
    */
-  public Command getAutonomousCommand() throws IOException {
-    // https://github.com/mjansen4857/pathplanner/wiki <-- Pathplanner Wiki
+  public Command getAutonomousCommand() {
+    var selectedAuto = autoChooser.getSelected();
+    if (loadedPathTrajectory != null) {
+      // if (selectedAuto != null && !selectedAuto.getName().equals(loadedPathName))
+      // loadedPathTrajectory =
+      // PathPlanner.loadPath(pathExtensionRemover.apply(selectedAuto.getName()), 5.5,
+      // 50);
+      PIDController xController = SwerveDriveConstants.X_CONTROLLER;
+      PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
+      ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
+      thetaController.enableContinuousInput(-Math.PI, Math.PI);
 
-    // Command[] trajCommands = buildAuto(0.5, 0.5, "Move Forward", "Move Down");
-    // SequentialCommandGroup ret = new SequentialCommandGroup(trajCommands);
-    // return (new ParallelCommandGroup(buildAuto(0.1, 0.1, ret, new InstantCommand(() -> m_testMotor.set(TalonFXControlMode.PercentOutput, 0.2)))));
-    Optional<File> recordedAuto = Arrays.stream(Filesystem.getDeployDirectory().toPath().resolve("pathplanner").toFile().listFiles()).filter(f -> f.getName().startsWith("recording")).max(Comparator.comparingLong(File::lastModified));
-    if (recordedAuto.isPresent()) {
-      String name = recordedAuto.orElseThrow().getName().replace(".path", "");
-      Logger.getLogger(this.getClass().getName()).fine(name);
-      return new SequentialCommandGroup(buildAuto(1.0, 1.0, name));
+      PathPlannerState initialState = loadedPathTrajectory.getInitialState();
+      Pose2d initialPosition = new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation);
+      return new SequentialCommandGroup(
+          new InstantCommand(m_robotSwerveDrive.m_gyro::reset),
+          new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initialPosition)),
+          new PPSwerveControllerCommand(loadedPathTrajectory, m_robotSwerveDrive::getOdometry,
+              m_robotSwerveDrive.m_kinematics, xController, yController, thetaController,
+              m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive),
+          new InstantCommand(m_robotSwerveDrive::stopModules)).withName("Run Autonomous Path");
+    } else {
+      Logger.getLogger(this.getClass().getName()).severe("No auto selected.");
+      return new RunCommand(() -> {
+      }).withName("No Autonomous Path");
     }
-    Logger.getLogger(this.getClass().getName()).severe("No recorded autos found.");
-    return new InstantCommand();
-    // return new PrintCommand("No recorded autos found.");
   }
 
-  /**
-   * Add your docs here.
-   */
   public XboxController getDriverController() {
     return m_driverXbox;
   }
@@ -183,10 +255,67 @@ public class RobotContainer {
     m_robotSwerveDrive.resetOdometry(pose);
   }
 
-  /**
-   * Add your docs here.
-   */
   public XboxController getOperatorController() {
     return m_operatorXbox;
   }
+
+  private void updateAutoChooser() {
+    Arrays.stream(Filesystem.getDeployDirectory().toPath().resolve("pathplanner").toFile().listFiles())
+        .filter(file -> file.getName().endsWith(".path")).sorted(Comparator.comparingLong(File::lastModified))
+        .forEachOrdered(file -> autoChooser.addOption(file.getName(), file));
+    SmartDashboard.putData("Auto Chooser", autoChooser);
+  }
+
+  private final List<Waypoint> pathPoints = new ArrayList<>();
+
+  public void recordInit() {
+    SmartDashboard.putData("Recording",
+        new RunCommand(this::recordPeriodic) {
+          @Override
+          public void end(boolean interupted) {
+            new InstantCommand(RobotContainer.this::saveRecording) {
+              @Override
+              public boolean runsWhenDisabled() {
+                return true;
+              }
+            }.withName("Save Recording").schedule();
+          }
+        }.withName("Record Path (Cancel to Save)"));
+  }
+
+  private void saveRecording() {
+    // IMPORTANT: Had to chown the pathplanner folder in order to save autos.
+    File outputFile = Filesystem.getDeployDirectory().toPath().resolve("pathplanner")
+        .resolve("recording." + System.currentTimeMillis() + ".path").toFile();
+    if (Boolean.TRUE.equals(Errors.log().getWithDefault(outputFile::createNewFile, false))) {
+      createPath(null, null, false).write(outputFile);
+      autoChooser.setDefaultOption(outputFile.getName(), outputFile);
+      LOGGER.log(Level.SEVERE, "Recorded path to {0}.", outputFile.getPath());
+    } else
+      LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath());
+  }
+
+  public void recordPeriodic() {
+    Translation2d position = m_robotSwerveDrive.m_poseEstimator.getEstimatedPosition().getTranslation();
+    Rotation2d rotation = m_robotSwerveDrive.m_gyro.getRotation2d();
+    Translation2d velocity = new Translation2d(m_robotSwerveDrive.chassisSpeeds.vxMetersPerSecond,
+        m_robotSwerveDrive.chassisSpeeds.vyMetersPerSecond);
+    Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false, velocity.getNorm(),
+        false);
+    pathPoints.add(waypoint);
+  }
+
+  public PathPlannerUtil.Path createPath(Double maxVelocity, Double maxAcceleration, Boolean isReversed) {
+    PathPlannerUtil.Path path = new PathPlannerUtil.Path();
+    for (int i = 0; i < pathPoints.size() - 2; i++)
+      pathPoints.get(i).nextControl = pathPoints.get(i + 1).anchorPoint;
+    for (int i = 1; i < pathPoints.size() - 1; i++)
+      pathPoints.get(i).prevControl = pathPoints.get(i - 1).anchorPoint;
+    path.waypoints = Optional.ofNullable(pathPoints.toArray(PathPlannerUtil.Path.Waypoint[]::new));
+    path.maxVelocity = Optional.ofNullable(maxVelocity);
+    path.maxAcceleration = Optional.ofNullable(maxAcceleration);
+    path.isReversed = Optional.ofNullable(isReversed);
+    pathPoints.clear();
+    return path;
+  }
 }

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

@@ -4,8 +4,8 @@
 
 package frc4388.robot.subsystems;
 
-import com.ctre.phoenix.sensors.WPI_PigeonIMU;
 import com.ctre.phoenix.sensors.PigeonIMU.FusionStatus;
+import com.ctre.phoenix.sensors.WPI_PigeonIMU;
 
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
@@ -23,8 +23,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.OIConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.utility.Gains;
-import frc4388.utility.RobotLogger;
-import frc4388.utility.PathPlannerUtil.Path.Waypoint.Point;
 
 public class SwerveDrive extends SubsystemBase {
 
@@ -57,7 +55,7 @@ public class SwerveDrive extends SubsystemBase {
   public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
   public boolean ignoreAngles;
   public Rotation2d rotTarget = new Rotation2d();;
-  private ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
+  public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
 
   private final Field2d m_field = new Field2d();
 
@@ -160,23 +158,7 @@ public class SwerveDrive extends SubsystemBase {
 
   @Override
   public void periodic() {
-    // System.err.println(m_gyro.getFusedHeading() +"    aaa");
     updateOdometry();
-    // m_gyro.setFusedHeadingToCompass();
-    // m_gyro.setYawToCompass();
-    Pose2d pos = m_poseEstimator.getEstimatedPosition();
-    RobotLogger.getInstance().put(
-      /* anchorPointX */ pos.getX(),
-      /* anchorPointY */ pos.getY(),
-      /* prevControlX */ pos.getX(),
-      /* prevControlY */ pos.getY(),
-      /* nextControlX */ pos.getX(),
-      /* nextControlY */ pos.getY(),
-      /* holonomicAngle */ m_gyro.getRotation2d().getDegrees(),
-      /* isReversal */ false,
-      /* velOverride */ null,
-      /* isLocked */ false
-    );
     SmartDashboard.putNumber("Pigeon Fused Heading", m_gyro.getFusedHeading(fstatus));
     SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
     SmartDashboard.putNumber("Pigeon Get Angle", m_gyro.getAngle());
@@ -186,8 +168,7 @@ public class SwerveDrive extends SubsystemBase {
     // m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
     // m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_6_SensorFusion, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
     // m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_1_General, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
-
-    // m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
+    m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
     super.periodic();
   }