Writing code for a color sensor


#1

Hi everyone! I’m the head programmer for team 7034. For BunnyBots this year, our robot needs a color sensor. However, there isn’t a class for one in the FRC library, and I have no experience of writing code like this before. We’re working with the Vex Color Sensor V2. I found the data sheet here and also the sensor’s datasheet here. We have the sensor wired up to the RoboRio, but whenever we try to read an RGB value, it simply returns the address we’re trying to read the value from.


public class ColorSensor{
    ByteBuffer buffer = ByteBuffer.allocate(1);
    I2C sensor;
    public ColorSensor(){
        sensor = new I2C(I2C.Port.kOnboard, 0x39); //0x39 is the sensor's i2c address
        sensor.write(0x00, 192); //0b11000000 ... Power on, color sensor on. (page 20 of sensor datasheet)
    }
    public int red(){
        sensor.read(0x16, 1, buffer);
        return buffer.get(0);
    }
    public int green(){
        sensor.read(0x18, 1, buffer);
        return buffer.get(0);
    }
    public int blue(){
        sensor.read(0x1A, 1, buffer);
        return buffer.get(0);
    }
}

I don’t have access to the code right now so there might be minor syntax errors but for the most part it should be correct. We write 192 to the 0x00 register in order to turn the sensor on and to enable the RGB functions (page 20 in the sensor’s datasheet). We weren’t sure if we were supposed to read the binary left to right, or from 0-7, so we also tried writing 3 to the register (0b00000011), but it also did not change the result. When we read from any address, it simply returns the address. So if we are trying to read the blue value from 0x1A, it returns 26, the address in decimal. We simply cannot decipher the datasheet and, with no experience and no software mentors, are at wit’s end. If anyone has experience writing code for sensors or reading datasheets, your help would be greatly appreciated!


REV Color Sensor V2 and RoboRIO Communication
Lining up with tape on ground for new game?
#2

What does the read() method return? If you’re getting good data (from the sensor) you’ll get a false return value. A good sanity check for I2C devices is to read their WHO_AM_I register (0x12 device ID in this case) to make sure you’re connected. It should be 0x60 or 0x69 for this device.

Your values for the Enable register are the wrong endianness. It should in fact be 0b00 00 00 11 for PON and AEN.

Note also that the ADC registers (rgbc) are 16 bit, which means to properly get all the data from it you need to read 2 bytes then join them together.


#3

Thanks for the help Nick. We’re still not able to get the right data from the sensor because while the read() method returns false, we get a weird result. Every register we try to read returns the number of the address, converted to decimal (after “sensor.read(0x16, 2, buf);”, “buf.get(0);” returns 22.0). We tried the WHO_AM_I register, but it gave us 18.0 (0x12 as hex). Here is the code we are using:


package org.usfirst.frc.team7034.robot;

import java.nio.ByteBuffer;
import edu.wpi.first.wpilibj.I2C;

public class ColorSensor {
    private I2C sensor;
    private ByteBuffer buf = ByteBuffer.allocate(5);
    
    public ColorSensor(I2C.Port port) {
   	 sensor = new I2C(port, 0x39); //port, I2c address    

   	 sensor.write(0x00, 0b00000011); //power on, color sensor on
    }
    
    public int red() {
   	 //reads the address 0x16, 2 bytes of data, gives it to the buffer
   	 sensor.read(0x16, 3, buf);
   	 return buf.get(0);
    }
    
    public int green() {
   	 sensor.read(0x18, 2, buf);
   	 return buf.get(0);
    }
    
    public int blue() {
   	 sensor.read(0x1a, 2, buf);
   	 return buf.get(0);
    }
}

We tested to see if anything would change if the sensor was unplugged, and we got zeros on all registers. The sensor is making some sort of communication, but is not giving us good data. We also tried the I2C addressOnly() method, which returned “true” for aborted.

While looking at pg.17 of the sensor documentation, we found the Command Register, which “specifies the address of the target register for future write and read operations.” We think we need to write to this to select the Enable Register address before writing to that. However FRC’s I2C only allows you to write to an address. Also, after you select the address with this command, you wouldn’t need to specify the address to write to, because you already specified it, correct? We are wondering if this Command Register is already handled in the I2C class, and if not, how we might go about writing our own class to handle this. Or maybe we are completely off.


#4

I can’t speak for the Vex sensor, but based on the datasheet I can share some insight. :slight_smile:

The descriptions in the datasheet are a bit unclear, but you are close, the key part that I suspect you are missing is actually the description of bit 7 in the command register:

Select Command Register. Must write as 1 when addressing COMMAND register.

I have not used the sensor with the roboRIO before, but the way we handle this on our Expansion Hub software is by writing the address with bit 7 set (we also use the auto-increment function), so something similar to this:

COMMAND_REGISTER_BIT = 0x80
MULTI_BYTE_BIT = 0x20

def getRegisterValue( self, registerAddress ):
  self.writeByte( COMMAND_REGISTER_BIT | MULTI_BYTE_BIT | registerAddress )

  return self.readMultipleBytes( 2 )

Looking at the specific roboRIO code for I2C Read, it looks like it does write the address, then reads the corresponding number of bytes, similar to the code snippet above. So I expect that if you were to set that bit you should be able to read from the correct address.

You will also need the command register bit for your initialization.

So something like this:


protected final static int COMMAND_REGISTER_BIT = 0x80;
protected final static int MULTI_BYTE_BIT = 0x20;

protected final static int ENABLE_REGISTER   = 0x00
protected final static int ATIME_REGISTER    = 0x01
protected final static int PPULSE_REGISTER   = 0x0E

protected final static int ID_REGISTER       = 0x12
protected final static int CDATA_REGISTER    = 0x14
protected final static int RDATA_REGISTER    = 0x16
protected final static int GDATA_REGISTER    = 0x18
protected final static int BDATA_REGISTER    = 0x1A
protected final static int PDATA_REGISTER    = 0x1C

public ColorSensor(I2C.Port port) {
  sensor = new I2C(port, 0x39); //port, I2c address    

  sensor.write(COMMAND_REGISTER_BIT | 0x00, 0b00000011); //power on, color sensor on
}

protected int readWordRegister(int address) {
  ByteBuffer buf = ByteBuffer.allocate(2);
  sensor.read(COMMAND_REGISTER_BIT | MULTI_BYTE_BIT | address, 2, buf);
  buf.order(ByteOrder.LITTLE_ENDIAN);
  return buf.getShort(0);
}

public int red() {
 return readWordRegister(RDATA_REGISTER);
}

public int green() {
 return readWordRegister(GDATA_REGISTER);
}

public int blue() {
 return readWordRegister(BDATA_REGISTER);
}

public int clear() {
 return readWordRegister(CDATA_REGISTER);
}

public int proximity() {
 return readWordRegister(PDATA_REGISTER);
}

I haven’t tested this or used it with the roboRIO, so let us know if you get it working!


REV Color Sensor V2 and RoboRIO Communication
#5

Hey thank you for your help!! We got it working successfully. :]


#6

You might find this post useful:


#7

Would you share the fully functioning code? We are looking to use this sensor too. Also, when you call the ColorSensor constructor, what gets passed in for the port variable?


#8

Here is our version of the final code. We used some of our own design and some of Will’s code.

package org.usfirst.frc.team7034.robot;

import edu.wpi.first.wpilibj.I2C;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;

public class ColorSensor {
protected final static int CMD = 0x80;
protected final static int MULTI_BYTE_BIT = 0x20;

protected final static int ENABLE_REGISTER  = 0x00;
protected final static int ATIME_REGISTER   = 0x01;
protected final static int PPULSE_REGISTER  = 0x0E;

protected final static int ID_REGISTER     = 0x12;
protected final static int CDATA_REGISTER  = 0x14;
protected final static int RDATA_REGISTER  = 0x16;
protected final static int GDATA_REGISTER  = 0x18;
protected final static int BDATA_REGISTER  = 0x1A;
protected final static int PDATA_REGISTER  = 0x1C;

protected final static int PON   = 0b00000001;
protected final static int AEN   = 0b00000010;
protected final static int PEN   = 0b00000100;
protected final static int WEN   = 0b00001000;
protected final static int AIEN  = 0b00010000;
protected final static int PIEN  = 0b00100000;

private final double integrationTime = 10;


private I2C sensor;

private ByteBuffer buffy = ByteBuffer.allocate(8);

public short red = 0, green = 0, blue = 0, prox = 0;

public ColorSensor(I2C.Port port) {
	buffy.order(ByteOrder.LITTLE_ENDIAN);
    sensor = new I2C(port, 0x39); //0x39 is the address of the Vex ColorSensor V2
    
    sensor.write(CMD | 0x00, PON | AEN | PEN);
    
    sensor.write(CMD | 0x01, (int) (256-integrationTime/2.38)); //configures the integration time (time for updating color data)
    sensor.write(CMD | 0x0E, 0b1111);
    
}



public void read() {
	buffy.clear();
    sensor.read(CMD | MULTI_BYTE_BIT | RDATA_REGISTER, 8, buffy);
    
    red = buffy.getShort(0);
    if(red < 0) { red += 0b10000000000000000; }
    
    green = buffy.getShort(2);
    if(green < 0) { green += 0b10000000000000000; }
    
    blue = buffy.getShort(4); 
    if(blue < 0) { blue += 0b10000000000000000; }
    
    prox = buffy.getShort(6); 
    if(prox < 0) { prox += 0b10000000000000000; }
    
}

public int status() {
	buffy.clear();
	sensor.read(CMD | 0x13, 1, buffy);
	return buffy.get(0);
}

public void free() {
	sensor.free();
}

}

We used I2C.Port.kOnboard in the constructor for the roboRIO I2C port.


#9

Thanks for sharing.


#10

Do you have any example code for how we can use the sensor? So far, I’ve called read, and tried to print out the values of the colors onto the the Message Console, but we’re just outputting a whole bunch of zeroes


#11

Sure, here’s some pseudocode!

//in robot init
private ColorSensor cs = new ColorSensor(I2C.Port.kOnboard); //I2C.Port.kMXP if plugged into the NavX's I2C port

//in teleop periodic
cs.read(); //update color sensor values
cs.red; //access the values for red, green, blue like any other variable
cs.green;
cs.blue;

One thing to note is that the I2C pinout on the sensor is different than the I2C port on the RoboRio. The SCL and SDA lines are swapped. This could be causing the error. Check the sensor’s datasheet here for the sensor pinout and make sure it matches what’s printed on the RoboRio. Hope this helps!


#12

What does the PON-PIEN lines stand for?


#13

Those are the “commands” that you can issue the color sensor. To briefly list their names, PON is Power On, AEN enables the ADC (4 channel RGBC), PEN enables proximity sensing (we haven’t gotten this to work; we also haven’t tried a lot), WEN enables the wait feature, AIEN is Ambient Light Sensing Intterupt Enable, PIEN is Proximity Interrupt Enable. These are all features that can be enabled by writing to the “Enable Register (0x00)”.

To see a better description of what each of those mean, and how to use them, you’ll have to read the sensor’s datasheet. We only really used PON to turn the sensor on and AEN to enable color sensing. You can see a table with these values on page 20 of the datasheet.


#14

For those interested, PeacefulPaul has created some LabVIEW sample code for using the REV Robotics Color Sensor V2. You can find in it this thread on the NI Forum.

We’ve not tested it yet, but it looks promising!