001 /*----------------------------------------------------------------------------*/
002 /* Copyright (c) FIRST 2008-2012. All Rights Reserved. */
003 /* Open Source Software - may be modified and shared by FRC teams. The code */
004 /* must be accompanied by the FIRST BSD license file in the root directory of */
005 /* the project. */
006 /*----------------------------------------------------------------------------*/
007
008 package edu.wpi.first.wpilibj;
009
010 import edu.wpi.first.wpilibj.communication.UsageReporting;
011 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
012 import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
013 import edu.wpi.first.wpilibj.networktables2.type.NumberArray;
014 import edu.wpi.first.wpilibj.parsing.ISensor;
015 import edu.wpi.first.wpilibj.tables.ITable;
016
017 /**
018 * HiTechnic NXT Color Sensor.
019 *
020 * This class allows access to a HiTechnic NXT Color Sensor on an I2C bus.
021 * These sensors do not allow changing addresses so you cannot have more
022 * than one on a single bus.
023 *
024 * Details on the sensor can be found here:
025 * http://www.hitechnic.com/index.html?lang=en-us&target=d17.html
026 *
027 */
028 public class HiTechnicColorSensor extends SensorBase implements ISensor, LiveWindowSendable {
029
030 /**
031 * An exception dealing with connecting to and communicating with the
032 * HiTechnicCompass
033 */
034 public class ColorSensorException extends RuntimeException {
035
036 /**
037 * Create a new exception with the given message
038 * @param message the message to pass with the exception
039 */
040 public ColorSensorException(String message) {
041 super(message);
042 }
043
044 }
045
046 /**
047 * A set of three color values bundled into one object
048 */
049 public class RGB {
050 public double red, green, blue;
051
052 public double getRed() {return red;}
053 public double getGreen() {return green;}
054 public double getBlue() {return blue;}
055 }
056
057 public static class tColorSensorMode {
058 public final int value;
059 static final int kActive_val = 0;
060 static final int kPassive_val = 1;
061 static final int kRaw_val = 3;
062 public static final tColorSensorMode kActive = new tColorSensorMode(kActive_val);
063 public static final tColorSensorMode kPassive = new tColorSensorMode(kPassive_val);
064 public static final tColorSensorMode kRaw = new tColorSensorMode(kRaw_val);
065
066 private tColorSensorMode(int value) {
067 this.value = value;
068 }
069 }
070
071 private static final byte kAddress = 0x02;
072 private static final byte kManufacturerBaseRegister = 0x08;
073 private static final byte kManufacturerSize = 0x08;
074 private static final byte kSensorTypeBaseRegister = 0x10;
075 private static final byte kSensorTypeSize = 0x08;
076 private static final byte kModeRegister = 0x41;
077 private static final byte kColorRegister = 0x42;
078 private static final byte kRedRegister = 0x43;
079 private static final byte kGreenRegister = 0x44;
080 private static final byte kBlueRegister = 0x45;
081 private static final byte kRawRedRegister = 0x43;
082 private static final byte kRawGreenRegister = 0x45;
083 private static final byte kRawBlueRegister = 0x47;
084 private I2C m_i2c;
085 private int m_mode = tColorSensorMode.kActive.value;
086
087 /**
088 * Constructor.
089 *
090 * @param slot The slot of the digital module that the sensor is plugged into.
091 */
092 public HiTechnicColorSensor(int slot) {
093 DigitalModule module = DigitalModule.getInstance(slot);
094 m_i2c = module.getI2C(kAddress);
095
096 // Verify Sensor
097 final byte[] kExpectedManufacturer = "HiTechnc".getBytes();
098 final byte[] kExpectedSensorType = "ColorPD ".getBytes();
099 if (!m_i2c.verifySensor(kManufacturerBaseRegister, kManufacturerSize, kExpectedManufacturer)) {
100 throw new ColorSensorException("Invalid Color Sensor Manufacturer");
101 }
102 if (!m_i2c.verifySensor(kSensorTypeBaseRegister, kSensorTypeSize, kExpectedSensorType)) {
103 throw new ColorSensorException("Invalid Sensor type");
104 }
105
106 LiveWindow.addSensor("HiTechnicColorSensor", slot, 0, this);
107 UsageReporting.report(UsageReporting.kResourceType_HiTechnicColorSensor, module.getModuleNumber()-1);
108 }
109
110 /**
111 * Destructor.
112 */
113 public void free() {
114 if (m_i2c != null) {
115 m_i2c.free();
116 }
117 m_i2c = null;
118 }
119
120 /**
121 * Get the estimated color.
122 *
123 * Gets a color estimate from the sensor corresponding to the
124 * table found with the sensor or at the following site:
125 * http://www.hitechnic.com/cgi-bin/commerce.cgi?preadd=action&key=NCO1038
126 *
127 * @return The estimated color.
128 */
129 public byte getColor() {
130 byte[] color = new byte[1];
131 if(m_mode != tColorSensorMode.kActive.value)
132 {
133 setMode(tColorSensorMode.kActive);
134 }
135 m_i2c.read(kColorRegister, (byte) color.length, color);
136
137 return color[0];
138 }
139
140 /**
141 * Get the value of all three colors from a single sensor reading.
142 * Using this method ensures that all three values come from the
143 * same sensor reading, using the individual color methods provides
144 * no such guarantee.
145 *
146 * The sensor must be in active mode to access the regular RGB data
147 * if the sensor is not in active mode, it will be placed into active
148 * mode by this method.
149 *
150 * @return RGB object with the three color values
151 */
152 public RGB getRGB() {
153 byte[] colors = new byte[3];
154 RGB result = new RGB();
155 if(m_mode != tColorSensorMode.kActive.value)
156 {
157 setMode(tColorSensorMode.kActive);
158 }
159 m_i2c.read(kRedRegister, (byte) colors.length, colors);
160
161 result.red = (colors[0]&0xFFFF);
162 result.green = (colors[1]&0xFFFF);
163 result.blue = (colors[2]&0xFFFF);
164 return result;
165 }
166
167 /**
168 * Get the Red value.
169 *
170 * Gets the (0-255) red value from the sensor.
171 *
172 * The sensor must be in active mode to access the regular RGB data
173 * if the sensor is not in active mode, it will be placed into active
174 * mode by this method.
175 *
176 * @return The Red sensor value.
177 */
178 public int getRed() {
179 byte[] red = new byte[1];
180 if(m_mode != tColorSensorMode.kActive.value)
181 {
182 setMode(tColorSensorMode.kActive);
183 }
184 m_i2c.read(kRedRegister, (byte) red.length, red);
185
186 return (red[0]&0xFF);
187 }
188
189 /**
190 * Get the Green value.
191 *
192 * Gets the(0-255) green value from the sensor.
193 *
194 * The sensor must be in active mode to access the regular RGB data
195 * if the sensor is not in active mode, it will be placed into active
196 * mode by this method.
197 *
198 * @return The Green sensor value.
199 */
200 public int getGreen() {
201 byte[] green = new byte[1];
202 if(m_mode != tColorSensorMode.kActive.value)
203 {
204 setMode(tColorSensorMode.kActive);
205 }
206 m_i2c.read(kGreenRegister, (byte) green.length, green);
207
208 return (green[0]&0xFF);
209 }
210
211 /**
212 * Get the Blue value.
213 *
214 * Gets the raw (0-255) blue value from the sensor.
215 *
216 * The sensor must be in active mode to access the regular RGB data
217 * if the sensor is not in active mode, it will be placed into active
218 * mode by this method.
219 *
220 * @return The Blue sensor value.
221 */
222 public int getBlue() {
223 byte[] blue = new byte[1];
224 if(m_mode != tColorSensorMode.kActive.value)
225 {
226 setMode(tColorSensorMode.kActive);
227 }
228 m_i2c.read(kBlueRegister, (byte) blue.length, blue);
229
230 return (blue[0]&0xFF);
231 }
232
233 /**
234 * Get the Raw Red value.
235 *
236 * Gets the (0-65536) raw red value from the sensor.
237 *
238 * The sensor must be in raw or passive mode to access the regular RGB data
239 * if the sensor is not in raw or passive mode, it will be placed into raw
240 * mode by this method.
241 *
242 * @return The Raw Red sensor value.
243 */
244 public double getRawRed() {
245 byte[] rawRed = new byte[2];
246 if(m_mode == tColorSensorMode.kActive.value)
247 {
248 setMode(tColorSensorMode.kRaw);
249 }
250 m_i2c.read(kRawRedRegister, (byte) rawRed.length, rawRed);
251
252 return (((int)rawRed[0]&0xFF) * (int) (1 << 8) + ((int)rawRed[1]&0xFF));
253 }
254
255 /**
256 * Get the Raw Green value.
257 *
258 * Gets the (0-65536) raw green value from the sensor.
259 *
260 * The sensor must be in raw or passive mode to access the regular RGB data
261 * if the sensor is not in raw or passive mode, it will be placed into raw
262 * mode by this method.
263 *
264 * @return The Raw Green sensor value.
265 */
266 public double getRawGreen() {
267 byte[] rawGreen = new byte[2];
268 if(m_mode == tColorSensorMode.kActive.value)
269 {
270 setMode(tColorSensorMode.kRaw);
271 }
272 m_i2c.read(kRawGreenRegister, (byte) rawGreen.length, rawGreen);
273
274 return (((int)rawGreen[0]&0xFF) * (int) (1 << 8) + ((int)rawGreen[1]&0xFF));
275 }
276
277 /**
278 * Get the Raw Blue value.
279 *
280 * Gets the (0-65536) raw blue value from the sensor.
281 *
282 * The sensor must be in raw or passive mode to access the regular RGB data
283 * if the sensor is not in raw or passive mode, it will be placed into raw
284 * mode by this method.
285 *
286 * @return The Raw Blue sensor value.
287 */
288 public double getRawBlue() {
289 byte[] rawBlue = new byte[2];
290 if(m_mode == tColorSensorMode.kActive.value)
291 {
292 setMode(tColorSensorMode.kRaw);
293 }
294 m_i2c.read(kRawBlueRegister, (byte) rawBlue.length, rawBlue);
295
296 return (((int)rawBlue[0]&0xFF) * (int) (1 << 8) + ((int)rawBlue[1]&0xFF));
297 }
298
299 /**
300 * Get the raw value of all three colors from a single sensor reading.
301 * Using this method ensures that all three values come from the
302 * same sensor reading, using the individual color methods provides
303 * no such guarantee.
304 *
305 * Gets the (0-65536) raw color values from the sensor.
306 *
307 * The sensor must be in raw or passive mode to access the regular RGB data
308 * if the sensor is not in raw or passive mode, it will be placed into raw
309 * mode by this method.
310 *
311 * @return An RGB object with the raw sensor values.
312 */
313 public RGB getRawRGB() {
314 byte[] colors = new byte[6];
315 RGB result = new RGB();
316 if(m_mode == tColorSensorMode.kActive.value)
317 {
318 setMode(tColorSensorMode.kRaw);
319 }
320 m_i2c.read(kRawRedRegister, (byte) colors.length, colors);
321
322 result.red = (((int)colors[0]&0xFF) * (int) (1 << 8) + ((int)colors[1]&0xFF));
323 result.green = (((int)colors[2]&0xFF) * (int) (1 << 8) + ((int)colors[3]&0xFF));
324 result.blue = (((int)colors[4]&0xFF) * (int) (1 << 8) + ((int)colors[5]&0xFF));
325 return result;
326 }
327
328 /**
329 * Set the Mode of the color sensor
330 * This method is used to set the color sensor to one of the three modes,
331 * active, passive or raw. The sensor defaults to active mode which uses the
332 * internal LED and returns an interpreted color value and 3 8-bit RGB channel
333 * values. Raw mode uses the internal LED and returns 3 16-bit RGB channel values.
334 * Passive mode disables the internal LED and returns 3 16-bit RGB channel values.
335 * @param mode The mode to set
336 */
337 public void setMode(tColorSensorMode mode) {
338 m_i2c.write(kModeRegister, mode.value);
339 m_mode = mode.value;
340 }
341
342 /*
343 * Live Window code, only does anything if live window is activated.
344 * TODO: Should this have its own type?
345 */
346 public String getSmartDashboardType(){
347 return "Counter";
348 }
349 private ITable m_table;
350
351 /**
352 * {@inheritDoc}
353 */
354 public void initTable(ITable subtable) {
355 m_table = subtable;
356 updateTable();
357 }
358
359 /**
360 * {@inheritDoc}
361 */
362 public ITable getTable(){
363 return m_table;
364 }
365
366 private NumberArray rgbArray = new NumberArray();
367 /**
368 * {@inheritDoc}
369 */
370 public void updateTable() {
371 if (m_table != null) {
372 if(rgbArray.size() == 0) {
373 rgbArray.setSize(3);
374 }
375 if(m_mode == tColorSensorMode.kActive.value)
376 {
377 m_table.putNumber("Color", getColor());
378 rgbArray.set(0, getRed());
379 rgbArray.set(1, getGreen());
380 rgbArray.set(2, getBlue());
381 m_table.putValue("RGB", rgbArray);
382 }
383 else
384 {
385 m_table.putNumber("Color", 99);
386 m_table.putValue("RGB", rgbArray);
387 }
388 }
389 }
390
391 /**
392 * {@inheritDoc}
393 */
394 public void startLiveWindowMode() {}
395
396 /**
397 * {@inheritDoc}
398 */
399 public void stopLiveWindowMode() {}
400 }