Configuration

While the sensor is in I2C mode, it is possible to write persistent configurations that will take effect the next time the sensor powers on.

Resetting to I2C Mode

Once you write a configuration, the sensor will no longer communicate data over I2C, but it will listen for attempts to communicate with it over I2C in the first second after a powerup.

To get a Rev Hub to continuously try to communicate with the sensor, initialize and stop the configuration opmode with the sensor unplugged, and you will get a warning which says "failed to communicate with I2C device." At this point, plug in the sensor, and it will reset to I2C mode, with the warning going away and the sensor led blinking.

Reset Sequence:

1. Initialize configuration opmode. (with sensor unplugged)

2. Stop configuration opmode. (the warning "failed to communicate with I2C device" appears)

3. Plug in the sensor. (warning goes away, sensor led blinks)

For sensors shipped before Nov. 2024: there is a physical button used to reset the sensor, use a small tool like a screwdriver or hex key to press the button through the circular hole on the top of the sensor. Hold until the LED blinks off. After a power cycle, the sensor will be back in I2C mode. If you've moved the physical switch for analog modes, make sure to push it back towards the connector side.

Configuring Pins

For configuration in FTC, we recommend using our example configuration opmode file, which contains all the necessary functions to write configurations to the sensor using the control hub. Copy and paste all the contents of the file below into a new blank file:

Configuration OpMode

package org.firstinspires.ftc.teamcode;

import com.qualcomm.hardware.rev.RevColorSensorV3;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple;


@TeleOp
public class ConfigureColorRangefinder extends LinearOpMode {
    @Override
    public void runOpMode() throws InterruptedException {
        ColorRangefinder crf =
                new ColorRangefinder(hardwareMap.get(RevColorSensorV3.class, "Color"));

        /*
        Using this example configuration, you can detect all three sample colors based on which pin is reading true:
        both      --> yellow
        only pin0 --> blue
        only pin1 --> red
        neither   --> no object
         */
        crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, 180 / 360.0 * 255, 250 / 360.0 * 255); // blue
        crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, 55 / 360.0 * 255, 90 / 360.0 * 255); // yellow
        crf.setPin0DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 20); // 20mm or closer requirement

        crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, 0 / 360.0 * 255, 50 / 360.0 * 255); // red
        crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, 55 / 360.0 * 255, 90 / 360.0 * 255); // yellow
        crf.setPin1DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 20); // 20mm or closer requirement

        waitForStart();

        stop();
    }
}

/**
 * Helper class for configuring the Brushland Labs Color Rangefinder.
 * Online documentation: <a href="https://docs.brushlandlabs.com">...</a>
 */
class ColorRangefinder {
    public final RevColorSensorV3 emulator;
    private final I2cDeviceSynchSimple i2c;

    public ColorRangefinder(RevColorSensorV3 emulator) {
        this.emulator = emulator;
        this.i2c = emulator.getDeviceClient();
        this.i2c.enableWriteCoalescing(true);
    }

    /**
     * Configure Pin 0 to be in digital mode, and add a threshold.
     * Multiple thresholds can be added to the same pin by calling this function repeatedly.
     * For colors, bounds should be from 0-255, and for distance, bounds should be from 0-100 (mm).
     */
    public void setPin0Digital(DigitalMode digitalMode, double lowerBound, double higherBound) {
        setDigital(PinNum.PIN0, digitalMode, lowerBound, higherBound);
    }

    /**
     * Configure Pin 1 to be in digital mode, and add a threshold.
     * Multiple thresholds can be added to the same pin by calling this function repeatedly.
     * For colors, bounds should be from 0-255, and for distance, bounds should be from 0-100 (mm).
     */
    public void setPin1Digital(DigitalMode digitalMode, double lowerBound, double higherBound) {
        setDigital(PinNum.PIN1, digitalMode, lowerBound, higherBound);
    }

    /**
     * Sets the maximum distance (in millimeters) within which an object must be located for Pin 0's thresholds to trigger.
     * This is most useful when we want to know if an object is both close and the correct color.
     */
    public void setPin0DigitalMaxDistance(DigitalMode digitalMode, double mmRequirement) {
        setPin0Digital(digitalMode, mmRequirement, mmRequirement);
    }

    /**
     * Sets the maximum distance (in millimeters) within which an object must be located for Pin 1's thresholds to trigger.
     * This is most useful when we want to know if an object is both close and the correct color.
     */
    public void setPin1DigitalMaxDistance(DigitalMode digitalMode, double mmRequirement) {
        setPin1Digital(digitalMode, mmRequirement, mmRequirement);
    }

    /**
     * Invert the hue value before thresholding it, meaning that the colors become their opposite.
     * This is useful if we want to threshold red; instead of having two thresholds we would invert
     * the color and look for blue.
     */
    public void setPin0InvertHue() {
        setPin0DigitalMaxDistance(DigitalMode.HSV, 200);
    }

    /**
     * Invert the hue value before thresholding it, meaning that the colors become their opposite.
     * This is useful if we want to threshold red; instead of having two thresholds we would invert
     * the color and look for blue.
     */
    public void setPin1InvertHue() {
        setPin1DigitalMaxDistance(DigitalMode.HSV, 200);
    }

    /**
     * The denominator is what the raw sensor readings will be divided by before being scaled to 12-bit analog.
     * For the full range of that channel, leave the denominator as 65535 for colors or 100 for distance.
     * Smaller values will clip off higher ranges of the data in exchange for higher resolution within a lower range.
     */
    public void setPin0Analog(AnalogMode analogMode, int denominator) {
        byte denom0 = (byte) (denominator & 0xFF);
        byte denom1 = (byte) ((denominator & 0xFF00) >> 8);
        i2c.write(PinNum.PIN0.modeAddress, new byte[]{analogMode.value, denom0, denom1});
    }

    /**
     * Configure Pin 0 as analog output of one of the six data channels.
     * To read analog, make sure the physical switch on the sensor is flipped away from the
     * connector side.
     */
    public void setPin0Analog(AnalogMode analogMode) {
        setPin0Analog(analogMode, analogMode == AnalogMode.DISTANCE ? 100 : 0xFFFF);
    }

    public float[] getCalibration() {
        java.nio.ByteBuffer bytes =
                java.nio.ByteBuffer.wrap(i2c.read(CALIB_A_VAL_0, 16)).order(java.nio.ByteOrder.LITTLE_ENDIAN);
        return new float[]{bytes.getFloat(), bytes.getFloat(), bytes.getFloat(), bytes.getFloat()};
    }

    /**
     * Save a brightness value of the LED to the sensor.
     *
     * @param value brightness between 0-255
     */
    public void setLedBrightness(int value) {
        i2c.write8(LED_BRIGHTNESS, value);
    }

    /**
     * Change the I2C address at which the sensor will be found. The address can be reset to the
     * default of 0x52 by holding the reset button.
     *
     * @param value new I2C address from 1 to 127
     */
    public void setI2cAddress(int value) {
        i2c.write8(I2C_ADDRESS_REG, value << 1);
    }

    /**
     * Read distance via I2C
     * @return distance in millimeters
     */
    public double readDistance() {
        java.nio.ByteBuffer bytes =
                java.nio.ByteBuffer.wrap(i2c.read(PS_DISTANCE_0, 4)).order(java.nio.ByteOrder.LITTLE_ENDIAN);
        return bytes.getFloat();
    }

    private void setDigital(
            PinNum pinNum,
            DigitalMode digitalMode,
            double lowerBound,
            double higherBound
    ) {
        int lo, hi;
        if (lowerBound == higherBound) {
            lo = (int) lowerBound;
            hi = (int) higherBound;
        } else if (digitalMode.value <= DigitalMode.HSV.value) { // color value 0-255
            lo = (int) Math.round(lowerBound / 255.0 * 65535);
            hi = (int) Math.round(higherBound / 255.0 * 65535);
        } else { // distance in mm
            float[] calib = getCalibration();
            if (lowerBound < .5) hi = 2048;
            else hi = rawFromDistance(calib[0], calib[1], calib[2], calib[3], lowerBound);
            lo = rawFromDistance(calib[0], calib[1], calib[2], calib[3], higherBound);
        }

        byte lo0 = (byte) (lo & 0xFF);
        byte lo1 = (byte) ((lo & 0xFF00) >> 8);
        byte hi0 = (byte) (hi & 0xFF);
        byte hi1 = (byte) ((hi & 0xFF00) >> 8);
        i2c.write(pinNum.modeAddress, new byte[]{digitalMode.value, lo0, lo1, hi0, hi1});
        try {
            Thread.sleep(25);
        } catch (InterruptedException e) {
            throw new RuntimeException(e);
        }
    }

    private double root(double n, double v) {
        double val = Math.pow(v, 1.0 / Math.abs(n));
        if (n < 0) val = 1.0 / val;
        return val;
    }

    private int rawFromDistance(float a, float b, float c, float x0, double mm) {
        return (int) (root(b, (mm - c) / a) + x0);
    }

    private enum PinNum {
        PIN0(0x28), PIN1(0x2D);

        private final byte modeAddress;

        PinNum(int modeAddress) {
            this.modeAddress = (byte) modeAddress;
        }
    }

    // other writeable registers
    private static final byte CALIB_A_VAL_0 = 0x32;
    private static final byte PS_DISTANCE_0 = 0x42;
    private static final byte LED_BRIGHTNESS = 0x46;
    private static final byte I2C_ADDRESS_REG = 0x47;

    public static int invertHue(int hue360) {
        return ((hue360 - 180) % 360);
    }

    public enum DigitalMode {
        RED(1), BLUE(2), GREEN(3), ALPHA(4), HSV(5), DISTANCE(6);
        public final byte value;

        DigitalMode(int value) {
            this.value = (byte) value;
        }
    }

    public enum AnalogMode {
        RED(13), BLUE(14), GREEN(15), ALPHA(16), HSV(17), DISTANCE(18);
        public final byte value;

        AnalogMode(int value) {
            this.value = (byte) value;
        }
    }
}

To write the configuration to the sensor, plug in the sensor to I2C and configure it as a "Rev Color Sensor V3" with the name "Color". After activating the configuration, init the opmode, and the sensor's LED should blink twice to indicate success. At this point you can unplug the sensor and plug it in to the digital or analog port based on your configuration.

If the sensor has a physical switch on the side, make sure it is not pushed toward analog mode.

When one pin is configured to be non-i2c while the other remains in I2C, the sensor will revert to I2C mode. Make sure to configure both pins at once, even if you only need the output from one.

The file contains an example configuration that will let you read the color of a sample when it is 30mms or closer. Pin 0 outputs true when the color is red or yellow, while pin 1 outputs true for blue and yellow. You can then use the pin combinations to detect the color:

	Pin1 True	Pin1 False
Pin0 True	Yellow	Blue
Pin0 False	Red	Other / distance > 30mm

Digital Thresholds

The sensor will output true if the condition(s) are met, and false otherwise.

Color Thresholding

For color differentiation, using "HSV" mode will yield the best results, since hue values are independent to lighting conditions. To find the upper and lower bounds for a color's hue, scale the angles in the below color wheel from 0-360 degrees to 0-255, where 0 degrees is up and increasing clockwise.

HSV Color Wheel

Multiple thresholds ranges can be set by calling the setPinDigital() function repeatedly:

crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, 180 / 360.0 * 255, 250 / 360.0 * 255); // blue
crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, 55 / 360.0 * 255, 75 / 360.0 * 255); // yellow

This example will have Pin 0 output true for both blue and yellow.

Finding Threshold Bounds

Sometimes it is necessary to modify the example values for red, blue, and yellow. To find out what hue the sensor is internally calculating, configure the sensor for analog output:

crf.setPin0Analog(ColorRangefinder.AnalogMode.HSV);
crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, 0, 20); // placeholder

Refer to the Scaling Analog Voltage section to read the hue value. You can then go up and down around 20 for your boundaries.

For example, if the scaled analog voltage reads 70 with a yellow sample in front of the sensor, your bounds for yellow would be 50-90. Don't forget to scale those down to 255 in the configuration code!

Color Thresholding - Distance Requirement

Color thresholds can have a "distance requirement" in order for them to trigger at all.

crf.setPin0DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 30); // 30mm or closer requirement

In this example, if the distance reading is above 30mm, the pin will output false even if the correct color is detected.

Color Thresholding - Inverting Hue

HSV hue values wrap around at 360 degrees, which is in the middle of where the range of red is located. The solution to avoid having to deal with two thresholds is to invert the entire color wheel, so that everything is rotated by 180 degrees. This means that red will be straight down around 180 degrees, instead of straight up at 0 degrees.

crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, 140 / 360.0 * 255, 210 / 360.0 * 255); // inverted red
crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, 235 / 360.0 * 255, 255 / 360.0 * 255); // inverted yellow
crf.setPin1InvertHue(); // invert hue values

Note that this is only necessary for very specific shades of red, so for detecting red samples inversion is not needed.

Distance Thresholding

It is possible to only use the proximity function of the sensor in digital mode:

crf.setPin0Digital(ColorRangefinder.DigitalMode.DISTANCE, 0, 20);
crf.setPin1Digital(ColorRangefinder.DigitalMode.DISTANCE, 0, 40);

In this example, Pin0 will output when the distance reading is below 20mm, while Pin1 will output true when it is under 40mm. It is also possible to read the exact distance value using the analog output, as detailed below.

Analog Output

The sensor generates a voltage of 0-3.3v based on the value of the selected data channel.

crf.setPin0Analog(ColorRangefinder.AnalogMode.DISTANCE);

Only pin 0 is able to output analog. If the sensor has a switch on the side, flip it away from the connector side after configuring to access the analog value.

Refer to the Analog Reads section for how to get the value in code and convert units.

It is possible for pin 0 to output analog while pin 1 outputs digital since the two pins operate independently of each other (except in I2C mode), however this would require some custom wiring or a sensor splitter cable to interface with the hubs.

LED Brightness

The brightness of the illuminating white LED can be adjusted using the following code, where the input is a value from 0-100:

crf.setLedBrightness(20);

Note that this is nonlinear, so a brightness value of 50 will be almost identical to 100. The LED becomes visibly dimmer at values below 30.

I2C Address

The default I2C address of the sensor is 0x52, which is shared with some other optical sensors like the Rev Color Sensor. If you want to connect multiple devices to the same bus, you can change the address of the sensor using the configuration opmode:

crf.setI2cAddress(0x53);

Valid I2C addresses have to be between 1 and 127, or 0x1 and 0x7F in hexadecimal.