Delete Unused

Author: sophiep29

src/main/java/frc/robot/Robot.java

@@ -8,12 +8,10 @@
 import edu.wpi.first.epilogue.Logged;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.networktables.StructArrayPublisher;
 import edu.wpi.first.networktables.StructPublisher;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -27,17 +25,11 @@
 @Logged
 public class Robot extends TimedRobot {
   private Command m_autonomousCommand;
-  // last time we injected a synthetic vision measurement (seconds, FPGA time)
-  private double m_lastSimVisionTime = 0.0;
-  // whether we've injected a one-time offset vision measurement for testing
-  private boolean m_injectedOffset = false;
   private Pose2d robotPose = new Pose2d();
   private final RobotContainer m_robotContainer;
   private final CommandSwerveDrivetrain drivetrain;
   StructPublisher<Pose2d> publisher =
       NetworkTableInstance.getDefault().getStructTopic("Robot Pose", Pose2d.struct).publish();
-  StructArrayPublisher<Pose2d> arrayPublisher =
-      NetworkTableInstance.getDefault().getStructArrayTopic("MyPoseArray", Pose2d.struct).publish();
 
   public Robot() {
     m_robotContainer = new RobotContainer();
@@ -59,17 +51,8 @@ public void robotPeriodic() {
         "Flywheel On",
         !(FlywheelCommand.flywheelState.equals("none")
             || FlywheelCommand.flywheelState.equals("Stopped")));
-    try {
-      var pose = drivetrain.getState().Pose;
-      SmartDashboard.putNumber("OdometryX", pose.getX());
-      SmartDashboard.putNumber("OdometryY", pose.getY());
-      SmartDashboard.putNumber("OdometryRotDeg", pose.getRotation().getDegrees());
-      SmartDashboard.putNumber("SimVisionLastInject", m_lastSimVisionTime);
-    } catch (Exception ignored) {
-    }
     robotPose = drivetrain.getState().Pose;
     publisher.set(robotPose);
-    arrayPublisher.set(new Pose2d[] {robotPose});
   }
 
   @Override
@@ -124,24 +107,5 @@ public void testExit() {}
   public void simulationInit() {}
 
   @Override
-  public void simulationPeriodic() {
-    double now = Timer.getFPGATimestamp();
-    if (now - m_lastSimVisionTime >= 0.1) {
-      m_lastSimVisionTime = now;
-      try {
-        var truePose = drivetrain.getState().Pose;
-        // After 3 seconds, inject a single offset measurement (+1m X) to observe correction
-        if (!m_injectedOffset && now > 3.0) {
-          Pose2d offsetPose =
-              new Pose2d(truePose.getX() + 1.0, truePose.getY(), truePose.getRotation());
-          drivetrain.addVisionMeasurement(offsetPose, now);
-          m_injectedOffset = true;
-        } else {
-          drivetrain.addVisionMeasurement(truePose, now);
-        }
-      } catch (Exception e) {
-        DriverStation.reportError("Sim vision injection failed: " + e.getMessage(), false);
-      }
-    }
-  }
+  public void simulationPeriodic() {}
 }

src/main/java/frc/robot/commands/DrivetrainCommand.java

@@ -72,7 +72,7 @@ public DrivetrainCommand(
     this.rightX = rightX;
     double alignP = 75;
     double alignI = 0;
-    double alignD = 0;
+    double alignD = 10;
     this.autoAlignPidController =
         new ProfiledPIDController(
             alignP,