ColorSensorMuxed improvements

src/main/java/frc/robot/common/hardware/ColorSensorMuxed.java

@@ -25,8 +25,8 @@ public class ColorSensorMuxed {
   private I2C i2cMux;
   private final int tca9548Addr = 0x70;
   private ArrayList<Integer> i2cPorts;
-  private double lastProxRead;
-  private double lastColorRead;
+  private double[] lastProxReads;
+  private double[] lastColorReads;
   private int[] proximities;
   private Color[] colors;
   private double sensorPeriodInSeconds = MeasurementRate.kRate10Hz.period;
@@ -60,8 +60,8 @@ public ColorSensorMuxed(int... ports) {
     }
     proximities = new int[i2cPorts.size()];
     colors = new Color[i2cPorts.size()];
-    lastProxRead = 0;
-    lastColorRead = 0;
+    lastProxReads = new double[i2cPorts.size()];
+    lastColorReads = new double[i2cPorts.size()];
   }
 
   public boolean configureMeasurementRates(MeasurementRate rate) {
@@ -130,37 +130,59 @@ private boolean setI2cPort(int port) {
     }
   }
 
-  public Color[] getColors() {
-    if (Timer.getFPGATimestamp() - lastColorRead > sensorPeriodInSeconds) {
-      int i = 0;
-      for (int p : i2cPorts) {
-        if (setI2cPort(p)) {
-          colors[i] = sensors.getColor();
-        } else {
-          DriverStation.reportError("Failed to get color from sensor on I2C port " + p, false);
-          colors[i] = new Color(0, 0, 0);
-        }
-        i++;
+  public Color getColor(int i2cPort) {
+    int i = i2cPorts.indexOf(i2cPort);
+    if (i == -1) {
+      DriverStation.reportError("Invalid I2C port " + i2cPort, false);
+      return new Color(0, 0, 0);
+    }
+    if (Timer.getFPGATimestamp() - lastColorReads[i] > sensorPeriodInSeconds) {
+      if (setI2cPort(i2cPort)) {
+        colors[i] = sensors.getColor();
+        lastColorReads[i] = Timer.getFPGATimestamp();
+      } else {
+        DriverStation.reportError("Failed to get color from sensor on I2C port " + i2cPort, false);
+        colors[i] = new Color(0, 0, 0);
       }
-      lastColorRead = Timer.getFPGATimestamp();
     }
-    return colors;
+    return colors[i];
   }
 
-  public int[] getProximities() {
-    if (Timer.getFPGATimestamp() - lastProxRead > sensorPeriodInSeconds) {
-      int i = 0;
-      for (int p : i2cPorts) {
-        if (setI2cPort(p)) {
-          proximities[i] = sensors.getProximity();
-        } else {
-          DriverStation.reportError("Failed to get proximity from sensor on I2C port " + p, false);
-          proximities[i] = 0;
-        }
-        i++;
+  public int getProximity(int i2cPort) {
+
+    int i = i2cPorts.indexOf(i2cPort);
+    if (i == -1) {
+      DriverStation.reportError("Invalid I2C port " + i2cPort, false);
+      return 0;
+    }
+    if (Timer.getFPGATimestamp() - lastProxReads[i] > sensorPeriodInSeconds) {
+      if (setI2cPort(i2cPort)) {
+        proximities[i] = sensors.getProximity();
+        lastProxReads[i] = Timer.getFPGATimestamp();
+      } else {
+        DriverStation.reportError(
+            "Failed to get proximity from sensor on I2C port " + i2cPort, false);
+        proximities[i] = 0;
       }
-      lastProxRead = Timer.getFPGATimestamp();
+    }
+    return proximities[i];
+  }
+
+  public int[] getProximities() {
+    int i = 0;
+    for (int p : i2cPorts) {
+      proximities[i] = getProximity(p);
+      i++;
     }
     return proximities;
   }
+
+  public Color[] getColors() {
+    int i = 0;
+    for (int p : i2cPorts) {
+      colors[i] = getColor(p);
+      i++;
+    }
+    return colors;
+  }
 }

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

@@ -68,7 +68,6 @@ public enum ManagementState {
           .withPosition(3, 1)
           .getEntry();
 
-
   private ShuffleboardTab CDSTab = Shuffleboard.getTab("CDS Tab");
   private NetworkTableEntry ballColor = CDSTab.add("Ball Color", "Blue").getEntry();
   // private NetworkTableEntry CDSBallCount =
@@ -79,7 +78,8 @@ public enum ManagementState {
   private NetworkTableEntry CDSBallCount = CDSTab.add("Ball Count", 0).getEntry();
   private NetworkTableEntry CDSState = CDSTab.add("CDS State", "IDLE").getEntry();
   private NetworkTableEntry managementOnOff =
-      operatorTab.add("Run Auto Intake and Eject", true)
+      operatorTab
+          .add("Run Auto Intake and Eject", true)
           .withWidget(BuiltInWidgets.kToggleButton)
           .withPosition(5, 1)
           .getEntry();