new code

Author: ArjunSharma-ckl

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

@@ -20,40 +20,32 @@
 
 public class Robot extends LoggedRobot {
   private Command m_autonomousCommand;
+
   private final RobotContainer m_robotContainer;
-  private final Servo phlapServo;
 
-  // ✅ Added: reference to localization subsystem
-  private final localizationSubsystem m_localization = new localizationSubsystem();
+  private final Servo phlapServo;
 
   public Robot() {
 
     phlapServo = new Servo(3);
 
     PurpleManager.initialize(
-        this,
-        Constants.Field.FIELD_LAYOUT,
-        Path.of("/media/sda1"),
-        BuildConstants.MAVEN_NAME,
-        BuildConstants.GIT_SHA,
-        BuildConstants.BUILD_DATE,
-        false,
-        false);
-
+      this,
+      Constants.Field.FIELD_LAYOUT,
+      Path.of("/media/sda1"),
+      BuildConstants.MAVEN_NAME,
+      BuildConstants.GIT_SHA,
+      BuildConstants.BUILD_DATE,
+      false,
+      false
+      );
+      
     m_robotContainer = new RobotContainer();
 
     CameraServer.startAutomaticCapture();
 
     RobotController.setBrownoutVoltage(6.75);
 
-    // ✅ Initialize Limelight calibration ranges once here
-    m_localization.setCalibrationRanges(
-        new double[] {29000, 31000},
-        new double[] {12200, 12400},
-        new double[] {6200, 6400},
-        new double[] {3600, 3800},
-        new double[] {1900, 2100});
-
     // Threads.setCurrentThreadPriority(true, 99);
   }
 
@@ -63,23 +55,13 @@ public void robotPeriodic() {
     LoopTimer.resetTimer();
     CommandScheduler.getInstance().run();
     LoopTimer.addTimestamp("CommandScheduler");
-
-    // ✅ Feed real-time heading into localization subsystem each loop
-    try {
-      double headingDeg =
-          m_robotContainer.DRIVE_SUBSYSTEM.getPose().getRotation().getDegrees();
-      m_localization.setRobotHeadingDeg(headingDeg);
-    } catch (Exception e) {
-      Logger.recordOutput(
-          "Localization/HeadingUpdateError",
-          e.getMessage() == null ? "unknown" : e.getMessage());
-    }
-
     Threads.setCurrentThreadPriority(false, 0);
   }
 
   @Override
-  public void disabledInit() {}
+  public void disabledInit() {
+
+    }
 
   @Override
   public void disabledPeriodic() {
@@ -93,7 +75,7 @@ public void disabledExit() {}
   public void autonomousInit() {
     Logger.recordOutput("Auto/Lift/State", "starting");
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
-
+    
     phlapServo.set(1);
 
     if (m_autonomousCommand != null) {
@@ -121,13 +103,7 @@ public void teleopInit() {
   @Override
   public void teleopPeriodic() {
     m_robotContainer.checkRumble();
-
-    // ✅ Optional: log plan outputs for testing
-    var plan = m_localization.getPlan();
-    Logger.recordOutput("Teleop/AlignRecommendation", plan.recommendation);
-    Logger.recordOutput("Teleop/ForwardError_m", plan.forwardErrorMeters);
-    Logger.recordOutput("Teleop/StrafeError_m", plan.strafeErrorMeters);
-    Logger.recordOutput("Teleop/HeadingError_deg", plan.headingErrorDeg);
+    //localizationSubsystem.localizationSubsystem();
   }
 
   @Override

src/main/java/frc/robot/subsystems/localization/localizationSubsystem.java

@@ -154,6 +154,18 @@ private void calculateDiagonalsAndMidpoint() {
         // diag2, midX, midY);
     }
 
+    public void calculateMovment() {
+        int footOne = 30000;
+        int footThree = 12300;
+        int footFive = 6300;
+        int footSeven = 3700;
+        int footTen = 2000;
+        double realTimeTA = ta;
+        double realTimeDistance = 12.23504 * Math.pow(0.999818, realTimeTA) + 1.19735;
+        Logger.recordOutput("Localization/realTimeDistance", realTimeDistance);
+
+    }
+
     // Accessors
     public double getTx() {
         return tx;