Thread fixes

src/main/java/frc/robot/LaserCanHandler.java

@@ -4,6 +4,7 @@
 
 import au.grapplerobotics.ConfigurationFailedException;
 import au.grapplerobotics.LaserCan;
+import edu.wpi.first.wpilibj.RobotBase;
 import frc.robot.constants.RobotConstants;
 
 public class LaserCanHandler {
@@ -49,6 +50,10 @@ private LaserCanHandler() {
 
   @AutoLogOutput(key = "LaserCans/Entrance/seesCoral")
   public boolean getEntranceSeesCoral() { // LaserCAN before elevator
+    if(RobotBase.isSimulation()) {
+      return false;
+    }
+
     return m_entranceLaser.getMeasurement().distance_mm < 100.0;
   }
 

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

@@ -255,17 +255,20 @@ public void disabledInit() {
 
   @Override
   public void disabledPeriodic() {
+    if (m_operatorController.getWantsResetElevator()) {
+      m_elevator.reset();
+    }
+
     Alliance oldAlliance = m_alliance;
+    if(!DriverStation.getAlliance().isPresent()) {
+      return;
+    }
     m_alliance = DriverStation.getAlliance().get();
 
     if (oldAlliance != m_alliance) { // workin' 9 to 5
       m_odometry.setAllianceGyroAngleAdjustment();
     }
 
-    if (m_operatorController.getWantsResetElevator()) {
-      m_elevator.reset();
-    }
-
     // SCARY
     DriverStation.silenceJoystickConnectionWarning(DriverStation.getMatchType() == DriverStation.MatchType.None);
   }

src/main/java/frc/robot/controls/controllers/DPadButton.java

@@ -4,40 +4,40 @@
 
 public class DPadButton {
 
-    FilteredController m_controller;
-    Direction m_direction;
+  FilteredController m_controller;
+  Direction m_direction;
 
-    public boolean m_pressed = false;
+  public boolean m_pressed = false;
 
-    public DPadButton(FilteredController controller, Direction direction) {
-        this.m_controller = controller;
-        this.m_direction = direction;
-    }
-
-    public static enum Direction {
-        UP(0), RIGHT(90), DOWN(180), LEFT(270);
+  public DPadButton(FilteredController controller, Direction direction) {
+    this.m_controller = controller;
+    this.m_direction = direction;
+  }
 
-        int direction;
+  public static enum Direction {
+    UP(0), RIGHT(90), DOWN(180), LEFT(270);
 
-        private Direction(int direction) {
-            this.direction = direction;
-        }
-    }
+    int direction;
 
-    public boolean get() {
-        return m_controller.getPOV() == m_direction.direction;
+    private Direction(int direction) {
+      this.direction = direction;
     }
-
-    public boolean getPressed() {
-        if (get()) {
-            if (!m_pressed) {
-                m_pressed = true;
-                return true;
-            }
-        } else {
-            m_pressed = false;
-        }
-        return false;
+  }
+
+  public boolean get() {
+    return m_controller.getPOV() == m_direction.direction;
+  }
+
+  public boolean getPressed() {
+    if (get()) {
+      if (!m_pressed) {
+        m_pressed = true;
+        return true;
+      }
+    } else {
+      m_pressed = false;
     }
+    return false;
+  }
 
 }

src/main/java/frc/robot/subsystems/Limelight.java

@@ -1,5 +1,6 @@
 package frc.robot.subsystems;
 
+import java.util.concurrent.atomic.AtomicInteger;
 import java.util.concurrent.locks.ReadWriteLock;
 
 import org.littletonrobotics.junction.AutoLogOutput;
@@ -21,23 +22,32 @@
 import frc.robot.subsystems.drivetrain.RAROdometry;
 
 public class Limelight implements Runnable {
+  private final String m_limelightName;
+
   private final NetworkTable m_limelightTable;
   private final LimelightType m_limelightType;
+
   private VisionConstants m_visionConstants;
-  private final String m_limelightName;
+
   private final Thread m_thread;
   private boolean m_isRunning;
-  private int m_internalIMUMode = 0;
+
+  // making this atomic because it should help with concurrency issues
+  private final AtomicInteger m_internalIMUMode;
 
   /**
    * Constructor
    */
   public Limelight(String limelightName, ReadWriteLock lock, LimelightType llType) {
+    m_internalIMUMode = new AtomicInteger(IMUMode.INTERNAL_OFF);
+
     m_limelightName = limelightName;
     m_limelightType = llType;
+
     m_visionConstants = new VisionConstants(1, 100, 0, 100);
 
     m_limelightTable = NetworkTableInstance.getDefault().getTable(m_limelightName);
+
     m_thread = new Thread(this);
     m_thread.setDaemon(true);
   }
@@ -61,7 +71,7 @@ public void setLightEnabled(boolean enabled) {
   }
 
   public void setIMUMode(int mode) {
-    m_internalIMUMode = mode;
+    m_internalIMUMode.set(mode);
   }
 
   /**
@@ -210,7 +220,7 @@ public void run() {
         setIMUMode(IMUMode.INTERNAL_OFF);
       }
 
-      LimelightHelpers.SetIMUMode(m_limelightName, m_internalIMUMode);
+      LimelightHelpers.SetIMUMode(m_limelightName, m_internalIMUMode.get());
 
       // double yaw = LimelightHelpers.getIMUData(m_limelightName).Yaw;
 

src/main/java/frc/robot/subsystems/drivetrain/OdometryThread.java

@@ -12,7 +12,9 @@
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.wpilibj.Threads;
 import edu.wpi.first.wpilibj.Timer;
+import frc.robot.RobotTelemetry;
 import frc.robot.constants.RobotConstants;
+import frc.robot.subsystems.drivetrain.SwerveDrive.Module;
 
 /**
  * Thread enabling 250Hz odometry. Optimized from CTRE's internal swerve code.
@@ -30,9 +32,6 @@ public class OdometryThread implements Runnable {
 
   private final BaseStatusSignal[] m_allSignals;
 
-  private SwerveDrive m_swerve;
-  private RAROdometry m_odometry;
-
   private SwerveDrivePoseEstimator m_poseEstimator;
   private AHRS m_gyro;
 
@@ -45,8 +44,8 @@ public class OdometryThread implements Runnable {
   private SwerveModule[] m_modules;
 
   private volatile int m_threadPriorityToSet = START_THREAD_PRIORITY;
-  private final int k_updateFrequency = RobotConstants.robotConfig.Odometry.k_threadUpdateFrequency;
   private int m_lastThreadPriority = START_THREAD_PRIORITY;
+  private final int k_updateFrequency = RobotConstants.robotConfig.Odometry.k_threadUpdateFrequency;
 
   private double m_lastTime = 0;
   private double m_currentTime = 0;
@@ -56,23 +55,27 @@ public class OdometryThread implements Runnable {
   private int m_successfulDaqs = 0;
   private int m_failedDaqs = 0;
 
-  public OdometryThread(ReadWriteLock stateLock) {
+  public OdometryThread(ReadWriteLock stateLock, SwerveModule[] modules, SwerveDrivePoseEstimator poseEsimator, AHRS gyro) {
     m_stateLock = stateLock;
 
     m_thread = new Thread(this);
 
-    m_swerve = SwerveDrive.getInstance();
-    m_modules = m_swerve.getSwerveModules();
+    // m_swerve = SwerveDrive.getInstance();
+    m_modules = modules;
+    m_poseEstimator = poseEsimator;
+    m_gyro = gyro;
+
     /*
      * Mark this thread as a "daemon" (background) thread
      * so it doesn't hold up program shutdown
      */
     m_thread.setDaemon(true);
 
-    /* 4 signals for each module + 2 for Pigeon2 */
+    /* 4 signals for each module */
     m_allSignals = new BaseStatusSignal[4 * 4];
-    for (int i = 0; i < 4; ++i) {
+    for (int i = 0; i < 4; i++) {
       BaseStatusSignal[] signals = m_modules[i].getSignals();
+
       m_allSignals[(i * 4) + 0] = signals[0];
       m_allSignals[(i * 4) + 1] = signals[1];
       m_allSignals[(i * 4) + 2] = signals[2];
@@ -84,10 +87,6 @@ public OdometryThread(ReadWriteLock stateLock) {
    * Starts the odometry thread.
    */
   public void start() {
-    m_odometry = RAROdometry.getInstance();
-    m_poseEstimator = m_odometry.getPoseEstimator();
-    m_gyro = m_odometry.getGyro();
-
     m_running = true;
     m_thread.start();
   }
@@ -138,15 +137,19 @@ public void run() {
         }
 
         /* Keep track of previous and current pose to account for the carpet vector */
-        m_poseEstimator.updateWithTime(
-            m_currentTime,
-            m_gyro.getRotation2d(),
-            new SwerveModulePosition[] {
-                m_swerve.getModule(SwerveDrive.Module.FRONT_LEFT).getPosition(),
-                m_swerve.getModule(SwerveDrive.Module.FRONT_RIGHT).getPosition(),
-                m_swerve.getModule(SwerveDrive.Module.BACK_RIGHT).getPosition(),
-                m_swerve.getModule(SwerveDrive.Module.BACK_LEFT).getPosition()
-            });
+        if(m_poseEstimator != null) {
+          m_poseEstimator.updateWithTime(
+              m_currentTime,
+              m_gyro.getRotation2d(),
+              new SwerveModulePosition[] {
+                  m_modules[Module.FRONT_LEFT].getPosition(),
+                  m_modules[Module.FRONT_RIGHT].getPosition(),
+                  m_modules[Module.BACK_RIGHT].getPosition(),
+                  m_modules[Module.BACK_LEFT].getPosition()
+              });
+        } else {
+          RobotTelemetry.print("m_poseEstimator is null!");
+        }
       } finally {
         m_stateLock.writeLock().unlock();
       }
@@ -178,28 +181,28 @@ public void setThreadPriority(int priority) {
     m_threadPriorityToSet = priority;
   }
 
-  // // TODO Log these outside of the odometry thread
-  // // @AutoLogOutput(key = "Odometry/Thread/SuccessfulDataAquisitions")
-  // public int getSuccessfulDaqs() {
-  //   return m_successfulDaqs;
-  // }
+  // TODO Log these outside of the odometry thread
+  // @AutoLogOutput(key = "Odometry/Thread/SuccessfulDataAquisitions")
+  public int getSuccessfulDaqs() {
+    return m_successfulDaqs;
+  }
 
-  // // @AutoLogOutput(key = "Odometry/Thread/FailedDataAquisitions")
-  // public int getFailedDaqs() {
-  //   return m_failedDaqs;
-  // }
+  // @AutoLogOutput(key = "Odometry/Thread/FailedDataAquisitions")
+  public int getFailedDaqs() {
+    return m_failedDaqs;
+  }
 
-  // // @AutoLogOutput(key = "Odometry/Thread/AverageLoopTime")
-  // public double getAverageOdometryLoopTime() {
-  //   return m_averageOdometryLoopTime;
-  // }
+  // @AutoLogOutput(key = "Odometry/Thread/AverageLoopTime")
+  public double getAverageOdometryLoopTime() {
+    return m_averageOdometryLoopTime;
+  }
 
-  // // @AutoLogOutput(key = "Odometry/Thread/UpdatesPerSecond")
-  // public int getUpdatesPerSecond() {
-  //   return (int) (1.0 / getAverageOdometryLoopTime());
-  // }
+  // @AutoLogOutput(key = "Odometry/Thread/UpdatesPerSecond")
+  public int getUpdatesPerSecond() {
+    return (int) (1.0 / getAverageOdometryLoopTime());
+  }
 
-  // // @AutoLogOutput(key = "Odometry/Thread/Running")
+  // @AutoLogOutput(key = "Odometry/Thread/Running")
   public boolean isRunning() {
     return m_running;
   }

src/main/java/frc/robot/subsystems/drivetrain/RAROdometry.java

@@ -42,20 +42,6 @@ private RAROdometry() {
     m_limelight = new Limelight("limelight-front", m_stateLock, LimelightType.LL4);
     m_gyro = new AHRS(AHRS.NavXComType.kMXP_SPI, NavXUpdateRate.k200Hz);
 
-    m_odometryThread = new OdometryThread(m_stateLock);
-
-    Thread.UncaughtExceptionHandler odometryThreadHandler = new Thread.UncaughtExceptionHandler() {
-      @Override
-      public void uncaughtException(Thread thread, Throwable throwable) {
-        System.err.println("Exception in odemetry thread: " + throwable.getMessage());
-        System.err.println("Stack trace:");
-        throwable.printStackTrace();
-        // RobotTelemetry.print("Uncaught exception in odometry thread: " + throwable);
-      }
-    };
-
-    Thread.setDefaultUncaughtExceptionHandler(odometryThreadHandler);
-
     m_poseEstimator = new SwerveDrivePoseEstimator(
         m_swerve.getKinematics(),
         m_gyro.getRotation2d(),
@@ -67,6 +53,21 @@ public void uncaughtException(Thread thread, Throwable throwable) {
         },
         new Pose2d(0, 0, Rotation2d.fromDegrees(0)) // we clarified this works
     );
+
+    // starting the thread should be the last thing in this constructor
+    m_odometryThread = new OdometryThread(
+        m_stateLock, m_swerve.getSwerveModules(), m_poseEstimator, m_gyro);
+
+    Thread.UncaughtExceptionHandler odometryThreadHandler = new Thread.UncaughtExceptionHandler() {
+      @Override
+      public void uncaughtException(Thread thread, Throwable throwable) {
+        System.err.println("Exception in odemetry thread: " + throwable.getMessage());
+        System.err.println("Stack trace:");
+        throwable.printStackTrace();
+        // RobotTelemetry.print("Uncaught exception in odometry thread: " + throwable);
+      }
+    };
+    Thread.setDefaultUncaughtExceptionHandler(odometryThreadHandler);
   }
 
   public static RAROdometry getInstance() {
@@ -244,6 +245,33 @@ public Pose2d getPose() {
     return new Pose2d();
   }
 
+
+  /////////////////////////////////THREAD/////////////////////////////////
+  @AutoLogOutput(key = "Odometry/Thread/SuccessfulDataAquisitions")
+  public int getSuccessfulDaqs() {
+    return m_odometryThread.getSuccessfulDaqs();
+  }
+
+  @AutoLogOutput(key = "Odometry/Thread/FailedDataAquisitions")
+  public int getFailedDaqs() {
+    return m_odometryThread.getFailedDaqs();
+  }
+
+  @AutoLogOutput(key = "Odometry/Thread/AverageLoopTime")
+  public double getAverageOdometryLoopTime() {
+    return m_odometryThread.getAverageOdometryLoopTime();
+  }
+
+  @AutoLogOutput(key = "Odometry/Thread/UpdatesPerSecond")
+  public int getUpdatesPerSecond() {
+    return (int) (1.0 / getAverageOdometryLoopTime());
+  }
+
+  @AutoLogOutput(key = "Odometry/Thread/Running")
+  public boolean isRunning() {
+    return m_odometryThread.isRunning();
+  }
+
   // private enum LimelightInstance {
   // LEFT, RIGHT, CENTER
   // }