Merge pull request #1 from Shenzhen-Robotics-Alliance/Virtual-branch

Virtual branch

networktables.json

@@ -0,0 +1 @@
+[]

src/main/java/frc/robot/Constants.java

@@ -31,6 +31,17 @@ public enum RobotMode {
     }
 
     public static final class ChassisConfigs {
+        public enum ChassisType {
+            REV,
+            CTRE_ON_RIO,
+            CTRE_ON_CANIVORE
+        }
+        public static final ChassisType chassisType = ChassisType.CTRE_ON_CANIVORE;
+
         public static final String DEFAULT_CHASSIS_CANIVORE = "ChassisCanivore";
+
+        public static final int ODOMETRY_CACHE_CAPACITY = 20;
+        public static final double ODOMETRY_FREQUENCY = 250;
+        public static final double ODOMETRY_WAIT_TIMEOUT_SECONDS = 0.02;
     }
 }

src/main/java/frc/robot/RobotContainer.java

@@ -47,7 +47,7 @@ public RobotContainer() {
 //                        new ModuleIOSparkMax(3)
 //                );
                  drive = new Drive(
-                    new GyroIOPigeon2(true),
+                    new GyroIOPigeon2(),
                     new ModuleIOTalonFX(0),
                     new ModuleIOTalonFX(1),
                     new ModuleIOTalonFX(2),

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -22,14 +22,15 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+import frc.robot.Robot;
 import frc.robot.utils.LocalADStarAK;
 
-import java.util.concurrent.locks.Lock;
-import java.util.concurrent.locks.ReentrantLock;
-
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
+import java.util.Arrays;
+
 public class Drive extends SubsystemBase {
     private static final double MAX_LINEAR_SPEED = Units.feetToMeters(14.5);
     private static final double TRACK_WIDTH_X = Units.inchesToMeters(25.0);
@@ -38,7 +39,6 @@ public class Drive extends SubsystemBase {
             Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0);
     private static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS;
 
-    static final Lock odometryLock = new ReentrantLock();
     private final GyroIO gyroIO;
     private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged();
     private final Module[] modules = new Module[4]; // FL, FR, BL, BR
@@ -55,6 +55,8 @@ public class Drive extends SubsystemBase {
     private SwerveDrivePoseEstimator poseEstimator =
             new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, new Pose2d());
 
+    private final OdometryThread odometryThread;
+    private final OdometryThread.OdometryThreadInputs odometryThreadInputs;
     public Drive(
             GyroIO gyroIO,
             ModuleIO flModuleIO,
@@ -67,9 +69,9 @@ public Drive(
         modules[2] = new Module(blModuleIO, 2);
         modules[3] = new Module(brModuleIO, 3);
 
-        // Start threads (no-op for each if no signals have been created)
-        PhoenixOdometryThread.getInstance().start();
-        SparkMaxOdometryThread.getInstance().start();
+        this.odometryThread = OdometryThread.createInstance();
+        this.odometryThreadInputs = new OdometryThread.OdometryThreadInputs();
+        this.odometryThread.start();
 
         // Configure AutoBuilder for PathPlanner
         AutoBuilder.configureHolonomic(
@@ -92,16 +94,12 @@ MAX_LINEAR_SPEED, DRIVE_BASE_RADIUS, new ReplanningConfig()),
     }
 
     public void periodic() {
-        odometryLock.lock(); // Prevents odometry updates while reading data
+        odometryThread.updateInputs(odometryThreadInputs);
+        Logger.processInputs("Drive/OdometryThread", odometryThreadInputs);
         gyroIO.updateInputs(gyroInputs);
-        for (var module : modules) {
-            module.updateInputs();
-        }
-        odometryLock.unlock();
         Logger.processInputs("Drive/Gyro", gyroInputs);
-        for (var module : modules) {
+        for (var module : modules)
             module.periodic();
-        }
 
         // Stop moving when disabled
         if (DriverStation.isDisabled()) {
@@ -111,14 +109,11 @@ public void periodic() {
         }
         // Log empty setpoint states when disabled
         if (DriverStation.isDisabled()) {
-            Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[]{});
-            Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[]{});
+            // Logger.recordOutput("SwerveStates/Setpoints");
+            // Logger.recordOutput("SwerveStates/SetpointsOptimized");
         }
 
-        // Update odometry
-        double[] sampleTimestamps = modules[0].getOdometryTimestamps(); // All signals are sampled together
-        int sampleCount = sampleTimestamps.length;
-        for (int i = 0; i < sampleCount; i++) {
+        for (int i = 0; i < odometryThreadInputs.measurementTimeStamps.length; i++) {
             // Read wheel positions and deltas from each module
             SwerveModulePosition[] modulePositions = new SwerveModulePosition[4];
             SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4];
@@ -143,7 +138,7 @@ public void periodic() {
             }
 
             // Apply update
-            poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions);
+            poseEstimator.updateWithTime(odometryThreadInputs.measurementTimeStamps[i], rawGyroRotation, modulePositions);
         }
     }
 

src/main/java/frc/robot/subsystems/drive/GyroIO.java

@@ -11,7 +11,6 @@ public interface GyroIO {
     class GyroIOInputs {
         public boolean connected = false;
         public Rotation2d yawPosition = new Rotation2d();
-        public double[] odometryYawTimestamps = new double[]{};
         public Rotation2d[] odometryYawPositions = new Rotation2d[]{};
         public double yawVelocityRadPerSec = 0.0;
     }

src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java

@@ -4,44 +4,30 @@
 package frc.robot.subsystems.drive;
 
 import com.ctre.phoenix6.BaseStatusSignal;
-import com.ctre.phoenix6.StatusCode;
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.Pigeon2Configuration;
 import com.ctre.phoenix6.hardware.Pigeon2;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 import frc.robot.Constants;
 
-import java.util.OptionalDouble;
-import java.util.Queue;
+import java.util.Arrays;
 
 /**
  * IO implementation for Pigeon2
  */
 public class GyroIOPigeon2 implements GyroIO {
     private final Pigeon2 pigeon = new Pigeon2(0, Constants.ChassisConfigs.DEFAULT_CHASSIS_CANIVORE);
     private final StatusSignal<Double> yaw = pigeon.getYaw();
-    private final Queue<Double> yawPositionQueue;
-    private final Queue<Double> yawTimestampQueue;
+    private final OdometryThreadReal.OdometryDoubleInput yawPositionInput;
     private final StatusSignal<Double> yawVelocity = pigeon.getAngularVelocityZWorld();
 
-    public GyroIOPigeon2(boolean phoenixDrive) {
+    public GyroIOPigeon2() {
         pigeon.getConfigurator().apply(new Pigeon2Configuration());
         pigeon.getConfigurator().setYaw(0.0);
-        yaw.setUpdateFrequency(Module.ODOMETRY_FREQUENCY);
         yawVelocity.setUpdateFrequency(100.0);
         pigeon.optimizeBusUtilization();
-        if (phoenixDrive) {
-            yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
-            yawPositionQueue =
-                    PhoenixOdometryThread.getInstance().registerSignal(pigeon, pigeon.getYaw());
-        } else {
-            yawTimestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue();
-            yawPositionQueue = SparkMaxOdometryThread.getInstance().registerSignal(() -> {
-                boolean valid = yaw.refresh().getStatus().isOK();
-                return valid ? OptionalDouble.of(yaw.getValueAsDouble()) : OptionalDouble.empty();
-            });
-        }
+        yawPositionInput = OdometryThread.registerSignalInput(pigeon.getYaw());
     }
 
     @Override
@@ -50,11 +36,6 @@ public void updateInputs(GyroIOInputs inputs) {
         inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble());
         inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble());
 
-        inputs.odometryYawTimestamps =
-                yawTimestampQueue.stream().mapToDouble((Double value) -> value).toArray();
-        inputs.odometryYawPositions =
-                yawPositionQueue.stream().map(Rotation2d::fromDegrees).toArray(Rotation2d[]::new);
-        yawTimestampQueue.clear();
-        yawPositionQueue.clear();
+        inputs.odometryYawPositions = Arrays.stream(yawPositionInput.getValuesSincePreviousPeriod()).map(Rotation2d::fromDegrees).toArray(Rotation2d[]::new);
     }
 }

src/main/java/frc/robot/subsystems/drive/Module.java

@@ -58,15 +58,8 @@ public Module(ModuleIO io, int index) {
         setBrakeMode(true);
     }
 
-    /**
-     * Update inputs without running the rest of the periodic logic. This is useful since these
-     * updates need to be properly thread-locked.
-     */
-    public void updateInputs() {
-        io.updateInputs(inputs);
-    }
-
     public void periodic() {
+        io.updateInputs(inputs);
         Logger.processInputs("Drive/Module" + index, inputs);
 
         // On first cycle, reset relative turn encoder
@@ -99,9 +92,8 @@ public void periodic() {
         }
 
         // Calculate positions for odometry
-        int sampleCount = inputs.odometryTimestamps.length; // All signals are sampled together
-        odometryPositions = new SwerveModulePosition[sampleCount];
-        for (int i = 0; i < sampleCount; i++) {
+        odometryPositions = new SwerveModulePosition[inputs.odometryDrivePositionsRad.length];
+        for (int i = 0; i < odometryPositions.length; i++) {
             double positionMeters = inputs.odometryDrivePositionsRad[i] * WHEEL_RADIUS;
             Rotation2d angle =
                     inputs.odometryTurnPositions[i].plus(
@@ -190,11 +182,4 @@ public SwerveModuleState getState() {
     public SwerveModulePosition[] getOdometryPositions() {
         return odometryPositions;
     }
-
-    /**
-     * Returns the timestamps of the samples received this cycle.
-     */
-    public double[] getOdometryTimestamps() {
-        return inputs.odometryTimestamps;
-    }
 }

src/main/java/frc/robot/subsystems/drive/ModuleIO.java

@@ -12,15 +12,14 @@ class ModuleIOInputs {
         public double drivePositionRad = 0.0;
         public double driveVelocityRadPerSec = 0.0;
         public double driveAppliedVolts = 0.0;
-        public double[] driveCurrentAmps = new double[]{};
+        public double driveCurrentAmps = 0;
 
         public Rotation2d turnAbsolutePosition = new Rotation2d();
         public Rotation2d turnPosition = new Rotation2d();
         public double turnVelocityRadPerSec = 0.0;
         public double turnAppliedVolts = 0.0;
-        public double[] turnCurrentAmps = new double[]{};
+        public double turnCurrentAmps = 0.0;
 
-        public double[] odometryTimestamps = new double[]{};
         public double[] odometryDrivePositionsRad = new double[]{};
         public Rotation2d[] odometryTurnPositions = new Rotation2d[]{};
     }

src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java

@@ -44,16 +44,15 @@ public void updateInputs(ModuleIOInputs inputs) {
         inputs.drivePositionRad = driveSim.getAngularPositionRad();
         inputs.driveVelocityRadPerSec = driveSim.getAngularVelocityRadPerSec();
         inputs.driveAppliedVolts = driveAppliedVolts;
-        inputs.driveCurrentAmps = new double[]{Math.abs(driveSim.getCurrentDrawAmps())};
+        inputs.driveCurrentAmps = Math.abs(driveSim.getCurrentDrawAmps());
 
         inputs.turnAbsolutePosition =
                 new Rotation2d(turnSim.getAngularPositionRad()).plus(turnAbsoluteInitPosition);
         inputs.turnPosition = new Rotation2d(turnSim.getAngularPositionRad());
         inputs.turnVelocityRadPerSec = turnSim.getAngularVelocityRadPerSec();
         inputs.turnAppliedVolts = turnAppliedVolts;
-        inputs.turnCurrentAmps = new double[]{Math.abs(turnSim.getCurrentDrawAmps())};
+        inputs.turnCurrentAmps = Math.abs(turnSim.getCurrentDrawAmps());
 
-        inputs.odometryTimestamps = new double[]{Timer.getFPGATimestamp()};
         inputs.odometryDrivePositionsRad = new double[]{inputs.drivePositionRad};
         inputs.odometryTurnPositions = new Rotation2d[]{inputs.turnPosition};
     }

src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java

@@ -17,15 +17,13 @@
 import com.revrobotics.CANSparkLowLevel.MotorType;
 import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
 import com.revrobotics.CANSparkMax;
-import com.revrobotics.REVLibError;
 import com.revrobotics.RelativeEncoder;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.AnalogInput;
 import edu.wpi.first.wpilibj.RobotController;
 
-import java.util.OptionalDouble;
-import java.util.Queue;
+import java.util.Arrays;
 
 /**
  * Module IO implementation for SparkMax drive motor controller, SparkMax turn motor controller (NEO
@@ -50,9 +48,8 @@ public class ModuleIOSparkMax implements ModuleIO {
     private final RelativeEncoder driveEncoder;
     private final RelativeEncoder turnRelativeEncoder;
     private final AnalogInput turnAbsoluteEncoder;
-    private final Queue<Double> timestampQueue;
-    private final Queue<Double> drivePositionQueue;
-    private final Queue<Double> turnPositionQueue;
+    private final OdometryThreadReal.OdometryDoubleInput drivePositionInput;
+    private final OdometryThreadReal.OdometryDoubleInput turnPositionInput;
 
     private final boolean isTurnMotorInverted = true;
     private final Rotation2d absoluteEncoderOffset;
@@ -117,29 +114,8 @@ public ModuleIOSparkMax(int index) {
                 PeriodicFrame.kStatus2, (int) (1000.0 / Module.ODOMETRY_FREQUENCY));
         turnSparkMax.setPeriodicFramePeriod(
                 PeriodicFrame.kStatus2, (int) (1000.0 / Module.ODOMETRY_FREQUENCY));
-        timestampQueue = SparkMaxOdometryThread.getInstance().makeTimestampQueue();
-        drivePositionQueue =
-                SparkMaxOdometryThread.getInstance()
-                        .registerSignal(
-                                () -> {
-                                    double value = driveEncoder.getPosition();
-                                    if (driveSparkMax.getLastError() == REVLibError.kOk) {
-                                        return OptionalDouble.of(value);
-                                    } else {
-                                        return OptionalDouble.empty();
-                                    }
-                                });
-        turnPositionQueue =
-                SparkMaxOdometryThread.getInstance()
-                        .registerSignal(
-                                () -> {
-                                    double value = turnRelativeEncoder.getPosition();
-                                    if (turnSparkMax.getLastError() == REVLibError.kOk) {
-                                        return OptionalDouble.of(value);
-                                    } else {
-                                        return OptionalDouble.empty();
-                                    }
-                                });
+        this.drivePositionInput = OdometryThread.registerInput(driveEncoder::getPosition);
+        this.turnPositionInput = OdometryThread.registerInput(turnRelativeEncoder::getPosition);
 
         driveSparkMax.burnFlash();
         turnSparkMax.burnFlash();
@@ -152,7 +128,7 @@ public void updateInputs(ModuleIOInputs inputs) {
         inputs.driveVelocityRadPerSec =
                 Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) / DRIVE_GEAR_RATIO;
         inputs.driveAppliedVolts = driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage();
-        inputs.driveCurrentAmps = new double[]{driveSparkMax.getOutputCurrent()};
+        inputs.driveCurrentAmps = driveSparkMax.getOutputCurrent();
 
         inputs.turnAbsolutePosition =
                 new Rotation2d(
@@ -164,21 +140,14 @@ public void updateInputs(ModuleIOInputs inputs) {
                 Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity())
                         / TURN_GEAR_RATIO;
         inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage();
-        inputs.turnCurrentAmps = new double[]{turnSparkMax.getOutputCurrent()};
-
-        inputs.odometryTimestamps =
-                timestampQueue.stream().mapToDouble((Double value) -> value).toArray();
-        inputs.odometryDrivePositionsRad =
-                drivePositionQueue.stream()
-                        .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO)
-                        .toArray();
-        inputs.odometryTurnPositions =
-                turnPositionQueue.stream()
-                        .map((Double value) -> Rotation2d.fromRotations(value / TURN_GEAR_RATIO))
-                        .toArray(Rotation2d[]::new);
-        timestampQueue.clear();
-        drivePositionQueue.clear();
-        turnPositionQueue.clear();
+        inputs.turnCurrentAmps = turnSparkMax.getOutputCurrent();
+
+        inputs.odometryDrivePositionsRad = Arrays.stream(drivePositionInput.getValuesSincePreviousPeriod())
+                .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO)
+                .toArray();
+        inputs.odometryTurnPositions = Arrays.stream(turnPositionInput.getValuesSincePreviousPeriod())
+                .map((Double value) -> Rotation2d.fromRotations(value / TURN_GEAR_RATIO))
+                .toArray(Rotation2d[]::new);
     }
 
     @Override