diff --git a/drivers/src/main/java/org/wpilib/drivers/range/RevColorSensorV2.java b/drivers/src/main/java/org/wpilib/drivers/range/RevColorSensorV2.java
new file mode 100644
index 00000000000..55a46d2e471
--- /dev/null
+++ b/drivers/src/main/java/org/wpilib/drivers/range/RevColorSensorV2.java
@@ -0,0 +1,883 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+//
+// This file is based on the FTC SDK drivers for the AMS TMD3782 family of color
+// sensors, which were made available under the BSD 3-Clause License.
+// Copyright (c) 2016-2017 Robert Atkinson and Steve Geffner.
+
+package org.wpilib.drivers.range;
+
+import static org.wpilib.units.Units.Meters;
+import static org.wpilib.units.Units.Milliseconds;
+
+import org.wpilib.hardware.bus.I2C;
+import org.wpilib.units.measure.Distance;
+import org.wpilib.units.measure.Time;
+import org.wpilib.util.Color;
+import org.wpilib.util.ErrorMessages;
+import org.wpilib.util.UsageReporting;
+
+/**
+ * Driver for the REV Robotics Color Sensor V2.
+ *
+ * The sensor is an AMS TMD37821, which combines an RGBC color sensor with an infrared proximity
+ * sensor. The proximity channel is exposed both as a calibrated distance and as a raw reflected
+ * light level.
+ *
+ *
Call {@link #update()} once per robot loop. All accessors return the values cached by the most
+ * recent update; they do not access the device.
+ *
+ *
Aborted I2C transactions, an unexpected device ID, and data the sensor reports as not yet
+ * valid are reported through {@link DeviceStatus} instead of throwing an exception, and the
+ * previously cached measurements are preserved. Unless otherwise noted, methods that access the
+ * device throw {@link IllegalStateException} after this driver is closed.
+ */
+public class RevColorSensorV2 implements AutoCloseable {
+ /** Default 7-bit I2C address of the Color Sensor V2. */
+ public static final int DEFAULT_ADDRESS = 0x39;
+
+ /** Every register access is prefixed with the command bit. */
+ private static final int COMMAND_BIT = 0x80;
+
+ /**
+ * Command type selecting the auto-increment protocol. Without it the sensor uses the
+ * repeated-byte protocol, which returns the addressed register once per byte instead of advancing
+ * through consecutive registers, so a multi-byte read would return the same register repeatedly.
+ */
+ private static final int COMMAND_TYPE_AUTO_INCREMENT = 0x01 << 5;
+
+ /** Register address prefix used for every read and write. */
+ private static final int COMMAND_AUTO_INCREMENT = COMMAND_BIT | COMMAND_TYPE_AUTO_INCREMENT;
+
+ private static final byte TMD37821_DEVICE_ID = 0x60;
+ private static final byte TMD37823_DEVICE_ID = 0x69;
+
+ private static final int ENABLE_POWER_ON = 0x01;
+ private static final int ENABLE_COLOR = 0x02;
+ private static final int ENABLE_PROXIMITY = 0x04;
+ private static final int ENABLE_WAIT = 0x08;
+ private static final int ENABLE_WRITABLE_MASK =
+ ENABLE_POWER_ON | ENABLE_COLOR | ENABLE_PROXIMITY | ENABLE_WAIT;
+
+ private static final int STATUS_COLOR_VALID = 0x01;
+ private static final int STATUS_PROXIMITY_VALID = 0x02;
+
+ /**
+ * CONTROL proximity diode select field (bits 5:4), set to the value that measures proximity with
+ * the infrared diode. The other values of this field are reserved.
+ */
+ private static final int CONTROL_PDIODE_IR = 0x02 << 4;
+
+ /** STATUS through the high byte of the proximity data, read as one block. */
+ private static final int BULK_READ_LENGTH = 11;
+
+ private static final int CLEAR_OFFSET = 1;
+ private static final int RED_OFFSET = 3;
+ private static final int GREEN_OFFSET = 5;
+ private static final int BLUE_OFFSET = 7;
+ private static final int PROXIMITY_OFFSET = 9;
+
+ private static final double INTEGRATION_CYCLE_MILLISECONDS = 2.4;
+ private static final int MAX_INTEGRATION_CYCLES = 256;
+ private static final int COUNTS_PER_INTEGRATION_CYCLE = 1024;
+ private static final int MAX_RAW_COLOR_VALUE = 65535;
+ private static final int PROXIMITY_SATURATION = 1023;
+
+ /** The datasheet specifies a 2.4 ms warm-up delay after power on. */
+ private static final int POWER_ON_DELAY_MILLISECONDS = 3;
+
+ private static final int ENABLE_SETTLE_DELAY_MILLISECONDS = 5;
+
+ private static final double DEFAULT_INTEGRATION_TIME_MILLISECONDS = 24.0;
+ private static final int DEFAULT_PROXIMITY_PULSE_COUNT = 8;
+
+ private static final double DEFAULT_A_PARAM = 186.347;
+ private static final double DEFAULT_B_PARAM = 30403.5;
+ private static final double DEFAULT_C_PARAM = 0.576649;
+
+ /** Register map for the TMD37821. */
+ public enum Register {
+ /** Enables states and interrupts. */
+ ENABLE(0x00),
+ /** RGBC integration time. */
+ ATIME(0x01),
+ /** Wait time. */
+ WTIME(0x03),
+ /** Clear channel interrupt low threshold. */
+ AILT(0x04),
+ /** Clear channel interrupt high threshold. */
+ AIHT(0x06),
+ /** Interrupt persistence filter. */
+ PERS(0x0C),
+ /** Configuration. */
+ CONFIGURATION(0x0D),
+ /** Proximity LED pulse count. */
+ PPULSE(0x0E),
+ /** Gain and proximity LED drive control. */
+ CONTROL(0x0F),
+ /** Device identifier. */
+ DEVICE_ID(0x12),
+ /** Device status. */
+ STATUS(0x13),
+ /** Clear channel data. */
+ CLEAR(0x14),
+ /** Red channel data. */
+ RED(0x16),
+ /** Green channel data. */
+ GREEN(0x18),
+ /** Blue channel data. */
+ BLUE(0x1A),
+ /** Proximity data. */
+ PROXIMITY(0x1C);
+
+ private final int m_address;
+
+ Register(int address) {
+ m_address = address;
+ }
+
+ /**
+ * Returns the address of this register.
+ *
+ * @return register address
+ */
+ public int getAddress() {
+ return m_address;
+ }
+ }
+
+ /** Analog gain applied to the color channels by the sensor. */
+ public enum Gain {
+ /** 1x gain. */
+ GAIN_1(0x00),
+ /** 4x gain. */
+ GAIN_4(0x01),
+ /** 16x gain. */
+ GAIN_16(0x02),
+ /** 60x gain. */
+ GAIN_60(0x03);
+
+ private final int m_value;
+
+ Gain(int value) {
+ m_value = value;
+ }
+ }
+
+ /** Nominal drive current of the proximity LED. */
+ public enum LedDrive {
+ /** 100% drive current. */
+ PERCENT_100(0x00 << 6),
+ /** 50% drive current. */
+ PERCENT_50(0x01 << 6),
+ /** 25% drive current. */
+ PERCENT_25(0x02 << 6),
+ /** 12.5% drive current. */
+ PERCENT_12_5(0x03 << 6);
+
+ private final int m_value;
+
+ LedDrive(int value) {
+ m_value = value;
+ }
+ }
+
+ /** Sensor status, including device-reported faults and local read failures. */
+ public enum DeviceStatus {
+ /** The sensor has not been configured successfully yet. */
+ NOT_INITIALIZED,
+ /** The sensor is operating normally. */
+ READY,
+ /** The device ID register did not report a TMD3782 family part. */
+ FAULT_UNEXPECTED_DEVICE_ID,
+ /** An I2C transaction was aborted, or the sensor reported that its data was not valid. */
+ FAULT_BAD_READ
+ }
+
+ /** Reason the driver rejected data or an I2C transaction. */
+ public enum FailureReason {
+ /** The I2C controller aborted a read transaction. */
+ I2C_READ_ABORTED,
+ /** The I2C controller aborted a write transaction. */
+ I2C_WRITE_ABORTED,
+ /** The device ID register did not report a TMD3782 family part. */
+ UNEXPECTED_DEVICE_ID,
+ /** The sensor had not completed an RGBC cycle since the color channels were enabled. */
+ COLOR_DATA_NOT_VALID,
+ /** The sensor had not completed a proximity cycle since the proximity channel was enabled. */
+ PROXIMITY_DATA_NOT_VALID
+ }
+
+ private I2C m_i2c;
+
+ private Gain m_gain = Gain.GAIN_4;
+ private LedDrive m_ledDrive = LedDrive.PERCENT_50;
+ private int m_integrationTimeRegister =
+ integrationTimeRegister(DEFAULT_INTEGRATION_TIME_MILLISECONDS);
+ private int m_proximityPulseCount = DEFAULT_PROXIMITY_PULSE_COUNT;
+ private double m_softwareGain = 1.0;
+
+ private double m_aParam = DEFAULT_A_PARAM;
+ private double m_bParam = DEFAULT_B_PARAM;
+ private double m_cParam = DEFAULT_C_PARAM;
+
+ private boolean m_initialized;
+ private byte m_deviceId;
+ private DeviceStatus m_deviceStatus = DeviceStatus.NOT_INITIALIZED;
+ private FailureReason m_lastFailureReason;
+ private long m_failureCount;
+
+ private int m_rawClear;
+ private int m_rawRed;
+ private int m_rawGreen;
+ private int m_rawBlue;
+ private int m_rawProximity;
+
+ /**
+ * Constructs a Color Sensor V2 on its default I2C address and configures it.
+ *
+ * @param port I2C port to which the sensor is connected
+ * @throws NullPointerException if {@code port} is null
+ */
+ public RevColorSensorV2(I2C.Port port) {
+ this(port, DEFAULT_ADDRESS);
+ }
+
+ /**
+ * Constructs a Color Sensor V2 and configures it.
+ *
+ *
Configuration failures are reported through {@link #getDeviceStatus()}; {@link #update()}
+ * retries configuration until it succeeds.
+ *
+ * @param port I2C port to which the sensor is connected
+ * @param deviceAddress 7-bit I2C address
+ * @throws NullPointerException if {@code port} is null
+ * @throws IllegalArgumentException if {@code deviceAddress} is outside the 7-bit address range
+ */
+ public RevColorSensorV2(I2C.Port port, int deviceAddress) {
+ ErrorMessages.requireNonNullParam(port, "port", "RevColorSensorV2");
+ if (deviceAddress < 0 || deviceAddress > 0x7f) {
+ throw new IllegalArgumentException("deviceAddress must be a 7-bit I2C address");
+ }
+
+ m_i2c = new I2C(port, deviceAddress);
+ UsageReporting.reportUsage("I2C[" + port.value + "]", deviceAddress, "RevColorSensorV2");
+ initialize();
+ }
+
+ RevColorSensorV2(I2C i2c) {
+ m_i2c = ErrorMessages.requireNonNullParam(i2c, "i2c", "RevColorSensorV2");
+ initialize();
+ }
+
+ /**
+ * Returns the I2C port.
+ *
+ * @return I2C port
+ * @throws IllegalStateException if this driver has been closed
+ */
+ public I2C.Port getPort() {
+ return requireOpen().getPort();
+ }
+
+ /**
+ * Returns the I2C address.
+ *
+ * @return 7-bit I2C address
+ * @throws IllegalStateException if this driver has been closed
+ */
+ public int getDeviceAddress() {
+ return requireOpen().getDeviceAddress();
+ }
+
+ /** Closes the I2C connection. Calling this method more than once has no effect. */
+ @Override
+ public void close() {
+ if (m_i2c != null) {
+ m_i2c.close();
+ m_i2c = null;
+ }
+ }
+
+ /**
+ * Reads the color and proximity channels and updates the cached measurements. Call this once per
+ * robot loop.
+ *
+ *
If the sensor has not been configured successfully, this retries configuration first, which
+ * blocks for approximately 8 ms.
+ *
+ *
An aborted I2C transaction or data the sensor reports as not yet valid does not throw; it
+ * sets the device status to {@link DeviceStatus#FAULT_BAD_READ} and preserves the affected
+ * measurements.
+ *
+ * @throws IllegalStateException if this driver has been closed
+ */
+ public void update() {
+ requireOpen();
+ if (!m_initialized && !initialize()) {
+ return;
+ }
+
+ m_deviceStatus = DeviceStatus.READY;
+ byte[] data = readRegister(Register.STATUS, BULK_READ_LENGTH);
+ if (data.length == 0) {
+ return;
+ }
+
+ int status = data[0] & 0xFF;
+ if ((status & STATUS_COLOR_VALID) == 0) {
+ recordFailure(FailureReason.COLOR_DATA_NOT_VALID);
+ } else {
+ m_rawClear = decodeUnsignedShort(data, CLEAR_OFFSET);
+ m_rawRed = decodeUnsignedShort(data, RED_OFFSET);
+ m_rawGreen = decodeUnsignedShort(data, GREEN_OFFSET);
+ m_rawBlue = decodeUnsignedShort(data, BLUE_OFFSET);
+ }
+
+ if ((status & STATUS_PROXIMITY_VALID) == 0) {
+ recordFailure(FailureReason.PROXIMITY_DATA_NOT_VALID);
+ } else {
+ m_rawProximity = decodeUnsignedShort(data, PROXIMITY_OFFSET);
+ }
+ }
+
+ /**
+ * Sets the analog gain applied to the color channels.
+ *
+ *
The sensor only accepts gain changes while its integrator is off, so this briefly disables
+ * and reenables the sensor, blocking for approximately 8 ms. If the sensor has not been
+ * configured successfully, the gain is applied the next time configuration succeeds.
+ *
+ * @param gain color channel gain
+ * @throws NullPointerException if {@code gain} is null
+ * @throws IllegalStateException if this driver has been closed
+ */
+ public void setGain(Gain gain) {
+ m_gain = ErrorMessages.requireNonNullParam(gain, "gain", "setGain");
+ reconfigure();
+ }
+
+ /**
+ * Returns the configured analog gain.
+ *
+ * @return color channel gain
+ */
+ public Gain getGain() {
+ return m_gain;
+ }
+
+ /**
+ * Sets the nominal drive current of the proximity LED.
+ *
+ *
The distance calibration returned by {@link #getDistanceMeters()} was fitted at {@link
+ * LedDrive#PERCENT_50}, the default. Changing the drive current changes the reflected light level
+ * for a given distance, so {@link #setDistanceCalibration(double, double, double)} must be used
+ * to refit the calibration.
+ *
+ *
This briefly disables and reenables the sensor, blocking for approximately 8 ms. If the
+ * sensor has not been configured successfully, the drive current is applied the next time
+ * configuration succeeds.
+ *
+ * @param ledDrive proximity LED drive current
+ * @throws NullPointerException if {@code ledDrive} is null
+ * @throws IllegalStateException if this driver has been closed
+ */
+ public void setLedDrive(LedDrive ledDrive) {
+ m_ledDrive = ErrorMessages.requireNonNullParam(ledDrive, "ledDrive", "setLedDrive");
+ reconfigure();
+ }
+
+ /**
+ * Returns the configured proximity LED drive current.
+ *
+ * @return proximity LED drive current
+ */
+ public LedDrive getLedDrive() {
+ return m_ledDrive;
+ }
+
+ /**
+ * Sets the color channel integration time.
+ *
+ *
The sensor integrates in 2.4 ms cycles, so the time is rounded up to the next whole cycle,
+ * up to a maximum of 614.4 ms. A longer integration time raises {@link
+ * #getMaximumRawColorValue()} and therefore the resolution of the color channels.
+ *
+ *
The sensor only accepts integration time changes while its integrator is off, so this
+ * briefly disables and reenables the sensor, blocking for approximately 8 ms. If the sensor has
+ * not been configured successfully, the integration time is applied the next time configuration
+ * succeeds.
+ *
+ * @param integrationTime color channel integration time
+ * @throws NullPointerException if {@code integrationTime} is null
+ * @throws IllegalArgumentException if {@code integrationTime} is nonfinite or not positive
+ * @throws IllegalStateException if this driver has been closed
+ */
+ public void setIntegrationTime(Time integrationTime) {
+ ErrorMessages.requireNonNullParam(integrationTime, "integrationTime", "setIntegrationTime");
+ m_integrationTimeRegister = integrationTimeRegister(integrationTime.in(Milliseconds));
+ reconfigure();
+ }
+
+ /**
+ * Returns the configured color channel integration time.
+ *
+ * @return color channel integration time
+ */
+ public Time getIntegrationTime() {
+ return Milliseconds.of(integrationCycles() * INTEGRATION_CYCLE_MILLISECONDS);
+ }
+
+ /**
+ * Sets the number of times the proximity LED is pulsed during each proximity cycle. More pulses
+ * raise the reflected light level for a given distance.
+ *
+ *
The distance calibration returned by {@link #getDistanceMeters()} was fitted at the default
+ * of 8 pulses, so changing the pulse count requires refitting the calibration with {@link
+ * #setDistanceCalibration(double, double, double)}.
+ *
+ *
This briefly disables and reenables the sensor, blocking for approximately 8 ms. If the
+ * sensor has not been configured successfully, the pulse count is applied the next time
+ * configuration succeeds.
+ *
+ * @param proximityPulseCount number of LED pulses per proximity cycle, from 1 to 255
+ * @throws IllegalArgumentException if {@code proximityPulseCount} is outside 1 to 255
+ * @throws IllegalStateException if this driver has been closed
+ */
+ public void setProximityPulseCount(int proximityPulseCount) {
+ if (proximityPulseCount < 1 || proximityPulseCount > 255) {
+ throw new IllegalArgumentException("proximityPulseCount must be between 1 and 255");
+ }
+ m_proximityPulseCount = proximityPulseCount;
+ reconfigure();
+ }
+
+ /**
+ * Returns the configured number of proximity LED pulses per proximity cycle.
+ *
+ * @return number of LED pulses per proximity cycle
+ */
+ public int getProximityPulseCount() {
+ return m_proximityPulseCount;
+ }
+
+ /**
+ * Sets a scale factor applied by this driver to the normalized color channels. This is applied in
+ * software after the sensor's own gain; it does not change the raw channel values.
+ *
+ * @param softwareGain scale factor applied to the normalized color channels
+ * @throws IllegalArgumentException if {@code softwareGain} is nonfinite or not positive
+ */
+ public void setSoftwareGain(double softwareGain) {
+ if (!Double.isFinite(softwareGain) || softwareGain <= 0.0) {
+ throw new IllegalArgumentException("softwareGain must be finite and greater than zero");
+ }
+ m_softwareGain = softwareGain;
+ }
+
+ /**
+ * Returns the scale factor applied by this driver to the normalized color channels.
+ *
+ * @return scale factor applied to the normalized color channels
+ */
+ public double getSoftwareGain() {
+ return m_softwareGain;
+ }
+
+ /**
+ * Sets the parameters of the curve that converts the raw proximity reading into a distance.
+ *
+ *
The raw proximity signal follows an inverse square law, and the parameters fit it to a
+ * linear measure of distance in centimeters:
+ *
+ *
rawProximity = a + b * (cm + c)^-2
+ *
+ * The default parameters were fitted for this sensor at the default LED drive current and
+ * proximity pulse count. Because the fit is affected by the infrared reflectivity of the target
+ * surface, the linearity it produces is usually preserved on other surfaces even when the scale
+ * is not, so a simple multiplicative correction is often enough to retune it.
+ *
+ * @param a the {@code a} parameter of the fitted curve
+ * @param b the {@code b} parameter of the fitted curve
+ * @param c the {@code c} parameter of the fitted curve
+ * @throws IllegalArgumentException if any parameter is nonfinite
+ */
+ public void setDistanceCalibration(double a, double b, double c) {
+ if (!Double.isFinite(a) || !Double.isFinite(b) || !Double.isFinite(c)) {
+ throw new IllegalArgumentException("Distance calibration parameters must be finite");
+ }
+ m_aParam = a;
+ m_bParam = b;
+ m_cParam = c;
+ }
+
+ /**
+ * Returns the distance to the target measured by the infrared proximity channel, from the plastic
+ * housing at the front of the sensor.
+ *
+ *
Readings are most accurate perpendicular to the target surface; a cosine correction is
+ * usually appropriate otherwise. The usable range is roughly 1 to 10 cm, and this returns {@link
+ * Double#NaN} when the reflected light level is too low for the calibration curve, which is the
+ * case when the target is beyond that range.
+ *
+ * @return distance in meters, or {@link Double#NaN} if the target is out of range
+ * @throws IllegalStateException if this driver has been closed
+ */
+ public double getDistanceMeters() {
+ requireOpen();
+ if (m_rawProximity <= m_aParam) {
+ return Double.NaN;
+ }
+ double centimeters =
+ (-m_aParam * m_cParam
+ + m_cParam * m_rawProximity
+ - Math.sqrt(-m_aParam * m_bParam + m_bParam * m_rawProximity))
+ / (m_aParam - m_rawProximity);
+ return centimeters / 100.0;
+ }
+
+ /**
+ * Returns the distance to the target measured by the infrared proximity channel, from the plastic
+ * housing at the front of the sensor.
+ *
+ * @return distance, or a measure of {@link Double#NaN} if the target is out of range
+ * @throws IllegalStateException if this driver has been closed
+ * @see #getDistanceMeters()
+ */
+ public Distance getDistance() {
+ return Meters.of(getDistanceMeters());
+ }
+
+ /**
+ * Returns the raw reflected light level measured by the infrared proximity channel. The value
+ * rises as the target gets closer.
+ *
+ * @return raw proximity reading, from 0 to {@link #getMaximumRawProximityValue()}
+ * @throws IllegalStateException if this driver has been closed
+ */
+ public int getRawProximity() {
+ requireOpen();
+ return m_rawProximity;
+ }
+
+ /**
+ * Returns the reflected light level measured by the infrared proximity channel, normalized
+ * against its saturation value. The value rises as the target gets closer.
+ *
+ * @return proximity reading, from 0 to 1
+ * @throws IllegalStateException if this driver has been closed
+ */
+ public double getProximity() {
+ requireOpen();
+ return Math.clamp(m_rawProximity / (double) PROXIMITY_SATURATION, 0.0, 1.0);
+ }
+
+ /**
+ * Returns the raw proximity reading at which the proximity channel saturates.
+ *
+ * @return maximum raw proximity reading
+ */
+ public int getMaximumRawProximityValue() {
+ return PROXIMITY_SATURATION;
+ }
+
+ /**
+ * Returns the color measured by the sensor, with each channel normalized against {@link
+ * #getMaximumRawColorValue()} and scaled by the software gain.
+ *
+ * @return measured color
+ * @throws IllegalStateException if this driver has been closed
+ */
+ public Color getColor() {
+ return new Color(getRed(), getGreen(), getBlue());
+ }
+
+ /**
+ * Returns the normalized red channel.
+ *
+ * @return red channel, from 0 to 1
+ * @throws IllegalStateException if this driver has been closed
+ */
+ public double getRed() {
+ return normalize(getRawRed());
+ }
+
+ /**
+ * Returns the normalized green channel.
+ *
+ * @return green channel, from 0 to 1
+ * @throws IllegalStateException if this driver has been closed
+ */
+ public double getGreen() {
+ return normalize(getRawGreen());
+ }
+
+ /**
+ * Returns the normalized blue channel.
+ *
+ * @return blue channel, from 0 to 1
+ * @throws IllegalStateException if this driver has been closed
+ */
+ public double getBlue() {
+ return normalize(getRawBlue());
+ }
+
+ /**
+ * Returns the normalized clear channel, which measures unfiltered light and is a useful measure
+ * of overall brightness.
+ *
+ * @return clear channel, from 0 to 1
+ * @throws IllegalStateException if this driver has been closed
+ */
+ public double getClear() {
+ return normalize(getRawClear());
+ }
+
+ /**
+ * Returns the raw red channel count.
+ *
+ * @return red channel, from 0 to {@link #getMaximumRawColorValue()}
+ * @throws IllegalStateException if this driver has been closed
+ */
+ public int getRawRed() {
+ requireOpen();
+ return m_rawRed;
+ }
+
+ /**
+ * Returns the raw green channel count.
+ *
+ * @return green channel, from 0 to {@link #getMaximumRawColorValue()}
+ * @throws IllegalStateException if this driver has been closed
+ */
+ public int getRawGreen() {
+ requireOpen();
+ return m_rawGreen;
+ }
+
+ /**
+ * Returns the raw blue channel count.
+ *
+ * @return blue channel, from 0 to {@link #getMaximumRawColorValue()}
+ * @throws IllegalStateException if this driver has been closed
+ */
+ public int getRawBlue() {
+ requireOpen();
+ return m_rawBlue;
+ }
+
+ /**
+ * Returns the raw clear channel count.
+ *
+ * @return clear channel, from 0 to {@link #getMaximumRawColorValue()}
+ * @throws IllegalStateException if this driver has been closed
+ */
+ public int getRawClear() {
+ requireOpen();
+ return m_rawClear;
+ }
+
+ /**
+ * Returns the highest raw color channel count reachable with the configured integration time.
+ *
+ * @return maximum raw color channel count
+ */
+ public int getMaximumRawColorValue() {
+ return Math.min(MAX_RAW_COLOR_VALUE, COUNTS_PER_INTEGRATION_CYCLE * integrationCycles());
+ }
+
+ /**
+ * Returns the device identifier reported by the sensor.
+ *
+ * @return device identifier, or zero if it has not been read successfully
+ * @throws IllegalStateException if this driver has been closed
+ */
+ public byte getDeviceId() {
+ requireOpen();
+ return m_deviceId;
+ }
+
+ /**
+ * Returns the current device or read status.
+ *
+ * @return device status
+ * @throws IllegalStateException if this driver has been closed
+ */
+ public DeviceStatus getDeviceStatus() {
+ requireOpen();
+ return m_deviceStatus;
+ }
+
+ /**
+ * Returns the reason for the most recent driver-detected failure.
+ *
+ * @return failure reason, or null if the driver has not detected a failure
+ * @throws IllegalStateException if this driver has been closed
+ */
+ public FailureReason getLastFailureReason() {
+ requireOpen();
+ return m_lastFailureReason;
+ }
+
+ /**
+ * Returns the total number of failures detected by this driver instance.
+ *
+ * @return total failure count
+ * @throws IllegalStateException if this driver has been closed
+ */
+ public long getFailureCount() {
+ requireOpen();
+ return m_failureCount;
+ }
+
+ private I2C requireOpen() {
+ if (m_i2c == null) {
+ throw new IllegalStateException("RevColorSensorV2 has been closed");
+ }
+ return m_i2c;
+ }
+
+ private boolean initialize() {
+ m_initialized = false;
+ m_deviceStatus = DeviceStatus.NOT_INITIALIZED;
+
+ byte[] data = readRegister(Register.DEVICE_ID, 1);
+ if (data.length == 0) {
+ return false;
+ }
+ m_deviceId = data[0];
+ if (m_deviceId != TMD37821_DEVICE_ID && m_deviceId != TMD37823_DEVICE_ID) {
+ recordFailure(FailureReason.UNEXPECTED_DEVICE_ID);
+ return false;
+ }
+ return configure();
+ }
+
+ private void reconfigure() {
+ requireOpen();
+ if (m_initialized) {
+ configure();
+ }
+ }
+
+ /**
+ * Writes the cached configuration to the sensor. The integrator must be off while the gain and
+ * integration time are written, so the sensor is disabled first and reenabled afterwards.
+ *
+ * @return whether every transaction succeeded
+ */
+ private boolean configure() {
+ m_initialized = false;
+ return disable()
+ && writeRegister(Register.ATIME, m_integrationTimeRegister)
+ && writeControl()
+ && writeRegister(Register.PPULSE, m_proximityPulseCount)
+ && enable();
+ }
+
+ /**
+ * Writes every CONTROL field explicitly rather than preserving the register's current contents.
+ * Disabling the sensor does not reset CONTROL, so a value retained from an earlier configuration
+ * could leave a reserved proximity diode or proximity gain selection in place, which changes
+ * every proximity reading and invalidates the distance calibration.
+ *
+ *
This covers all eight bits: the LED drive occupies bits 7:6, the proximity diode select bits
+ * 5:4, the proximity gain bits 3:2, and the color gain bits 1:0. The proximity gain is left as
+ * zero, selecting the 1x gain the distance calibration was fitted at and the only value this part
+ * defines.
+ *
+ * @return whether the write succeeded
+ */
+ private boolean writeControl() {
+ int control = m_ledDrive.m_value | CONTROL_PDIODE_IR | m_gain.m_value;
+ return writeRegister(Register.CONTROL, control);
+ }
+
+ private boolean enable() {
+ if (!writeEnable(ENABLE_POWER_ON)) {
+ return false;
+ }
+ delay(POWER_ON_DELAY_MILLISECONDS);
+
+ if (!writeEnable(ENABLE_POWER_ON | ENABLE_COLOR | ENABLE_PROXIMITY)) {
+ return false;
+ }
+ delay(ENABLE_SETTLE_DELAY_MILLISECONDS);
+
+ m_initialized = true;
+ m_deviceStatus = DeviceStatus.READY;
+ return true;
+ }
+
+ private boolean disable() {
+ return writeEnable(0);
+ }
+
+ private boolean writeEnable(int value) {
+ // The interrupt enables are not used, and the reserved high bits must be written as zero.
+ return writeRegister(Register.ENABLE, value & ENABLE_WRITABLE_MASK);
+ }
+
+ private byte[] readRegister(Register register, int count) {
+ byte[] data = new byte[count];
+ if (requireOpen().read(register.m_address | COMMAND_AUTO_INCREMENT, count, data)) {
+ recordFailure(FailureReason.I2C_READ_ABORTED);
+ return new byte[0];
+ }
+ return data;
+ }
+
+ private boolean writeRegister(Register register, int value) {
+ if (requireOpen().write(register.m_address | COMMAND_AUTO_INCREMENT, value)) {
+ recordFailure(FailureReason.I2C_WRITE_ABORTED);
+ return false;
+ }
+ return true;
+ }
+
+ private void recordFailure(FailureReason reason) {
+ m_lastFailureReason = reason;
+ m_failureCount++;
+ m_deviceStatus =
+ reason == FailureReason.UNEXPECTED_DEVICE_ID
+ ? DeviceStatus.FAULT_UNEXPECTED_DEVICE_ID
+ : DeviceStatus.FAULT_BAD_READ;
+ }
+
+ private double normalize(int rawValue) {
+ return Math.clamp(m_softwareGain * rawValue / getMaximumRawColorValue(), 0.0, 1.0);
+ }
+
+ private int integrationCycles() {
+ return MAX_INTEGRATION_CYCLES - m_integrationTimeRegister;
+ }
+
+ /**
+ * Returns the ATIME register value that integrates for at least the given duration. ATIME is the
+ * two's complement of the number of 2.4 ms integration cycles.
+ *
+ * @param milliseconds requested integration time
+ * @return ATIME register value
+ */
+ private static int integrationTimeRegister(double milliseconds) {
+ if (!Double.isFinite(milliseconds) || milliseconds <= 0.0) {
+ throw new IllegalArgumentException("integrationTime must be finite and greater than zero");
+ }
+ int cycles = (int) Math.ceil(milliseconds / INTEGRATION_CYCLE_MILLISECONDS);
+ return MAX_INTEGRATION_CYCLES - Math.min(cycles, MAX_INTEGRATION_CYCLES);
+ }
+
+ private static int decodeUnsignedShort(byte[] data, int offset) {
+ return (data[offset] & 0xFF) | ((data[offset + 1] & 0xFF) << 8);
+ }
+
+ private static void delay(int milliseconds) {
+ try {
+ Thread.sleep(milliseconds);
+ } catch (InterruptedException e) {
+ Thread.currentThread().interrupt();
+ }
+ }
+}
diff --git a/drivers/src/main/java/org/wpilib/drivers/range/package-info.java b/drivers/src/main/java/org/wpilib/drivers/range/package-info.java
new file mode 100644
index 00000000000..4732a1f225b
--- /dev/null
+++ b/drivers/src/main/java/org/wpilib/drivers/range/package-info.java
@@ -0,0 +1,6 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+/** WPILib range and color sensor drivers. */
+package org.wpilib.drivers.range;
diff --git a/drivers/src/main/native/cpp/range/RevColorSensorV2.cpp b/drivers/src/main/native/cpp/range/RevColorSensorV2.cpp
new file mode 100644
index 00000000000..a80a703fff1
--- /dev/null
+++ b/drivers/src/main/native/cpp/range/RevColorSensorV2.cpp
@@ -0,0 +1,377 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+//
+// This file is based on the FTC SDK drivers for the AMS TMD3782 family of color
+// sensors, which were made available under the BSD 3-Clause License.
+// Copyright (c) 2016-2017 Robert Atkinson and Steve Geffner.
+
+#include "wpi/drivers/range/RevColorSensorV2.hpp"
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "wpi/util/UsageReporting.hpp"
+
+using namespace wpi;
+
+namespace {
+
+void Delay(int milliseconds) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(milliseconds));
+}
+
+int DecodeUnsignedShort(const std::vector& data, std::size_t offset) {
+ return data[offset] | (data[offset + 1] << 8);
+}
+
+} // namespace
+
+RevColorSensorV2::RevColorSensorV2(I2C::Port port, int deviceAddress)
+ : m_i2c{port, ValidateAddress(deviceAddress)} {
+ wpi::util::ReportUsage(std::format("I2C[{}]", static_cast(port)),
+ deviceAddress, "RevColorSensorV2");
+ Initialize();
+}
+
+I2C::Port RevColorSensorV2::GetPort() const {
+ return m_i2c.GetPort();
+}
+
+int RevColorSensorV2::GetDeviceAddress() const {
+ return m_i2c.GetDeviceAddress();
+}
+
+void RevColorSensorV2::Update() {
+ if (!m_initialized && !Initialize()) {
+ return;
+ }
+
+ m_deviceStatus = DeviceStatus::READY;
+ std::vector data = ReadRegister(Register::STATUS, BULK_READ_LENGTH);
+ if (data.empty()) {
+ return;
+ }
+
+ int status = data[0];
+ if ((status & STATUS_COLOR_VALID) == 0) {
+ RecordFailure(FailureReason::COLOR_DATA_NOT_VALID);
+ } else {
+ m_rawClear = DecodeUnsignedShort(data, CLEAR_OFFSET);
+ m_rawRed = DecodeUnsignedShort(data, RED_OFFSET);
+ m_rawGreen = DecodeUnsignedShort(data, GREEN_OFFSET);
+ m_rawBlue = DecodeUnsignedShort(data, BLUE_OFFSET);
+ }
+
+ if ((status & STATUS_PROXIMITY_VALID) == 0) {
+ RecordFailure(FailureReason::PROXIMITY_DATA_NOT_VALID);
+ } else {
+ m_rawProximity = DecodeUnsignedShort(data, PROXIMITY_OFFSET);
+ }
+}
+
+void RevColorSensorV2::SetGain(Gain gain) {
+ switch (gain) {
+ case Gain::GAIN_1:
+ case Gain::GAIN_4:
+ case Gain::GAIN_16:
+ case Gain::GAIN_60:
+ break;
+ default:
+ throw std::invalid_argument("Invalid gain");
+ }
+ m_gain = gain;
+ Reconfigure();
+}
+
+RevColorSensorV2::Gain RevColorSensorV2::GetGain() const {
+ return m_gain;
+}
+
+void RevColorSensorV2::SetLedDrive(LedDrive ledDrive) {
+ switch (ledDrive) {
+ case LedDrive::PERCENT_100:
+ case LedDrive::PERCENT_50:
+ case LedDrive::PERCENT_25:
+ case LedDrive::PERCENT_12_5:
+ break;
+ default:
+ throw std::invalid_argument("Invalid LED drive");
+ }
+ m_ledDrive = ledDrive;
+ Reconfigure();
+}
+
+RevColorSensorV2::LedDrive RevColorSensorV2::GetLedDrive() const {
+ return m_ledDrive;
+}
+
+void RevColorSensorV2::SetIntegrationTime(
+ wpi::units::millisecond_t integrationTime) {
+ m_integrationTimeRegister = IntegrationTimeRegister(integrationTime);
+ Reconfigure();
+}
+
+wpi::units::millisecond_t RevColorSensorV2::GetIntegrationTime() const {
+ return wpi::units::millisecond_t{IntegrationCycles() *
+ INTEGRATION_CYCLE_MILLISECONDS};
+}
+
+void RevColorSensorV2::SetProximityPulseCount(int proximityPulseCount) {
+ if (proximityPulseCount < 1 || proximityPulseCount > 255) {
+ throw std::invalid_argument(
+ "proximityPulseCount must be between 1 and 255");
+ }
+ m_proximityPulseCount = proximityPulseCount;
+ Reconfigure();
+}
+
+int RevColorSensorV2::GetProximityPulseCount() const {
+ return m_proximityPulseCount;
+}
+
+void RevColorSensorV2::SetSoftwareGain(double softwareGain) {
+ if (!std::isfinite(softwareGain) || softwareGain <= 0.0) {
+ throw std::invalid_argument(
+ "softwareGain must be finite and greater than zero");
+ }
+ m_softwareGain = softwareGain;
+}
+
+double RevColorSensorV2::GetSoftwareGain() const {
+ return m_softwareGain;
+}
+
+void RevColorSensorV2::SetDistanceCalibration(double a, double b, double c) {
+ if (!std::isfinite(a) || !std::isfinite(b) || !std::isfinite(c)) {
+ throw std::invalid_argument(
+ "Distance calibration parameters must be finite");
+ }
+ m_aParam = a;
+ m_bParam = b;
+ m_cParam = c;
+}
+
+wpi::units::meter_t RevColorSensorV2::GetDistance() const {
+ if (m_rawProximity <= m_aParam) {
+ return wpi::units::meter_t{std::numeric_limits::quiet_NaN()};
+ }
+ double centimeters =
+ (-m_aParam * m_cParam + m_cParam * m_rawProximity -
+ std::sqrt(-m_aParam * m_bParam + m_bParam * m_rawProximity)) /
+ (m_aParam - m_rawProximity);
+ return wpi::units::centimeter_t{centimeters};
+}
+
+int RevColorSensorV2::GetRawProximity() const {
+ return m_rawProximity;
+}
+
+double RevColorSensorV2::GetProximity() const {
+ return std::clamp(m_rawProximity / static_cast(PROXIMITY_SATURATION),
+ 0.0, 1.0);
+}
+
+int RevColorSensorV2::GetMaximumRawProximityValue() const {
+ return PROXIMITY_SATURATION;
+}
+
+wpi::util::Color RevColorSensorV2::GetColor() const {
+ return wpi::util::Color{GetRed(), GetGreen(), GetBlue()};
+}
+
+double RevColorSensorV2::GetRed() const {
+ return Normalize(m_rawRed);
+}
+
+double RevColorSensorV2::GetGreen() const {
+ return Normalize(m_rawGreen);
+}
+
+double RevColorSensorV2::GetBlue() const {
+ return Normalize(m_rawBlue);
+}
+
+double RevColorSensorV2::GetClear() const {
+ return Normalize(m_rawClear);
+}
+
+int RevColorSensorV2::GetRawRed() const {
+ return m_rawRed;
+}
+
+int RevColorSensorV2::GetRawGreen() const {
+ return m_rawGreen;
+}
+
+int RevColorSensorV2::GetRawBlue() const {
+ return m_rawBlue;
+}
+
+int RevColorSensorV2::GetRawClear() const {
+ return m_rawClear;
+}
+
+int RevColorSensorV2::GetMaximumRawColorValue() const {
+ return std::min(MAX_RAW_COLOR_VALUE,
+ COUNTS_PER_INTEGRATION_CYCLE * IntegrationCycles());
+}
+
+uint8_t RevColorSensorV2::GetDeviceId() const {
+ return m_deviceId;
+}
+
+RevColorSensorV2::DeviceStatus RevColorSensorV2::GetDeviceStatus() const {
+ return m_deviceStatus;
+}
+
+std::optional
+RevColorSensorV2::GetLastFailureReason() const {
+ return m_lastFailureReason;
+}
+
+uint64_t RevColorSensorV2::GetFailureCount() const {
+ return m_failureCount;
+}
+
+int RevColorSensorV2::ValidateAddress(int deviceAddress) {
+ if (deviceAddress < 0 || deviceAddress > 0x7f) {
+ throw std::invalid_argument("deviceAddress must be a 7-bit I2C address");
+ }
+ return deviceAddress;
+}
+
+int RevColorSensorV2::IntegrationTimeRegister(
+ wpi::units::millisecond_t integrationTime) {
+ double milliseconds = integrationTime.value();
+ if (!std::isfinite(milliseconds) || milliseconds <= 0.0) {
+ throw std::invalid_argument(
+ "integrationTime must be finite and greater than zero");
+ }
+ // The cycle count is clamped while it is still floating point. A duration
+ // longer than the sensor can integrate for produces a value beyond the range
+ // of int, and converting that to int is undefined behavior, so clamping after
+ // the conversion would be too late to saturate at the maximum.
+ double cycles = std::ceil(milliseconds / INTEGRATION_CYCLE_MILLISECONDS);
+ int clampedCycles = static_cast(
+ std::min(cycles, static_cast(MAX_INTEGRATION_CYCLES)));
+ return MAX_INTEGRATION_CYCLES - clampedCycles;
+}
+
+bool RevColorSensorV2::Initialize() {
+ m_initialized = false;
+ m_deviceStatus = DeviceStatus::NOT_INITIALIZED;
+
+ std::vector data = ReadRegister(Register::DEVICE_ID, 1);
+ if (data.empty()) {
+ return false;
+ }
+ m_deviceId = data[0];
+ if (m_deviceId != TMD37821_DEVICE_ID && m_deviceId != TMD37823_DEVICE_ID) {
+ RecordFailure(FailureReason::UNEXPECTED_DEVICE_ID);
+ return false;
+ }
+ return Configure();
+}
+
+void RevColorSensorV2::Reconfigure() {
+ if (m_initialized) {
+ Configure();
+ }
+}
+
+bool RevColorSensorV2::Configure() {
+ // The integrator must be off while the gain and integration time are
+ // written, so the sensor is disabled first and reenabled afterwards.
+ m_initialized = false;
+ return Disable() &&
+ WriteRegister(Register::ATIME, m_integrationTimeRegister) &&
+ WriteControl() &&
+ WriteRegister(Register::PPULSE, m_proximityPulseCount) && Enable();
+}
+
+bool RevColorSensorV2::WriteControl() {
+ // Every CONTROL field is written explicitly rather than preserving the
+ // register's current contents. Disabling the sensor does not reset CONTROL,
+ // so a value retained from an earlier configuration could leave a reserved
+ // proximity diode or proximity gain selection in place, which changes every
+ // proximity reading and invalidates the distance calibration.
+ //
+ // This covers all eight bits: the LED drive occupies bits 7:6, the proximity
+ // diode select bits 5:4, the proximity gain bits 3:2, and the color gain bits
+ // 1:0. The proximity gain is left as zero, selecting the 1x gain the distance
+ // calibration was fitted at and the only value this part defines.
+ int control = static_cast(m_ledDrive) | CONTROL_PDIODE_IR |
+ static_cast(m_gain);
+ return WriteRegister(Register::CONTROL, control);
+}
+
+bool RevColorSensorV2::Enable() {
+ if (!WriteEnable(ENABLE_POWER_ON)) {
+ return false;
+ }
+ Delay(POWER_ON_DELAY_MILLISECONDS);
+
+ if (!WriteEnable(ENABLE_POWER_ON | ENABLE_COLOR | ENABLE_PROXIMITY)) {
+ return false;
+ }
+ Delay(ENABLE_SETTLE_DELAY_MILLISECONDS);
+
+ m_initialized = true;
+ m_deviceStatus = DeviceStatus::READY;
+ return true;
+}
+
+bool RevColorSensorV2::Disable() {
+ return WriteEnable(0);
+}
+
+bool RevColorSensorV2::WriteEnable(int value) {
+ // The interrupt enables are not used, and the reserved high bits must be
+ // written as zero.
+ return WriteRegister(Register::ENABLE, value & ENABLE_WRITABLE_MASK);
+}
+
+std::vector RevColorSensorV2::ReadRegister(Register reg, int count) {
+ std::vector data(count);
+ if (m_i2c.Read(static_cast(reg) | COMMAND_AUTO_INCREMENT, count,
+ data.data())) {
+ RecordFailure(FailureReason::I2C_READ_ABORTED);
+ return {};
+ }
+ return data;
+}
+
+bool RevColorSensorV2::WriteRegister(Register reg, int value) {
+ if (m_i2c.Write(static_cast(reg) | COMMAND_AUTO_INCREMENT,
+ static_cast(value))) {
+ RecordFailure(FailureReason::I2C_WRITE_ABORTED);
+ return false;
+ }
+ return true;
+}
+
+void RevColorSensorV2::RecordFailure(FailureReason reason) {
+ m_lastFailureReason = reason;
+ ++m_failureCount;
+ m_deviceStatus = reason == FailureReason::UNEXPECTED_DEVICE_ID
+ ? DeviceStatus::FAULT_UNEXPECTED_DEVICE_ID
+ : DeviceStatus::FAULT_BAD_READ;
+}
+
+double RevColorSensorV2::Normalize(int rawValue) const {
+ return std::clamp(m_softwareGain * rawValue / GetMaximumRawColorValue(), 0.0,
+ 1.0);
+}
+
+int RevColorSensorV2::IntegrationCycles() const {
+ return MAX_INTEGRATION_CYCLES - m_integrationTimeRegister;
+}
diff --git a/drivers/src/main/native/include/wpi/drivers/range/RevColorSensorV2.hpp b/drivers/src/main/native/include/wpi/drivers/range/RevColorSensorV2.hpp
new file mode 100644
index 00000000000..53199e9ac91
--- /dev/null
+++ b/drivers/src/main/native/include/wpi/drivers/range/RevColorSensorV2.hpp
@@ -0,0 +1,488 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+//
+// This file is based on the FTC SDK drivers for the AMS TMD3782 family of color
+// sensors, which were made available under the BSD 3-Clause License.
+// Copyright (c) 2016-2017 Robert Atkinson and Steve Geffner.
+
+#pragma once
+
+#include
+#include
+#include
+
+#include "wpi/hardware/bus/I2C.hpp"
+#include "wpi/units/length.hpp"
+#include "wpi/units/time.hpp"
+#include "wpi/util/Color.hpp"
+
+namespace wpi {
+
+/**
+ * Driver for the REV Robotics Color Sensor V2.
+ *
+ * The sensor is an AMS TMD37821, which combines an RGBC color sensor with an
+ * infrared proximity sensor. The proximity channel is exposed both as a
+ * calibrated distance and as a raw reflected light level.
+ *
+ * Call Update() once per robot loop. All accessors return the values cached by
+ * the most recent update; they do not access the device.
+ *
+ * Aborted I2C transactions, an unexpected device ID, and data the sensor
+ * reports as not yet valid are reported through DeviceStatus instead of
+ * throwing an exception, and the previously cached measurements are preserved.
+ */
+class RevColorSensorV2 {
+ public:
+ /// Default 7-bit I2C address of the Color Sensor V2.
+ static constexpr int DEFAULT_ADDRESS = 0x39;
+
+ /** Register map for the TMD37821. */
+ enum class Register {
+ /// Enables states and interrupts.
+ ENABLE = 0x00,
+ /// RGBC integration time.
+ ATIME = 0x01,
+ /// Wait time.
+ WTIME = 0x03,
+ /// Clear channel interrupt low threshold.
+ AILT = 0x04,
+ /// Clear channel interrupt high threshold.
+ AIHT = 0x06,
+ /// Interrupt persistence filter.
+ PERS = 0x0C,
+ /// Configuration.
+ CONFIGURATION = 0x0D,
+ /// Proximity LED pulse count.
+ PPULSE = 0x0E,
+ /// Gain and proximity LED drive control.
+ CONTROL = 0x0F,
+ /// Device identifier.
+ DEVICE_ID = 0x12,
+ /// Device status.
+ STATUS = 0x13,
+ /// Clear channel data.
+ CLEAR = 0x14,
+ /// Red channel data.
+ RED = 0x16,
+ /// Green channel data.
+ GREEN = 0x18,
+ /// Blue channel data.
+ BLUE = 0x1A,
+ /// Proximity data.
+ PROXIMITY = 0x1C
+ };
+
+ /** Analog gain applied to the color channels by the sensor. */
+ enum class Gain {
+ /// 1x gain.
+ GAIN_1 = 0x00,
+ /// 4x gain.
+ GAIN_4 = 0x01,
+ /// 16x gain.
+ GAIN_16 = 0x02,
+ /// 60x gain.
+ GAIN_60 = 0x03
+ };
+
+ /** Nominal drive current of the proximity LED. */
+ enum class LedDrive {
+ /// 100% drive current.
+ PERCENT_100 = 0x00 << 6,
+ /// 50% drive current.
+ PERCENT_50 = 0x01 << 6,
+ /// 25% drive current.
+ PERCENT_25 = 0x02 << 6,
+ /// 12.5% drive current.
+ PERCENT_12_5 = 0x03 << 6
+ };
+
+ /** Sensor status, including device-reported faults and local read failures.
+ */
+ enum class DeviceStatus {
+ /// The sensor has not been configured successfully yet.
+ NOT_INITIALIZED,
+ /// The sensor is operating normally.
+ READY,
+ /// The device ID register did not report a TMD3782 family part.
+ FAULT_UNEXPECTED_DEVICE_ID,
+ /// An I2C transaction was aborted, or the sensor reported that its data was
+ /// not valid.
+ FAULT_BAD_READ
+ };
+
+ /** Reason the driver rejected data or an I2C transaction. */
+ enum class FailureReason {
+ /// The I2C controller aborted a read transaction.
+ I2C_READ_ABORTED,
+ /// The I2C controller aborted a write transaction.
+ I2C_WRITE_ABORTED,
+ /// The device ID register did not report a TMD3782 family part.
+ UNEXPECTED_DEVICE_ID,
+ /// The sensor had not completed an RGBC cycle since the color channels were
+ /// enabled.
+ COLOR_DATA_NOT_VALID,
+ /// The sensor had not completed a proximity cycle since the proximity
+ /// channel was enabled.
+ PROXIMITY_DATA_NOT_VALID
+ };
+
+ /**
+ * Constructs a Color Sensor V2 and configures it.
+ *
+ * Configuration failures are reported through GetDeviceStatus(); Update()
+ * retries configuration until it succeeds.
+ *
+ * @param port I2C port to which the sensor is connected.
+ * @param deviceAddress 7-bit I2C address.
+ * @throws std::invalid_argument if deviceAddress is outside the 7-bit range.
+ */
+ explicit RevColorSensorV2(I2C::Port port,
+ int deviceAddress = DEFAULT_ADDRESS);
+
+ RevColorSensorV2(RevColorSensorV2&&) = default;
+ RevColorSensorV2& operator=(RevColorSensorV2&&) = default;
+
+ /**
+ * Returns the I2C port.
+ *
+ * @return I2C port.
+ */
+ I2C::Port GetPort() const;
+
+ /**
+ * Returns the I2C address.
+ *
+ * @return 7-bit I2C address.
+ */
+ int GetDeviceAddress() const;
+
+ /**
+ * Reads the color and proximity channels and updates the cached
+ * measurements. Call this once per robot loop.
+ *
+ * If the sensor has not been configured successfully, this retries
+ * configuration first, which blocks for approximately 8 ms.
+ *
+ * An aborted I2C transaction or data the sensor reports as not yet valid
+ * does not throw; it sets the device status to DeviceStatus::FAULT_BAD_READ
+ * and preserves the affected measurements.
+ */
+ void Update();
+
+ /**
+ * Sets the analog gain applied to the color channels.
+ *
+ * The sensor only accepts gain changes while its integrator is off, so this
+ * briefly disables and reenables the sensor, blocking for approximately 8 ms.
+ * If the sensor has not been configured successfully, the gain is applied the
+ * next time configuration succeeds.
+ *
+ * @param gain Color channel gain.
+ * @throws std::invalid_argument if gain is invalid.
+ */
+ void SetGain(Gain gain);
+
+ /** @return Configured analog gain. */
+ Gain GetGain() const;
+
+ /**
+ * Sets the nominal drive current of the proximity LED.
+ *
+ * The distance calibration returned by GetDistance() was fitted at
+ * LedDrive::PERCENT_50, the default. Changing the drive current changes the
+ * reflected light level for a given distance, so SetDistanceCalibration()
+ * must be used to refit the calibration.
+ *
+ * This briefly disables and reenables the sensor, blocking for approximately
+ * 8 ms. If the sensor has not been configured successfully, the drive current
+ * is applied the next time configuration succeeds.
+ *
+ * @param ledDrive Proximity LED drive current.
+ * @throws std::invalid_argument if ledDrive is invalid.
+ */
+ void SetLedDrive(LedDrive ledDrive);
+
+ /** @return Configured proximity LED drive current. */
+ LedDrive GetLedDrive() const;
+
+ /**
+ * Sets the color channel integration time.
+ *
+ * The sensor integrates in 2.4 ms cycles, so the time is rounded up to the
+ * next whole cycle, up to a maximum of 614.4 ms. A longer integration time
+ * raises GetMaximumRawColorValue() and therefore the resolution of the color
+ * channels.
+ *
+ * The sensor only accepts integration time changes while its integrator is
+ * off, so this briefly disables and reenables the sensor, blocking for
+ * approximately 8 ms. If the sensor has not been configured successfully, the
+ * integration time is applied the next time configuration succeeds.
+ *
+ * @param integrationTime Color channel integration time.
+ * @throws std::invalid_argument if integrationTime is nonfinite or not
+ * positive.
+ */
+ void SetIntegrationTime(wpi::units::millisecond_t integrationTime);
+
+ /** @return Configured color channel integration time. */
+ wpi::units::millisecond_t GetIntegrationTime() const;
+
+ /**
+ * Sets the number of times the proximity LED is pulsed during each proximity
+ * cycle. More pulses raise the reflected light level for a given distance.
+ *
+ * The distance calibration returned by GetDistance() was fitted at the
+ * default of 8 pulses, so changing the pulse count requires refitting the
+ * calibration with SetDistanceCalibration().
+ *
+ * This briefly disables and reenables the sensor, blocking for approximately
+ * 8 ms. If the sensor has not been configured successfully, the pulse count
+ * is applied the next time configuration succeeds.
+ *
+ * @param proximityPulseCount Number of LED pulses per proximity cycle, from
+ * 1 to 255.
+ * @throws std::invalid_argument if proximityPulseCount is outside 1 to 255.
+ */
+ void SetProximityPulseCount(int proximityPulseCount);
+
+ /** @return Configured number of proximity LED pulses per proximity cycle. */
+ int GetProximityPulseCount() const;
+
+ /**
+ * Sets a scale factor applied by this driver to the normalized color
+ * channels. This is applied in software after the sensor's own gain; it does
+ * not change the raw channel values.
+ *
+ * @param softwareGain Scale factor applied to the normalized color channels.
+ * @throws std::invalid_argument if softwareGain is nonfinite or not positive.
+ */
+ void SetSoftwareGain(double softwareGain);
+
+ /** @return Scale factor applied to the normalized color channels. */
+ double GetSoftwareGain() const;
+
+ /**
+ * Sets the parameters of the curve that converts the raw proximity reading
+ * into a distance.
+ *
+ * The raw proximity signal follows an inverse square law, and the parameters
+ * fit it to a linear measure of distance in centimeters:
+ *
+ * rawProximity = a + b * (cm + c)^-2
+ *
+ * The default parameters were fitted for this sensor at the default LED drive
+ * current and proximity pulse count. Because the fit is affected by the
+ * infrared reflectivity of the target surface, the linearity it produces is
+ * usually preserved on other surfaces even when the scale is not, so a simple
+ * multiplicative correction is often enough to retune it.
+ *
+ * @param a The a parameter of the fitted curve.
+ * @param b The b parameter of the fitted curve.
+ * @param c The c parameter of the fitted curve.
+ * @throws std::invalid_argument if any parameter is nonfinite.
+ */
+ void SetDistanceCalibration(double a, double b, double c);
+
+ /**
+ * Returns the distance to the target measured by the infrared proximity
+ * channel, from the plastic housing at the front of the sensor.
+ *
+ * Readings are most accurate perpendicular to the target surface; a cosine
+ * correction is usually appropriate otherwise. The usable range is roughly
+ * 1 to 10 cm, and this returns NaN when the reflected light level is too low
+ * for the calibration curve, which is the case when the target is beyond that
+ * range.
+ *
+ * @return Distance, or NaN if the target is out of range.
+ */
+ wpi::units::meter_t GetDistance() const;
+
+ /**
+ * Returns the raw reflected light level measured by the infrared proximity
+ * channel. The value rises as the target gets closer.
+ *
+ * @return Raw proximity reading, from 0 to GetMaximumRawProximityValue().
+ */
+ int GetRawProximity() const;
+
+ /**
+ * Returns the reflected light level measured by the infrared proximity
+ * channel, normalized against its saturation value. The value rises as the
+ * target gets closer.
+ *
+ * @return Proximity reading, from 0 to 1.
+ */
+ double GetProximity() const;
+
+ /** @return Raw proximity reading at which the proximity channel saturates. */
+ int GetMaximumRawProximityValue() const;
+
+ /**
+ * Returns the color measured by the sensor, with each channel normalized
+ * against GetMaximumRawColorValue() and scaled by the software gain.
+ *
+ * @return Measured color.
+ */
+ wpi::util::Color GetColor() const;
+
+ /** @return Normalized red channel, from 0 to 1. */
+ double GetRed() const;
+
+ /** @return Normalized green channel, from 0 to 1. */
+ double GetGreen() const;
+
+ /** @return Normalized blue channel, from 0 to 1. */
+ double GetBlue() const;
+
+ /**
+ * Returns the normalized clear channel, which measures unfiltered light and
+ * is a useful measure of overall brightness.
+ *
+ * @return Clear channel, from 0 to 1.
+ */
+ double GetClear() const;
+
+ /** @return Raw red channel count, from 0 to GetMaximumRawColorValue(). */
+ int GetRawRed() const;
+
+ /** @return Raw green channel count, from 0 to GetMaximumRawColorValue(). */
+ int GetRawGreen() const;
+
+ /** @return Raw blue channel count, from 0 to GetMaximumRawColorValue(). */
+ int GetRawBlue() const;
+
+ /** @return Raw clear channel count, from 0 to GetMaximumRawColorValue(). */
+ int GetRawClear() const;
+
+ /**
+ * Returns the highest raw color channel count reachable with the configured
+ * integration time.
+ *
+ * @return Maximum raw color channel count.
+ */
+ int GetMaximumRawColorValue() const;
+
+ /**
+ * Returns the device identifier reported by the sensor.
+ *
+ * @return Device identifier, or zero if it has not been read successfully.
+ */
+ uint8_t GetDeviceId() const;
+
+ /** @return Current device or read status. */
+ DeviceStatus GetDeviceStatus() const;
+
+ /**
+ * @return Reason for the most recent driver-detected failure, or std::nullopt
+ * if none has occurred.
+ */
+ std::optional GetLastFailureReason() const;
+
+ /** @return Total number of failures detected by this driver instance. */
+ uint64_t GetFailureCount() const;
+
+ private:
+ /// Every register access is prefixed with the command bit.
+ static constexpr int COMMAND_BIT = 0x80;
+
+ /// Command type selecting the auto-increment protocol. Without it the sensor
+ /// uses the repeated-byte protocol, which returns the addressed register once
+ /// per byte instead of advancing through consecutive registers, so a
+ /// multi-byte read would return the same register repeatedly.
+ static constexpr int COMMAND_TYPE_AUTO_INCREMENT = 0x01 << 5;
+
+ /// Register address prefix used for every read and write.
+ static constexpr int COMMAND_AUTO_INCREMENT =
+ COMMAND_BIT | COMMAND_TYPE_AUTO_INCREMENT;
+
+ static constexpr uint8_t TMD37821_DEVICE_ID = 0x60;
+ static constexpr uint8_t TMD37823_DEVICE_ID = 0x69;
+
+ static constexpr int ENABLE_POWER_ON = 0x01;
+ static constexpr int ENABLE_COLOR = 0x02;
+ static constexpr int ENABLE_PROXIMITY = 0x04;
+ static constexpr int ENABLE_WAIT = 0x08;
+ static constexpr int ENABLE_WRITABLE_MASK =
+ ENABLE_POWER_ON | ENABLE_COLOR | ENABLE_PROXIMITY | ENABLE_WAIT;
+
+ static constexpr int STATUS_COLOR_VALID = 0x01;
+ static constexpr int STATUS_PROXIMITY_VALID = 0x02;
+
+ /// CONTROL proximity diode select field (bits 5:4), set to the value that
+ /// measures proximity with the infrared diode. The other values of this field
+ /// are reserved.
+ static constexpr int CONTROL_PDIODE_IR = 0x02 << 4;
+
+ /// STATUS through the high byte of the proximity data, read as one block.
+ static constexpr int BULK_READ_LENGTH = 11;
+
+ static constexpr std::size_t CLEAR_OFFSET = 1;
+ static constexpr std::size_t RED_OFFSET = 3;
+ static constexpr std::size_t GREEN_OFFSET = 5;
+ static constexpr std::size_t BLUE_OFFSET = 7;
+ static constexpr std::size_t PROXIMITY_OFFSET = 9;
+
+ static constexpr double INTEGRATION_CYCLE_MILLISECONDS = 2.4;
+ static constexpr int MAX_INTEGRATION_CYCLES = 256;
+ static constexpr int COUNTS_PER_INTEGRATION_CYCLE = 1024;
+ static constexpr int MAX_RAW_COLOR_VALUE = 65535;
+ static constexpr int PROXIMITY_SATURATION = 1023;
+
+ /// The datasheet specifies a 2.4 ms warm-up delay after power on.
+ static constexpr int POWER_ON_DELAY_MILLISECONDS = 3;
+
+ static constexpr int ENABLE_SETTLE_DELAY_MILLISECONDS = 5;
+
+ static constexpr wpi::units::millisecond_t DEFAULT_INTEGRATION_TIME{24.0};
+ static constexpr int DEFAULT_PROXIMITY_PULSE_COUNT = 8;
+
+ static constexpr double DEFAULT_A_PARAM = 186.347;
+ static constexpr double DEFAULT_B_PARAM = 30403.5;
+ static constexpr double DEFAULT_C_PARAM = 0.576649;
+
+ static int ValidateAddress(int deviceAddress);
+ static int IntegrationTimeRegister(wpi::units::millisecond_t integrationTime);
+
+ bool Initialize();
+ void Reconfigure();
+ bool Configure();
+ bool WriteControl();
+ bool Enable();
+ bool Disable();
+ bool WriteEnable(int value);
+ std::vector ReadRegister(Register reg, int count);
+ bool WriteRegister(Register reg, int value);
+ void RecordFailure(FailureReason reason);
+ double Normalize(int rawValue) const;
+ int IntegrationCycles() const;
+
+ I2C m_i2c;
+
+ Gain m_gain = Gain::GAIN_4;
+ LedDrive m_ledDrive = LedDrive::PERCENT_50;
+ int m_integrationTimeRegister =
+ IntegrationTimeRegister(DEFAULT_INTEGRATION_TIME);
+ int m_proximityPulseCount = DEFAULT_PROXIMITY_PULSE_COUNT;
+ double m_softwareGain = 1.0;
+
+ double m_aParam = DEFAULT_A_PARAM;
+ double m_bParam = DEFAULT_B_PARAM;
+ double m_cParam = DEFAULT_C_PARAM;
+
+ bool m_initialized = false;
+ uint8_t m_deviceId = 0;
+ DeviceStatus m_deviceStatus = DeviceStatus::NOT_INITIALIZED;
+ std::optional m_lastFailureReason;
+ uint64_t m_failureCount = 0;
+
+ int m_rawClear = 0;
+ int m_rawRed = 0;
+ int m_rawGreen = 0;
+ int m_rawBlue = 0;
+ int m_rawProximity = 0;
+};
+
+} // namespace wpi
diff --git a/drivers/src/test/java/org/wpilib/drivers/range/RevColorSensorV2Test.java b/drivers/src/test/java/org/wpilib/drivers/range/RevColorSensorV2Test.java
new file mode 100644
index 00000000000..7ab5b8b7770
--- /dev/null
+++ b/drivers/src/test/java/org/wpilib/drivers/range/RevColorSensorV2Test.java
@@ -0,0 +1,495 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package org.wpilib.drivers.range;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNull;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.wpilib.units.Units.Meters;
+import static org.wpilib.units.Units.Milliseconds;
+import static org.wpilib.units.Units.Seconds;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.HashMap;
+import java.util.List;
+import java.util.Map;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.parallel.ResourceLock;
+import org.wpilib.drivers.range.RevColorSensorV2.DeviceStatus;
+import org.wpilib.drivers.range.RevColorSensorV2.FailureReason;
+import org.wpilib.drivers.range.RevColorSensorV2.Gain;
+import org.wpilib.drivers.range.RevColorSensorV2.LedDrive;
+import org.wpilib.drivers.range.RevColorSensorV2.Register;
+import org.wpilib.hardware.bus.I2C;
+import org.wpilib.hardware.hal.HAL;
+import org.wpilib.simulation.CallbackStore;
+import org.wpilib.simulation.I2CSim;
+
+@ResourceLock("I2C")
+class RevColorSensorV2Test {
+ private static final double DELTA = 1e-9;
+
+ /**
+ * Command bit plus the auto-increment command type. The sensor advances through consecutive
+ * registers during a multi-byte read only when the command type is auto-increment.
+ */
+ private static final int COMMAND_AUTO_INCREMENT = 0x80 | (0x01 << 5);
+
+ private static final byte TMD37821_DEVICE_ID = 0x60;
+
+ private static final int STATUS_COLOR_VALID = 0x01;
+ private static final int STATUS_PROXIMITY_VALID = 0x02;
+ private static final int STATUS_ALL_VALID = STATUS_COLOR_VALID | STATUS_PROXIMITY_VALID;
+
+ /** Default integration time of 24 ms, expressed as ten 2.4 ms cycles. */
+ private static final int DEFAULT_ATIME = 246;
+
+ private static final int DEFAULT_MAXIMUM_RAW_COLOR_VALUE = 10240;
+
+ private final I2CSim m_i2cSim = new I2CSim(I2C.Port.PORT_0.value);
+ private final Map m_registerData = new HashMap<>();
+ private final List m_writes = new ArrayList<>();
+ private final List m_readRegisters = new ArrayList<>();
+ private final List m_readCounts = new ArrayList<>();
+
+ private CallbackStore m_readCallback;
+ private CallbackStore m_writeCallback;
+ private int m_selectedRegister;
+
+ @BeforeEach
+ void setUp() {
+ HAL.initialize();
+ m_i2cSim.resetData();
+ m_readCallback =
+ m_i2cSim.registerReadCallback(
+ (name, buffer, count) -> {
+ m_readRegisters.add(m_selectedRegister);
+ m_readCounts.add(count);
+ byte[] data = m_registerData.get(m_selectedRegister);
+ if (data != null) {
+ System.arraycopy(data, 0, buffer, 0, Math.min(count, data.length));
+ }
+ });
+ m_writeCallback =
+ m_i2cSim.registerWriteCallback(
+ (name, buffer, count) -> {
+ if (count == 0) {
+ return;
+ }
+ m_selectedRegister = Byte.toUnsignedInt(buffer[0]);
+ if (count > 1) {
+ m_writes.add(Arrays.copyOf(buffer, count));
+ }
+ });
+ setRegister(Register.DEVICE_ID, new byte[] {TMD37821_DEVICE_ID});
+ }
+
+ @AfterEach
+ void tearDown() {
+ m_readCallback.close();
+ m_writeCallback.close();
+ m_i2cSim.resetData();
+ }
+
+ @Test
+ void usesDefaultAddressAndCloses() {
+ var sensor = new RevColorSensorV2(I2C.Port.PORT_0);
+
+ assertEquals(I2C.Port.PORT_0, sensor.getPort());
+ assertEquals(RevColorSensorV2.DEFAULT_ADDRESS, sensor.getDeviceAddress());
+
+ sensor.close();
+ assertThrows(IllegalStateException.class, sensor::getPort);
+ assertThrows(IllegalStateException.class, sensor::update);
+ assertThrows(IllegalStateException.class, sensor::getDeviceStatus);
+ assertThrows(IllegalStateException.class, sensor::getColor);
+ assertThrows(IllegalStateException.class, sensor::getRawProximity);
+ assertThrows(IllegalStateException.class, sensor::getDistanceMeters);
+ assertThrows(IllegalStateException.class, () -> sensor.setGain(Gain.GAIN_16));
+
+ sensor.close();
+ }
+
+ @Test
+ void rejectsInvalidI2cAddresses() {
+ assertThrows(IllegalArgumentException.class, () -> new RevColorSensorV2(I2C.Port.PORT_0, -1));
+ assertThrows(IllegalArgumentException.class, () -> new RevColorSensorV2(I2C.Port.PORT_0, 0x80));
+ }
+
+ @Test
+ void configuresSensorOnConstruction() {
+ setRegister(Register.CONTROL, new byte[] {0x00});
+
+ try (var sensor = new RevColorSensorV2(I2C.Port.PORT_0)) {
+ assertEquals(DeviceStatus.READY, sensor.getDeviceStatus());
+ assertEquals(TMD37821_DEVICE_ID, sensor.getDeviceId());
+ assertEquals(0, sensor.getFailureCount());
+ assertNull(sensor.getLastFailureReason());
+
+ // The integrator is turned off, the gain and timing are written, and the color and
+ // proximity channels are enabled after the power on warm-up.
+ assertEquals(6, m_writes.size());
+ assertWrite(m_writes.get(0), Register.ENABLE, 0x00);
+ assertWrite(m_writes.get(1), Register.ATIME, DEFAULT_ATIME);
+ assertWrite(m_writes.get(2), Register.CONTROL, 0x61);
+ assertWrite(m_writes.get(3), Register.PPULSE, 8);
+ assertWrite(m_writes.get(4), Register.ENABLE, 0x01);
+ assertWrite(m_writes.get(5), Register.ENABLE, 0x07);
+ }
+ }
+
+ @Test
+ void overwritesRetainedControlFields() {
+ // A retained proximity diode bit and proximity gain, as the sensor holds them across a
+ // program restart. Disabling the sensor does not reset this register, and preserving these
+ // bits would select a reserved diode and proximity gain instead of the calibrated ones.
+ setRegister(Register.CONTROL, new byte[] {0x1C});
+
+ try (var sensor = new RevColorSensorV2(I2C.Port.PORT_0)) {
+ // Bits 7:6 LED drive (50%), 5:4 IR diode, 3:2 proximity gain 1x, 1:0 color gain (4x).
+ assertWrite(lastWriteTo(Register.CONTROL), Register.CONTROL, 0x61);
+
+ sensor.setGain(Gain.GAIN_60);
+ sensor.setLedDrive(LedDrive.PERCENT_12_5);
+
+ // The diode and proximity gain fields stay at their calibrated values.
+ assertWrite(lastWriteTo(Register.CONTROL), Register.CONTROL, 0xE3);
+ }
+ }
+
+ @Test
+ void rejectsUnexpectedDeviceId() {
+ setRegister(Register.DEVICE_ID, new byte[] {0x44});
+
+ try (var sensor = new RevColorSensorV2(I2C.Port.PORT_0)) {
+ assertEquals(DeviceStatus.FAULT_UNEXPECTED_DEVICE_ID, sensor.getDeviceStatus());
+ assertEquals(FailureReason.UNEXPECTED_DEVICE_ID, sensor.getLastFailureReason());
+ assertEquals(0x44, sensor.getDeviceId());
+ assertEquals(0, m_writes.size());
+ }
+ }
+
+ @Test
+ void retriesConfigurationOnUpdate() {
+ setRegister(Register.DEVICE_ID, new byte[] {0x00});
+
+ try (var sensor = new RevColorSensorV2(I2C.Port.PORT_0)) {
+ assertEquals(DeviceStatus.FAULT_UNEXPECTED_DEVICE_ID, sensor.getDeviceStatus());
+
+ setRegister(Register.DEVICE_ID, new byte[] {TMD37821_DEVICE_ID});
+ setRegister(Register.STATUS, statusBlock(STATUS_ALL_VALID, 1, 2, 3, 4, 5));
+ sensor.update();
+
+ assertEquals(DeviceStatus.READY, sensor.getDeviceStatus());
+ assertEquals(1, sensor.getRawClear());
+ assertEquals(5, sensor.getRawProximity());
+ }
+ }
+
+ @Test
+ void decodesColorAndProximity() {
+ setRegister(Register.STATUS, statusBlock(STATUS_ALL_VALID, 10240, 5120, 2560, 1024, 512));
+
+ try (var sensor = new RevColorSensorV2(I2C.Port.PORT_0)) {
+ sensor.update();
+
+ assertEquals(DeviceStatus.READY, sensor.getDeviceStatus());
+ assertEquals(DEFAULT_MAXIMUM_RAW_COLOR_VALUE, sensor.getMaximumRawColorValue());
+
+ assertEquals(10240, sensor.getRawClear());
+ assertEquals(5120, sensor.getRawRed());
+ assertEquals(2560, sensor.getRawGreen());
+ assertEquals(1024, sensor.getRawBlue());
+ assertEquals(512, sensor.getRawProximity());
+
+ assertEquals(1.0, sensor.getClear(), DELTA);
+ assertEquals(0.5, sensor.getRed(), DELTA);
+ assertEquals(0.25, sensor.getGreen(), DELTA);
+ assertEquals(0.1, sensor.getBlue(), DELTA);
+
+ // Color rounds to 12 bits of precision.
+ assertEquals(0.5, sensor.getColor().red, 1e-3);
+ assertEquals(0.25, sensor.getColor().green, 1e-3);
+ assertEquals(0.1, sensor.getColor().blue, 1e-3);
+
+ assertEquals(512.0 / 1023.0, sensor.getProximity(), DELTA);
+ }
+ }
+
+ @Test
+ void addressesTheBulkReadWithTheAutoIncrementCommandType() {
+ setRegister(Register.STATUS, statusBlock(STATUS_ALL_VALID, 1, 2, 3, 4, 5));
+
+ try (var sensor = new RevColorSensorV2(I2C.Port.PORT_0)) {
+ m_readRegisters.clear();
+ m_readCounts.clear();
+ sensor.update();
+
+ // Without the auto-increment command type the sensor would return the STATUS register
+ // once per byte instead of advancing through the color and proximity registers.
+ assertEquals(1, m_readRegisters.size());
+ assertEquals(0xB3, m_readRegisters.get(0));
+ assertEquals(Register.STATUS.getAddress() | 0x80 | 0x20, m_readRegisters.get(0));
+ assertEquals(11, m_readCounts.get(0));
+ }
+ }
+
+ @Test
+ void clampsNormalizedChannelsWithSoftwareGain() {
+ setRegister(Register.STATUS, statusBlock(STATUS_ALL_VALID, 0, 4096, 1024, 0, 2000));
+
+ try (var sensor = new RevColorSensorV2(I2C.Port.PORT_0)) {
+ sensor.setSoftwareGain(4.0);
+ sensor.update();
+
+ assertEquals(4.0, sensor.getSoftwareGain(), DELTA);
+ assertEquals(1.0, sensor.getRed(), DELTA);
+ assertEquals(0.4, sensor.getGreen(), DELTA);
+ assertEquals(4096, sensor.getRawRed());
+
+ // The proximity channel saturates at its raw maximum.
+ assertEquals(1.0, sensor.getProximity(), DELTA);
+ }
+ }
+
+ @Test
+ void convertsProximityToDistance() {
+ // A raw reading of a + b * (cm + c)^-2 must convert back to cm.
+ int rawOptical = (int) Math.round(186.347 + 30403.5 * Math.pow(5.0 + 0.576649, -2.0));
+ setRegister(Register.STATUS, statusBlock(STATUS_ALL_VALID, 0, 0, 0, 0, rawOptical));
+
+ try (var sensor = new RevColorSensorV2(I2C.Port.PORT_0)) {
+ sensor.update();
+
+ assertEquals(0.05, sensor.getDistanceMeters(), 1e-3);
+ assertEquals(0.05, sensor.getDistance().in(Meters), 1e-3);
+ }
+ }
+
+ @Test
+ void reportsOutOfRangeDistanceAsNaN() {
+ setRegister(Register.STATUS, statusBlock(STATUS_ALL_VALID, 0, 0, 0, 0, 100));
+
+ try (var sensor = new RevColorSensorV2(I2C.Port.PORT_0)) {
+ sensor.update();
+
+ assertTrue(Double.isNaN(sensor.getDistanceMeters()));
+ assertTrue(Double.isNaN(sensor.getDistance().magnitude()));
+ }
+ }
+
+ @Test
+ void appliesCustomDistanceCalibration() {
+ setRegister(Register.STATUS, statusBlock(STATUS_ALL_VALID, 0, 0, 0, 0, 300));
+
+ try (var sensor = new RevColorSensorV2(I2C.Port.PORT_0)) {
+ sensor.update();
+ double defaultDistance = sensor.getDistanceMeters();
+
+ sensor.setDistanceCalibration(186.347, 2 * 30403.5, 0.576649);
+
+ // Doubling the b parameter means the same reading is produced by a more distant target.
+ assertTrue(sensor.getDistanceMeters() > defaultDistance);
+ }
+ }
+
+ @Test
+ void preservesMeasurementsWhenDataIsNotValid() {
+ setRegister(Register.STATUS, statusBlock(STATUS_ALL_VALID, 100, 200, 300, 400, 500));
+
+ try (var sensor = new RevColorSensorV2(I2C.Port.PORT_0)) {
+ sensor.update();
+
+ setRegister(Register.STATUS, statusBlock(0x00, 1, 2, 3, 4, 5));
+ sensor.update();
+
+ assertEquals(DeviceStatus.FAULT_BAD_READ, sensor.getDeviceStatus());
+ assertEquals(FailureReason.PROXIMITY_DATA_NOT_VALID, sensor.getLastFailureReason());
+ assertEquals(2, sensor.getFailureCount());
+ assertEquals(100, sensor.getRawClear());
+ assertEquals(500, sensor.getRawProximity());
+
+ setRegister(Register.STATUS, statusBlock(STATUS_ALL_VALID, 7, 8, 9, 10, 11));
+ sensor.update();
+
+ assertEquals(DeviceStatus.READY, sensor.getDeviceStatus());
+ assertEquals(7, sensor.getRawClear());
+ assertEquals(11, sensor.getRawProximity());
+ }
+ }
+
+ @Test
+ void updatesProximityWhenOnlyColorIsNotValid() {
+ setRegister(Register.STATUS, statusBlock(STATUS_ALL_VALID, 100, 200, 300, 400, 500));
+
+ try (var sensor = new RevColorSensorV2(I2C.Port.PORT_0)) {
+ sensor.update();
+
+ setRegister(Register.STATUS, statusBlock(STATUS_PROXIMITY_VALID, 1, 2, 3, 4, 900));
+ sensor.update();
+
+ assertEquals(FailureReason.COLOR_DATA_NOT_VALID, sensor.getLastFailureReason());
+ assertEquals(1, sensor.getFailureCount());
+ assertEquals(100, sensor.getRawClear());
+ assertEquals(900, sensor.getRawProximity());
+ }
+ }
+
+ @Test
+ void reportsAbortedTransactions() {
+ var i2c = new FailingI2C();
+ i2c.m_failReads = true;
+
+ try (var sensor = new RevColorSensorV2(i2c)) {
+ assertEquals(DeviceStatus.FAULT_BAD_READ, sensor.getDeviceStatus());
+ assertEquals(FailureReason.I2C_READ_ABORTED, sensor.getLastFailureReason());
+ assertEquals(1, sensor.getFailureCount());
+ assertEquals(0, m_writes.size());
+
+ i2c.m_failReads = false;
+ i2c.m_failWrites = true;
+ sensor.update();
+
+ assertEquals(FailureReason.I2C_WRITE_ABORTED, sensor.getLastFailureReason());
+ assertEquals(2, sensor.getFailureCount());
+
+ i2c.m_failWrites = false;
+ setRegister(Register.STATUS, statusBlock(STATUS_ALL_VALID, 1, 2, 3, 4, 5));
+ sensor.update();
+
+ assertEquals(DeviceStatus.READY, sensor.getDeviceStatus());
+ assertEquals(2, sensor.getRawRed());
+ }
+ }
+
+ @Test
+ void roundsIntegrationTimeUpToWholeCycles() {
+ try (var sensor = new RevColorSensorV2(I2C.Port.PORT_0)) {
+ sensor.setIntegrationTime(Milliseconds.of(100));
+ assertEquals(100.8, sensor.getIntegrationTime().in(Milliseconds), DELTA);
+ assertEquals(42 * 1024, sensor.getMaximumRawColorValue());
+ assertWrite(lastWriteTo(Register.ATIME), Register.ATIME, 256 - 42);
+
+ // The color channels saturate at 16 bits well before the longest integration time.
+ sensor.setIntegrationTime(Seconds.of(1));
+ assertEquals(614.4, sensor.getIntegrationTime().in(Milliseconds), DELTA);
+ assertEquals(65535, sensor.getMaximumRawColorValue());
+ assertWrite(lastWriteTo(Register.ATIME), Register.ATIME, 0);
+
+ sensor.setIntegrationTime(Milliseconds.of(1));
+ assertEquals(2.4, sensor.getIntegrationTime().in(Milliseconds), DELTA);
+ assertEquals(1024, sensor.getMaximumRawColorValue());
+ assertWrite(lastWriteTo(Register.ATIME), Register.ATIME, 255);
+ }
+ }
+
+ @Test
+ void rejectsInvalidConfigurationValues() {
+ try (var sensor = new RevColorSensorV2(I2C.Port.PORT_0)) {
+ assertThrows(NullPointerException.class, () -> sensor.setGain(null));
+ assertThrows(NullPointerException.class, () -> sensor.setLedDrive(null));
+ assertThrows(NullPointerException.class, () -> sensor.setIntegrationTime(null));
+ assertThrows(
+ IllegalArgumentException.class, () -> sensor.setIntegrationTime(Milliseconds.of(0)));
+ assertThrows(
+ IllegalArgumentException.class, () -> sensor.setIntegrationTime(Milliseconds.of(-1)));
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> sensor.setIntegrationTime(Milliseconds.of(Double.NaN)));
+ assertThrows(IllegalArgumentException.class, () -> sensor.setProximityPulseCount(0));
+ assertThrows(IllegalArgumentException.class, () -> sensor.setProximityPulseCount(256));
+ assertThrows(IllegalArgumentException.class, () -> sensor.setSoftwareGain(0.0));
+ assertThrows(IllegalArgumentException.class, () -> sensor.setSoftwareGain(Double.NaN));
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> sensor.setDistanceCalibration(Double.POSITIVE_INFINITY, 1.0, 1.0));
+
+ // The sensor keeps its defaults after every rejected value.
+ assertEquals(Gain.GAIN_4, sensor.getGain());
+ assertEquals(LedDrive.PERCENT_50, sensor.getLedDrive());
+ assertEquals(24.0, sensor.getIntegrationTime().in(Milliseconds), DELTA);
+ assertEquals(8, sensor.getProximityPulseCount());
+ assertEquals(1.0, sensor.getSoftwareGain(), DELTA);
+ }
+ }
+
+ @Test
+ void storesConfigurationWrittenBeforeInitializationSucceeds() {
+ setRegister(Register.DEVICE_ID, new byte[] {0x00});
+
+ try (var sensor = new RevColorSensorV2(I2C.Port.PORT_0)) {
+ sensor.setGain(Gain.GAIN_16);
+ sensor.setProximityPulseCount(32);
+ assertEquals(0, m_writes.size());
+
+ setRegister(Register.DEVICE_ID, new byte[] {TMD37821_DEVICE_ID});
+ setRegister(Register.STATUS, statusBlock(STATUS_ALL_VALID, 0, 0, 0, 0, 0));
+ sensor.update();
+
+ assertEquals(DeviceStatus.READY, sensor.getDeviceStatus());
+ assertWrite(lastWriteTo(Register.CONTROL), Register.CONTROL, 0x62);
+ assertWrite(lastWriteTo(Register.PPULSE), Register.PPULSE, 32);
+ }
+ }
+
+ private void setRegister(Register register, byte[] data) {
+ m_registerData.put(register.getAddress() | COMMAND_AUTO_INCREMENT, data);
+ }
+
+ private byte[] lastWriteTo(Register register) {
+ for (int i = m_writes.size() - 1; i >= 0; i--) {
+ if (Byte.toUnsignedInt(m_writes.get(i)[0])
+ == (register.getAddress() | COMMAND_AUTO_INCREMENT)) {
+ return m_writes.get(i);
+ }
+ }
+ throw new AssertionError("No write to " + register);
+ }
+
+ private static byte[] statusBlock(
+ int status, int clear, int red, int green, int blue, int proximity) {
+ byte[] data = new byte[11];
+ data[0] = (byte) status;
+ putUnsignedShort(data, 1, clear);
+ putUnsignedShort(data, 3, red);
+ putUnsignedShort(data, 5, green);
+ putUnsignedShort(data, 7, blue);
+ putUnsignedShort(data, 9, proximity);
+ return data;
+ }
+
+ private static void putUnsignedShort(byte[] data, int offset, int value) {
+ data[offset] = (byte) value;
+ data[offset + 1] = (byte) (value >> 8);
+ }
+
+ private static void assertWrite(byte[] write, Register register, int expected) {
+ assertEquals(register.getAddress() | COMMAND_AUTO_INCREMENT, Byte.toUnsignedInt(write[0]));
+ assertEquals(expected, Byte.toUnsignedInt(write[1]));
+ }
+
+ private static final class FailingI2C extends I2C {
+ private boolean m_failReads;
+ private boolean m_failWrites;
+
+ FailingI2C() {
+ super(I2C.Port.PORT_0, RevColorSensorV2.DEFAULT_ADDRESS);
+ }
+
+ @Override
+ public boolean read(int registerAddress, int count, byte[] buffer) {
+ return m_failReads || super.read(registerAddress, count, buffer);
+ }
+
+ @Override
+ public synchronized boolean write(int registerAddress, int data) {
+ return m_failWrites || super.write(registerAddress, data);
+ }
+ }
+}
diff --git a/drivers/src/test/native/cpp/range/RevColorSensorV2Test.cpp b/drivers/src/test/native/cpp/range/RevColorSensorV2Test.cpp
new file mode 100644
index 00000000000..a525d367bf8
--- /dev/null
+++ b/drivers/src/test/native/cpp/range/RevColorSensorV2Test.cpp
@@ -0,0 +1,480 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "wpi/drivers/range/RevColorSensorV2.hpp"
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+
+#include "wpi/hal/simulation/I2CData.h"
+
+namespace {
+
+using DeviceStatus = wpi::RevColorSensorV2::DeviceStatus;
+using FailureReason = wpi::RevColorSensorV2::FailureReason;
+using Gain = wpi::RevColorSensorV2::Gain;
+using LedDrive = wpi::RevColorSensorV2::LedDrive;
+using Register = wpi::RevColorSensorV2::Register;
+
+/// Command bit plus the auto-increment command type. The sensor advances
+/// through consecutive registers during a multi-byte read only when the command
+/// type is auto-increment.
+constexpr int COMMAND_AUTO_INCREMENT = 0x80 | (0x01 << 5);
+
+constexpr uint8_t TMD37821_DEVICE_ID = 0x60;
+
+constexpr int STATUS_COLOR_VALID = 0x01;
+constexpr int STATUS_PROXIMITY_VALID = 0x02;
+constexpr int STATUS_ALL_VALID = STATUS_COLOR_VALID | STATUS_PROXIMITY_VALID;
+
+/// Default integration time of 24 ms, expressed as ten 2.4 ms cycles.
+constexpr int DEFAULT_ATIME = 246;
+
+constexpr int DEFAULT_MAXIMUM_RAW_COLOR_VALUE = 10240;
+
+void PutUnsignedShort(std::vector& data, std::size_t offset,
+ int value) {
+ data[offset] = static_cast(value);
+ data[offset + 1] = static_cast(value >> 8);
+}
+
+std::vector StatusBlock(int status, int clear, int red, int green,
+ int blue, int proximity) {
+ std::vector data(11, 0);
+ data[0] = static_cast(status);
+ PutUnsignedShort(data, 1, clear);
+ PutUnsignedShort(data, 3, red);
+ PutUnsignedShort(data, 5, green);
+ PutUnsignedShort(data, 7, blue);
+ PutUnsignedShort(data, 9, proximity);
+ return data;
+}
+
+class ColorSensorTestFixture {
+ public:
+ ColorSensorTestFixture() {
+ HALSIM_ResetI2CData(0);
+ m_readUid = HALSIM_RegisterI2CReadCallback(0, ReadCallback, this);
+ m_writeUid = HALSIM_RegisterI2CWriteCallback(0, WriteCallback, this);
+ SetRegister(Register::DEVICE_ID, {TMD37821_DEVICE_ID});
+ }
+
+ ~ColorSensorTestFixture() {
+ HALSIM_CancelI2CReadCallback(0, m_readUid);
+ HALSIM_CancelI2CWriteCallback(0, m_writeUid);
+ HALSIM_ResetI2CData(0);
+ }
+
+ void SetRegister(Register reg, std::vector data) {
+ m_registerData[static_cast(reg) | COMMAND_AUTO_INCREMENT] =
+ std::move(data);
+ }
+
+ /// Returns the most recent two-byte write to the given register.
+ std::vector LastWriteTo(Register reg) const {
+ auto address =
+ static_cast(static_cast(reg) | COMMAND_AUTO_INCREMENT);
+ for (auto it = m_writes.rbegin(); it != m_writes.rend(); ++it) {
+ if (it->size() > 1 && (*it)[0] == address) {
+ return *it;
+ }
+ }
+ FAIL("No write to the requested register");
+ return {};
+ }
+
+ std::unordered_map> m_registerData;
+ std::vector> m_writes;
+ std::vector m_readRegisters;
+ std::vector m_readCounts;
+ int m_selectedRegister = 0;
+
+ private:
+ static void ReadCallback(const char*, void* param, unsigned char* buffer,
+ unsigned int count) {
+ auto& self = *static_cast(param);
+ self.m_readRegisters.push_back(self.m_selectedRegister);
+ self.m_readCounts.push_back(count);
+ std::fill_n(buffer, count, 0);
+ auto it = self.m_registerData.find(self.m_selectedRegister);
+ if (it != self.m_registerData.end()) {
+ std::copy_n(it->second.begin(),
+ std::min(count, it->second.size()), buffer);
+ }
+ }
+
+ static void WriteCallback(const char*, void* param,
+ const unsigned char* buffer, unsigned int count) {
+ auto& self = *static_cast(param);
+ if (count == 0) {
+ return;
+ }
+ self.m_selectedRegister = buffer[0];
+ if (count > 1) {
+ self.m_writes.emplace_back(buffer, buffer + count);
+ }
+ }
+
+ int32_t m_readUid;
+ int32_t m_writeUid;
+};
+
+std::vector RegisterWrite(Register reg, int value) {
+ return {static_cast(static_cast(reg) | COMMAND_AUTO_INCREMENT),
+ static_cast(value)};
+}
+
+TEST_CASE_METHOD(ColorSensorTestFixture,
+ "RevColorSensorV2 configures the sensor on construction",
+ "[drivers][rev-color-sensor-v2]") {
+ SetRegister(Register::CONTROL, {0x00});
+
+ wpi::RevColorSensorV2 sensor{wpi::I2C::Port::PORT_0};
+
+ CHECK(sensor.GetPort() == wpi::I2C::Port::PORT_0);
+ CHECK(sensor.GetDeviceAddress() == wpi::RevColorSensorV2::DEFAULT_ADDRESS);
+ CHECK(sensor.GetDeviceStatus() == DeviceStatus::READY);
+ CHECK(sensor.GetDeviceId() == TMD37821_DEVICE_ID);
+ CHECK(sensor.GetFailureCount() == 0);
+ CHECK_FALSE(sensor.GetLastFailureReason().has_value());
+
+ // The integrator is turned off, the gain and timing are written, and the
+ // color and proximity channels are enabled after the power on warm-up.
+ REQUIRE(m_writes.size() == 6);
+ CHECK(m_writes[0] == RegisterWrite(Register::ENABLE, 0x00));
+ CHECK(m_writes[1] == RegisterWrite(Register::ATIME, DEFAULT_ATIME));
+ CHECK(m_writes[2] == RegisterWrite(Register::CONTROL, 0x61));
+ CHECK(m_writes[3] == RegisterWrite(Register::PPULSE, 8));
+ CHECK(m_writes[4] == RegisterWrite(Register::ENABLE, 0x01));
+ CHECK(m_writes[5] == RegisterWrite(Register::ENABLE, 0x07));
+}
+
+TEST_CASE_METHOD(ColorSensorTestFixture,
+ "RevColorSensorV2 overwrites retained control fields",
+ "[drivers][rev-color-sensor-v2]") {
+ // A retained proximity diode bit and proximity gain, as the sensor holds them
+ // across a program restart. Disabling the sensor does not reset this
+ // register, and preserving these bits would select a reserved diode and
+ // proximity gain instead of the calibrated ones.
+ SetRegister(Register::CONTROL, {0x1C});
+
+ wpi::RevColorSensorV2 sensor{wpi::I2C::Port::PORT_0};
+
+ // Bits 7:6 LED drive (50%), 5:4 IR diode, 3:2 proximity gain 1x, 1:0 color
+ // gain (4x).
+ CHECK(LastWriteTo(Register::CONTROL) ==
+ RegisterWrite(Register::CONTROL, 0x61));
+
+ sensor.SetGain(Gain::GAIN_60);
+ sensor.SetLedDrive(LedDrive::PERCENT_12_5);
+
+ // The diode and proximity gain fields stay at their calibrated values.
+ CHECK(LastWriteTo(Register::CONTROL) ==
+ RegisterWrite(Register::CONTROL, 0xE3));
+ CHECK(sensor.GetGain() == Gain::GAIN_60);
+ CHECK(sensor.GetLedDrive() == LedDrive::PERCENT_12_5);
+}
+
+TEST_CASE_METHOD(ColorSensorTestFixture,
+ "RevColorSensorV2 rejects an unexpected device ID",
+ "[drivers][rev-color-sensor-v2]") {
+ SetRegister(Register::DEVICE_ID, {0x44});
+
+ wpi::RevColorSensorV2 sensor{wpi::I2C::Port::PORT_0};
+
+ CHECK(sensor.GetDeviceStatus() == DeviceStatus::FAULT_UNEXPECTED_DEVICE_ID);
+ REQUIRE(sensor.GetLastFailureReason().has_value());
+ CHECK(*sensor.GetLastFailureReason() == FailureReason::UNEXPECTED_DEVICE_ID);
+ CHECK(sensor.GetDeviceId() == 0x44);
+ CHECK(m_writes.empty());
+}
+
+TEST_CASE_METHOD(ColorSensorTestFixture,
+ "RevColorSensorV2 retries configuration on update",
+ "[drivers][rev-color-sensor-v2]") {
+ SetRegister(Register::DEVICE_ID, {0x00});
+
+ wpi::RevColorSensorV2 sensor{wpi::I2C::Port::PORT_0};
+ CHECK(sensor.GetDeviceStatus() == DeviceStatus::FAULT_UNEXPECTED_DEVICE_ID);
+
+ SetRegister(Register::DEVICE_ID, {TMD37821_DEVICE_ID});
+ SetRegister(Register::STATUS, StatusBlock(STATUS_ALL_VALID, 1, 2, 3, 4, 5));
+ sensor.Update();
+
+ CHECK(sensor.GetDeviceStatus() == DeviceStatus::READY);
+ CHECK(sensor.GetRawClear() == 1);
+ CHECK(sensor.GetRawProximity() == 5);
+}
+
+TEST_CASE_METHOD(ColorSensorTestFixture,
+ "RevColorSensorV2 decodes color and proximity",
+ "[drivers][rev-color-sensor-v2]") {
+ SetRegister(Register::STATUS,
+ StatusBlock(STATUS_ALL_VALID, 10240, 5120, 2560, 1024, 512));
+
+ wpi::RevColorSensorV2 sensor{wpi::I2C::Port::PORT_0};
+ sensor.Update();
+
+ CHECK(sensor.GetDeviceStatus() == DeviceStatus::READY);
+ CHECK(sensor.GetMaximumRawColorValue() == DEFAULT_MAXIMUM_RAW_COLOR_VALUE);
+
+ CHECK(sensor.GetRawClear() == 10240);
+ CHECK(sensor.GetRawRed() == 5120);
+ CHECK(sensor.GetRawGreen() == 2560);
+ CHECK(sensor.GetRawBlue() == 1024);
+ CHECK(sensor.GetRawProximity() == 512);
+
+ CHECK(sensor.GetClear() == Catch::Approx(1.0));
+ CHECK(sensor.GetRed() == Catch::Approx(0.5));
+ CHECK(sensor.GetGreen() == Catch::Approx(0.25));
+ CHECK(sensor.GetBlue() == Catch::Approx(0.1));
+
+ // Color rounds to 12 bits of precision.
+ CHECK(sensor.GetColor().red == Catch::Approx(0.5).margin(1e-3));
+ CHECK(sensor.GetColor().green == Catch::Approx(0.25).margin(1e-3));
+ CHECK(sensor.GetColor().blue == Catch::Approx(0.1).margin(1e-3));
+
+ CHECK(sensor.GetProximity() == Catch::Approx(512.0 / 1023.0));
+}
+
+TEST_CASE_METHOD(
+ ColorSensorTestFixture,
+ "RevColorSensorV2 addresses the bulk read with the auto-increment type",
+ "[drivers][rev-color-sensor-v2]") {
+ SetRegister(Register::STATUS, StatusBlock(STATUS_ALL_VALID, 1, 2, 3, 4, 5));
+
+ wpi::RevColorSensorV2 sensor{wpi::I2C::Port::PORT_0};
+ m_readRegisters.clear();
+ m_readCounts.clear();
+ sensor.Update();
+
+ // Without the auto-increment command type the sensor would return the STATUS
+ // register once per byte instead of advancing through the color and
+ // proximity registers.
+ REQUIRE(m_readRegisters.size() == 1);
+ CHECK(m_readRegisters[0] == 0xB3);
+ CHECK(m_readRegisters[0] ==
+ (static_cast(Register::STATUS) | 0x80 | 0x20));
+ CHECK(m_readCounts[0] == 11);
+}
+
+TEST_CASE_METHOD(ColorSensorTestFixture,
+ "RevColorSensorV2 clamps normalized channels",
+ "[drivers][rev-color-sensor-v2]") {
+ SetRegister(Register::STATUS,
+ StatusBlock(STATUS_ALL_VALID, 0, 4096, 1024, 0, 2000));
+
+ wpi::RevColorSensorV2 sensor{wpi::I2C::Port::PORT_0};
+ sensor.SetSoftwareGain(4.0);
+ sensor.Update();
+
+ CHECK(sensor.GetSoftwareGain() == Catch::Approx(4.0));
+ CHECK(sensor.GetRed() == Catch::Approx(1.0));
+ CHECK(sensor.GetGreen() == Catch::Approx(0.4));
+ CHECK(sensor.GetRawRed() == 4096);
+
+ // The proximity channel saturates at its raw maximum.
+ CHECK(sensor.GetProximity() == Catch::Approx(1.0));
+}
+
+TEST_CASE_METHOD(ColorSensorTestFixture,
+ "RevColorSensorV2 converts proximity to distance",
+ "[drivers][rev-color-sensor-v2]") {
+ // A raw reading of a + b * (cm + c)^-2 must convert back to cm.
+ int rawOptical = static_cast(
+ std::lround(186.347 + 30403.5 * std::pow(5.0 + 0.576649, -2.0)));
+ SetRegister(Register::STATUS,
+ StatusBlock(STATUS_ALL_VALID, 0, 0, 0, 0, rawOptical));
+
+ wpi::RevColorSensorV2 sensor{wpi::I2C::Port::PORT_0};
+ sensor.Update();
+
+ CHECK(sensor.GetDistance().value() == Catch::Approx(0.05).margin(1e-3));
+}
+
+TEST_CASE_METHOD(ColorSensorTestFixture,
+ "RevColorSensorV2 reports an out of range distance as NaN",
+ "[drivers][rev-color-sensor-v2]") {
+ SetRegister(Register::STATUS, StatusBlock(STATUS_ALL_VALID, 0, 0, 0, 0, 100));
+
+ wpi::RevColorSensorV2 sensor{wpi::I2C::Port::PORT_0};
+ sensor.Update();
+
+ CHECK(std::isnan(sensor.GetDistance().value()));
+}
+
+TEST_CASE_METHOD(ColorSensorTestFixture,
+ "RevColorSensorV2 applies a custom distance calibration",
+ "[drivers][rev-color-sensor-v2]") {
+ SetRegister(Register::STATUS, StatusBlock(STATUS_ALL_VALID, 0, 0, 0, 0, 300));
+
+ wpi::RevColorSensorV2 sensor{wpi::I2C::Port::PORT_0};
+ sensor.Update();
+ auto defaultDistance = sensor.GetDistance();
+
+ sensor.SetDistanceCalibration(186.347, 2 * 30403.5, 0.576649);
+
+ // Doubling the b parameter means the same reading is produced by a more
+ // distant target.
+ CHECK(sensor.GetDistance() > defaultDistance);
+}
+
+TEST_CASE_METHOD(ColorSensorTestFixture,
+ "RevColorSensorV2 preserves measurements when data is invalid",
+ "[drivers][rev-color-sensor-v2]") {
+ SetRegister(Register::STATUS,
+ StatusBlock(STATUS_ALL_VALID, 100, 200, 300, 400, 500));
+
+ wpi::RevColorSensorV2 sensor{wpi::I2C::Port::PORT_0};
+ sensor.Update();
+
+ SetRegister(Register::STATUS, StatusBlock(0x00, 1, 2, 3, 4, 5));
+ sensor.Update();
+
+ CHECK(sensor.GetDeviceStatus() == DeviceStatus::FAULT_BAD_READ);
+ REQUIRE(sensor.GetLastFailureReason().has_value());
+ CHECK(*sensor.GetLastFailureReason() ==
+ FailureReason::PROXIMITY_DATA_NOT_VALID);
+ CHECK(sensor.GetFailureCount() == 2);
+ CHECK(sensor.GetRawClear() == 100);
+ CHECK(sensor.GetRawProximity() == 500);
+
+ SetRegister(Register::STATUS, StatusBlock(STATUS_ALL_VALID, 7, 8, 9, 10, 11));
+ sensor.Update();
+
+ CHECK(sensor.GetDeviceStatus() == DeviceStatus::READY);
+ CHECK(sensor.GetRawClear() == 7);
+ CHECK(sensor.GetRawProximity() == 11);
+}
+
+TEST_CASE_METHOD(
+ ColorSensorTestFixture,
+ "RevColorSensorV2 updates proximity when only color is invalid",
+ "[drivers][rev-color-sensor-v2]") {
+ SetRegister(Register::STATUS,
+ StatusBlock(STATUS_ALL_VALID, 100, 200, 300, 400, 500));
+
+ wpi::RevColorSensorV2 sensor{wpi::I2C::Port::PORT_0};
+ sensor.Update();
+
+ SetRegister(Register::STATUS,
+ StatusBlock(STATUS_PROXIMITY_VALID, 1, 2, 3, 4, 900));
+ sensor.Update();
+
+ REQUIRE(sensor.GetLastFailureReason().has_value());
+ CHECK(*sensor.GetLastFailureReason() == FailureReason::COLOR_DATA_NOT_VALID);
+ CHECK(sensor.GetFailureCount() == 1);
+ CHECK(sensor.GetRawClear() == 100);
+ CHECK(sensor.GetRawProximity() == 900);
+}
+
+TEST_CASE_METHOD(ColorSensorTestFixture,
+ "RevColorSensorV2 rounds integration time up to whole cycles",
+ "[drivers][rev-color-sensor-v2]") {
+ wpi::RevColorSensorV2 sensor{wpi::I2C::Port::PORT_0};
+
+ sensor.SetIntegrationTime(wpi::units::millisecond_t{100});
+ CHECK(sensor.GetIntegrationTime().value() == Catch::Approx(100.8));
+ CHECK(sensor.GetMaximumRawColorValue() == 42 * 1024);
+ CHECK(LastWriteTo(Register::ATIME) ==
+ RegisterWrite(Register::ATIME, 256 - 42));
+
+ // The color channels saturate at 16 bits well before the longest
+ // integration time.
+ sensor.SetIntegrationTime(wpi::units::second_t{1});
+ CHECK(sensor.GetIntegrationTime().value() == Catch::Approx(614.4));
+ CHECK(sensor.GetMaximumRawColorValue() == 65535);
+ CHECK(LastWriteTo(Register::ATIME) == RegisterWrite(Register::ATIME, 0));
+
+ sensor.SetIntegrationTime(wpi::units::millisecond_t{1});
+ CHECK(sensor.GetIntegrationTime().value() == Catch::Approx(2.4));
+ CHECK(sensor.GetMaximumRawColorValue() == 1024);
+ CHECK(LastWriteTo(Register::ATIME) == RegisterWrite(Register::ATIME, 255));
+}
+
+TEST_CASE_METHOD(
+ ColorSensorTestFixture,
+ "RevColorSensorV2 saturates integration times beyond the sensor range",
+ "[drivers][rev-color-sensor-v2]") {
+ wpi::RevColorSensorV2 sensor{wpi::I2C::Port::PORT_0};
+
+ // A duration whose cycle count cannot be represented as an int still has to
+ // saturate at the documented maximum. Converting such a value to int before
+ // clamping would be undefined behavior.
+ for (double milliseconds : {1e6, 1e18, std::numeric_limits::max()}) {
+ sensor.SetIntegrationTime(wpi::units::millisecond_t{milliseconds});
+
+ CHECK(sensor.GetIntegrationTime().value() == Catch::Approx(614.4));
+ CHECK(sensor.GetMaximumRawColorValue() == 65535);
+ CHECK(LastWriteTo(Register::ATIME) == RegisterWrite(Register::ATIME, 0));
+ }
+}
+
+TEST_CASE_METHOD(ColorSensorTestFixture,
+ "RevColorSensorV2 rejects invalid configuration values",
+ "[drivers][rev-color-sensor-v2]") {
+ wpi::RevColorSensorV2 sensor{wpi::I2C::Port::PORT_0};
+
+ CHECK_THROWS_AS(sensor.SetIntegrationTime(wpi::units::millisecond_t{0}),
+ std::invalid_argument);
+ CHECK_THROWS_AS(sensor.SetIntegrationTime(wpi::units::millisecond_t{-1}),
+ std::invalid_argument);
+ CHECK_THROWS_AS(sensor.SetProximityPulseCount(0), std::invalid_argument);
+ CHECK_THROWS_AS(sensor.SetProximityPulseCount(256), std::invalid_argument);
+ CHECK_THROWS_AS(sensor.SetSoftwareGain(0.0), std::invalid_argument);
+ CHECK_THROWS_AS(sensor.SetGain(static_cast(9)), std::invalid_argument);
+ CHECK_THROWS_AS(sensor.SetLedDrive(static_cast(9)),
+ std::invalid_argument);
+ CHECK_THROWS_AS(sensor.SetDistanceCalibration(
+ std::numeric_limits::infinity(), 1.0, 1.0),
+ std::invalid_argument);
+
+ // The sensor keeps its defaults after every rejected value.
+ CHECK(sensor.GetGain() == Gain::GAIN_4);
+ CHECK(sensor.GetLedDrive() == LedDrive::PERCENT_50);
+ CHECK(sensor.GetIntegrationTime().value() == Catch::Approx(24.0));
+ CHECK(sensor.GetProximityPulseCount() == 8);
+ CHECK(sensor.GetSoftwareGain() == Catch::Approx(1.0));
+}
+
+TEST_CASE_METHOD(ColorSensorTestFixture,
+ "RevColorSensorV2 rejects invalid I2C addresses",
+ "[drivers][rev-color-sensor-v2]") {
+ CHECK_THROWS_AS(wpi::RevColorSensorV2(wpi::I2C::Port::PORT_0, -1),
+ std::invalid_argument);
+ CHECK_THROWS_AS(wpi::RevColorSensorV2(wpi::I2C::Port::PORT_0, 0x80),
+ std::invalid_argument);
+}
+
+TEST_CASE_METHOD(
+ ColorSensorTestFixture,
+ "RevColorSensorV2 stores configuration written before initialization",
+ "[drivers][rev-color-sensor-v2]") {
+ SetRegister(Register::DEVICE_ID, {0x00});
+
+ wpi::RevColorSensorV2 sensor{wpi::I2C::Port::PORT_0};
+ sensor.SetGain(Gain::GAIN_16);
+ sensor.SetProximityPulseCount(32);
+ CHECK(m_writes.empty());
+
+ SetRegister(Register::DEVICE_ID, {TMD37821_DEVICE_ID});
+ SetRegister(Register::STATUS, StatusBlock(STATUS_ALL_VALID, 0, 0, 0, 0, 0));
+ sensor.Update();
+
+ CHECK(sensor.GetDeviceStatus() == DeviceStatus::READY);
+ CHECK(LastWriteTo(Register::CONTROL) ==
+ RegisterWrite(Register::CONTROL, 0x62));
+ CHECK(LastWriteTo(Register::PPULSE) == RegisterWrite(Register::PPULSE, 32));
+}
+
+} // namespace