From 7a538d367cec45051e7d81f13bcd2c465555cc47 Mon Sep 17 00:00:00 2001 From: GingerJake Date: Tue, 25 Feb 2025 19:02:16 -0500 Subject: [PATCH 1/3] Don't call our subsystems from odometry thread Co-authored-by: HeroesofOlympus444 Co-authored-by: Andrew Campbell Co-authored-by: Jesse Graham --- src/main/java/frc/robot/LaserCanHandler.java | 5 ++ src/main/java/frc/robot/Robot.java | 11 ++-- .../subsystems/drivetrain/OdometryThread.java | 56 ++++++++++--------- .../subsystems/drivetrain/RAROdometry.java | 29 +++++----- 4 files changed, 56 insertions(+), 45 deletions(-) diff --git a/src/main/java/frc/robot/LaserCanHandler.java b/src/main/java/frc/robot/LaserCanHandler.java index cafdf60..78124fc 100644 --- a/src/main/java/frc/robot/LaserCanHandler.java +++ b/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; } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 2ec801d..55a3bcd 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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); } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/OdometryThread.java b/src/main/java/frc/robot/subsystems/drivetrain/OdometryThread.java index 6bfdc40..4a61c47 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/OdometryThread.java +++ b/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,22 +55,25 @@ 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]; @@ -84,10 +86,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 +136,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(); } @@ -179,27 +181,27 @@ public void setThreadPriority(int priority) { } // // TODO Log these outside of the odometry thread - // // @AutoLogOutput(key = "Odometry/Thread/SuccessfulDataAquisitions") + // @AutoLogOutput(key = "Odometry/Thread/SuccessfulDataAquisitions") // public int getSuccessfulDaqs() { // return m_successfulDaqs; // } - // // @AutoLogOutput(key = "Odometry/Thread/FailedDataAquisitions") + // @AutoLogOutput(key = "Odometry/Thread/FailedDataAquisitions") // public int getFailedDaqs() { // return m_failedDaqs; // } - // // @AutoLogOutput(key = "Odometry/Thread/AverageLoopTime") + // @AutoLogOutput(key = "Odometry/Thread/AverageLoopTime") // public double getAverageOdometryLoopTime() { // return m_averageOdometryLoopTime; // } - // // @AutoLogOutput(key = "Odometry/Thread/UpdatesPerSecond") + // @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; } diff --git a/src/main/java/frc/robot/subsystems/drivetrain/RAROdometry.java b/src/main/java/frc/robot/subsystems/drivetrain/RAROdometry.java index 9835f17..e854836 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/RAROdometry.java +++ b/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() { From 035bd9f201a79877f2ea8bbb7795ce20d1c9073c Mon Sep 17 00:00:00 2001 From: GingerJake Date: Tue, 25 Feb 2025 19:47:35 -0500 Subject: [PATCH 2/3] Use AtomicInteger in Limelight where we are changing the IMUMode outside of the LL thread Log OdometryThread in main 50hz thread Co-authored-by: HeroesofOlympus444 Co-authored-by: Andrew Campbell Co-authored-by: Jesse Graham --- .../java/frc/robot/subsystems/Limelight.java | 18 ++++++++++--- .../subsystems/drivetrain/OdometryThread.java | 27 ++++++++++--------- .../subsystems/drivetrain/RAROdometry.java | 27 +++++++++++++++++++ 3 files changed, 55 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java index 4af982c..465990a 100644 --- a/src/main/java/frc/robot/subsystems/Limelight.java +++ b/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; diff --git a/src/main/java/frc/robot/subsystems/drivetrain/OdometryThread.java b/src/main/java/frc/robot/subsystems/drivetrain/OdometryThread.java index 4a61c47..ced501a 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/OdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/OdometryThread.java @@ -75,6 +75,7 @@ public OdometryThread(ReadWriteLock stateLock, SwerveModule[] modules, SwerveDri m_allSignals = new BaseStatusSignal[4 * 4]; 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]; @@ -180,26 +181,26 @@ public void setThreadPriority(int priority) { m_threadPriorityToSet = priority; } - // // TODO Log these outside of the odometry thread + // TODO Log these outside of the odometry thread // @AutoLogOutput(key = "Odometry/Thread/SuccessfulDataAquisitions") - // public int getSuccessfulDaqs() { - // return m_successfulDaqs; - // } + public int getSuccessfulDaqs() { + return m_successfulDaqs; + } // @AutoLogOutput(key = "Odometry/Thread/FailedDataAquisitions") - // public int getFailedDaqs() { - // return m_failedDaqs; - // } + public int getFailedDaqs() { + return m_failedDaqs; + } // @AutoLogOutput(key = "Odometry/Thread/AverageLoopTime") - // public double getAverageOdometryLoopTime() { - // return m_averageOdometryLoopTime; - // } + public double getAverageOdometryLoopTime() { + return m_averageOdometryLoopTime; + } // @AutoLogOutput(key = "Odometry/Thread/UpdatesPerSecond") - // public int getUpdatesPerSecond() { - // return (int) (1.0 / getAverageOdometryLoopTime()); - // } + public int getUpdatesPerSecond() { + return (int) (1.0 / getAverageOdometryLoopTime()); + } // @AutoLogOutput(key = "Odometry/Thread/Running") public boolean isRunning() { diff --git a/src/main/java/frc/robot/subsystems/drivetrain/RAROdometry.java b/src/main/java/frc/robot/subsystems/drivetrain/RAROdometry.java index e854836..7ad39bd 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/RAROdometry.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/RAROdometry.java @@ -245,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 // } From dff1d50bb511e2867e8f85d78f252698ddb8c467 Mon Sep 17 00:00:00 2001 From: GingerJake Date: Tue, 25 Feb 2025 19:48:54 -0500 Subject: [PATCH 3/3] why was dpadbutton 4 spaced this is gonna make me crash out so badddddddddd --- .../controls/controllers/DPadButton.java | 56 +++++++++---------- 1 file changed, 28 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/controls/controllers/DPadButton.java b/src/main/java/frc/robot/controls/controllers/DPadButton.java index 6f7ceba..2eaf20f 100644 --- a/src/main/java/frc/robot/controls/controllers/DPadButton.java +++ b/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; + } }