From 6f0242511ab31d860f9a8d89909285f39af1486f Mon Sep 17 00:00:00 2001 From: Adam Vander Pas Date: Mon, 4 Apr 2022 16:12:41 -0500 Subject: [PATCH 1/3] Make ColorSensorMuxed accesses indexable --- .../common/hardware/ColorSensorMuxed.java | 80 ++++++++++++------- 1 file changed, 51 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc/robot/common/hardware/ColorSensorMuxed.java b/src/main/java/frc/robot/common/hardware/ColorSensorMuxed.java index 652db808..13e17fef 100644 --- a/src/main/java/frc/robot/common/hardware/ColorSensorMuxed.java +++ b/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 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(); + } else { + DriverStation.reportError("Failed to get color from sensor on I2C port " + i2cPort, false); + colors[i] = new Color(0, 0, 0); } - lastColorRead = Timer.getFPGATimestamp(); + lastColorReads[i] = 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(); + } else { + DriverStation.reportError( + "Failed to get proximity from sensor on I2C port " + i2cPort, false); + proximities[i] = 0; } - lastProxRead = Timer.getFPGATimestamp(); + } + lastProxReads[i] = 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; + } } From 30a3896519244b3d8d29a0fd7b338414255a0e6d Mon Sep 17 00:00:00 2001 From: Adam Vander Pas Date: Mon, 4 Apr 2022 16:12:49 -0500 Subject: [PATCH 2/3] spotless run --- src/main/java/frc/robot/subsystems/CDSSubsystem.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/CDSSubsystem.java b/src/main/java/frc/robot/subsystems/CDSSubsystem.java index 779a8dc9..193ecd67 100644 --- a/src/main/java/frc/robot/subsystems/CDSSubsystem.java +++ b/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(); From 5ea07ea0e5aa98c2cb15d6379ebd9ca9a8361b41 Mon Sep 17 00:00:00 2001 From: Adam Vander Pas Date: Mon, 4 Apr 2022 16:17:09 -0500 Subject: [PATCH 3/3] Only update last read timestamp if we successfully read --- src/main/java/frc/robot/common/hardware/ColorSensorMuxed.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/common/hardware/ColorSensorMuxed.java b/src/main/java/frc/robot/common/hardware/ColorSensorMuxed.java index 13e17fef..a60d8380 100644 --- a/src/main/java/frc/robot/common/hardware/ColorSensorMuxed.java +++ b/src/main/java/frc/robot/common/hardware/ColorSensorMuxed.java @@ -139,11 +139,11 @@ public Color getColor(int i2cPort) { 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); } - lastColorReads[i] = Timer.getFPGATimestamp(); } return colors[i]; } @@ -158,13 +158,13 @@ public int getProximity(int i2cPort) { 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; } } - lastProxReads[i] = Timer.getFPGATimestamp(); return proximities[i]; }