mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
69d9ad70ab
This is the changes made by Patrick Plenefisch converting the native code to use CMake and the CMake Maven Plugin, as opposed to the native Maven plugin. This is to allow for compatibility with newer versions of the GCC toolchain. All the cpp sources were moved from maven style directories to cpp style directories for CMake. Change-Id: I67f5e3608948f37c83b0990d232105a3784f8593
389 lines
8.8 KiB
C++
389 lines
8.8 KiB
C++
/*----------------------------------------------------------------------------*/
|
|
/* Copyright (c) FIRST 2008. All Rights Reserved. */
|
|
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
|
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
|
|
/*----------------------------------------------------------------------------*/
|
|
|
|
#include "HiTechnicColorSensor.h"
|
|
#include "DigitalModule.h"
|
|
#include "I2C.h"
|
|
//#include "NetworkCommunication/UsageReporting.h"
|
|
#include "networktables2/type/NumberArray.h"
|
|
#include "WPIErrors.h"
|
|
|
|
const uint8_t HiTechnicColorSensor::kAddress;
|
|
const uint8_t HiTechnicColorSensor::kManufacturerBaseRegister;
|
|
const uint8_t HiTechnicColorSensor::kManufacturerSize;
|
|
const uint8_t HiTechnicColorSensor::kSensorTypeBaseRegister;
|
|
const uint8_t HiTechnicColorSensor::kSensorTypeSize;
|
|
const uint8_t HiTechnicColorSensor::kModeRegister;
|
|
const uint8_t HiTechnicColorSensor::kColorRegister;
|
|
const uint8_t HiTechnicColorSensor::kRedRegister;
|
|
const uint8_t HiTechnicColorSensor::kGreenRegister;
|
|
const uint8_t HiTechnicColorSensor::kBlueRegister;
|
|
const uint8_t HiTechnicColorSensor::kRawRedRegister;
|
|
const uint8_t HiTechnicColorSensor::kRawGreenRegister;
|
|
const uint8_t HiTechnicColorSensor::kRawBlueRegister;
|
|
|
|
/**
|
|
* Constructor.
|
|
*
|
|
* @param moduleNumber The digital module that the sensor is plugged into (1 or 2).
|
|
*/
|
|
HiTechnicColorSensor::HiTechnicColorSensor(uint8_t moduleNumber)
|
|
: m_i2c (NULL)
|
|
{
|
|
m_table = NULL;
|
|
DigitalModule *module = DigitalModule::GetInstance(moduleNumber);
|
|
m_mode = kActive;
|
|
|
|
if (module)
|
|
{
|
|
m_i2c = module->GetI2C(kAddress);
|
|
|
|
// Verify Sensor
|
|
const uint8_t kExpectedManufacturer[] = "HiTechnc";
|
|
const uint8_t kExpectedSensorType[] = "ColorPD ";
|
|
if ( ! m_i2c->VerifySensor(kManufacturerBaseRegister, kManufacturerSize, kExpectedManufacturer) )
|
|
{
|
|
wpi_setWPIError(CompassManufacturerError);
|
|
return;
|
|
}
|
|
if ( ! m_i2c->VerifySensor(kSensorTypeBaseRegister, kSensorTypeSize, kExpectedSensorType) )
|
|
{
|
|
wpi_setWPIError(CompassTypeError);
|
|
}
|
|
|
|
HALReport(HALUsageReporting::kResourceType_HiTechnicColorSensor, moduleNumber - 1);
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Destructor.
|
|
*/
|
|
HiTechnicColorSensor::~HiTechnicColorSensor()
|
|
{
|
|
delete m_i2c;
|
|
m_i2c = NULL;
|
|
}
|
|
|
|
/**
|
|
* Get the estimated color.
|
|
*
|
|
* Gets a color estimate from the sensor corresponding to the
|
|
* table found with the sensor or at the following site:
|
|
* http://www.hitechnic.com/cgi-bin/commerce.cgi?preadd=action&key=NCO1038
|
|
*
|
|
* @return The estimated color.
|
|
*/
|
|
uint8_t HiTechnicColorSensor::GetColor()
|
|
{
|
|
uint8_t color = 0;
|
|
|
|
if(m_mode != kActive)
|
|
{
|
|
SetMode(kActive);
|
|
}
|
|
if (m_i2c)
|
|
{
|
|
m_i2c->Read(kColorRegister, sizeof(color), &color);
|
|
}
|
|
return color;
|
|
}
|
|
|
|
/**
|
|
* Get the Red value.
|
|
*
|
|
* Gets the (0-255) red value from the sensor.
|
|
*
|
|
* The sensor must be in active mode to access the regular RGB data
|
|
* if the sensor is not in active mode, it will be placed into active
|
|
* mode by this method.
|
|
*
|
|
* @return The Red sensor value.
|
|
*/
|
|
uint8_t HiTechnicColorSensor::GetRed()
|
|
{
|
|
uint8_t red = 0;
|
|
|
|
if(m_mode != kActive)
|
|
{
|
|
SetMode(kActive);
|
|
}
|
|
if (m_i2c)
|
|
{
|
|
m_i2c->Read(kRedRegister, sizeof(red), &red);
|
|
}
|
|
return red;
|
|
}
|
|
|
|
/**
|
|
* Get the Green value.
|
|
*
|
|
* Gets the(0-255) green value from the sensor.
|
|
*
|
|
* The sensor must be in active mode to access the regular RGB data
|
|
* if the sensor is not in active mode, it will be placed into active
|
|
* mode by this method.
|
|
*
|
|
* @return The Green sensor value.
|
|
*/
|
|
uint8_t HiTechnicColorSensor::GetGreen()
|
|
{
|
|
uint8_t green = 0;
|
|
|
|
if(m_mode != kActive)
|
|
{
|
|
SetMode(kActive);
|
|
}
|
|
if (m_i2c)
|
|
{
|
|
m_i2c->Read(kGreenRegister, sizeof(green), &green);
|
|
}
|
|
return green;
|
|
}
|
|
|
|
/**
|
|
* Get the Blue value.
|
|
*
|
|
* Gets the raw (0-255) blue value from the sensor.
|
|
*
|
|
* The sensor must be in active mode to access the regular RGB data
|
|
* if the sensor is not in active mode, it will be placed into active
|
|
* mode by this method.
|
|
*
|
|
* @return The Blue sensor value.
|
|
*/
|
|
uint8_t HiTechnicColorSensor::GetBlue()
|
|
{
|
|
uint8_t blue = 0;
|
|
|
|
if(m_mode != kActive)
|
|
{
|
|
SetMode(kActive);
|
|
}
|
|
if (m_i2c)
|
|
{
|
|
m_i2c->Read(kBlueRegister, sizeof(blue), &blue);
|
|
}
|
|
return blue;
|
|
}
|
|
|
|
/**
|
|
* Get the value of all three colors from a single sensor reading.
|
|
* Using this method ensures that all three values come from the
|
|
* same sensor reading, using the individual color methods provides
|
|
* no such guarantee.
|
|
*
|
|
* The sensor must be in active mode to access the regular RGB data.
|
|
* If the sensor is not in active mode, it will be placed into active
|
|
* mode by this method.
|
|
*
|
|
* @return RGB object with the three color values
|
|
*/
|
|
HiTechnicColorSensor::RGB HiTechnicColorSensor::GetRGB()
|
|
{
|
|
uint8_t colors[3] = {0,0,0};
|
|
RGB result;
|
|
|
|
if(m_mode != kActive)
|
|
{
|
|
SetMode(kActive);
|
|
}
|
|
if(m_i2c)
|
|
{
|
|
m_i2c->Read(kRawRedRegister, sizeof(colors), (uint8_t*)&colors);
|
|
}
|
|
|
|
result.red = colors[0];
|
|
result.green = colors[1];
|
|
result.blue = colors[2];
|
|
|
|
return result;
|
|
}
|
|
|
|
/**
|
|
* Get the Raw Red value.
|
|
*
|
|
* Gets the (0-65536) raw red value from the sensor.
|
|
*
|
|
* The sensor must be in raw or passive mode to access the regular RGB data
|
|
* if the sensor is not in raw or passive mode, it will be placed into raw
|
|
* mode by this method.
|
|
*
|
|
* @return The Raw Red sensor value.
|
|
*/
|
|
uint16_t HiTechnicColorSensor::GetRawRed()
|
|
{
|
|
uint16_t rawRed = 0;
|
|
|
|
if(m_mode == kActive)
|
|
{
|
|
SetMode(kRaw);
|
|
}
|
|
if (m_i2c)
|
|
{
|
|
m_i2c->Read(kRawRedRegister, sizeof(rawRed), (uint8_t *)&rawRed);
|
|
}
|
|
return rawRed;
|
|
}
|
|
|
|
/**
|
|
* Get the Raw Green value.
|
|
*
|
|
* Gets the (0-65536) raw green value from the sensor.
|
|
*
|
|
* The sensor must be in raw or passive mode to access the regular RGB data
|
|
* if the sensor is not in raw or passive mode, it will be placed into raw
|
|
* mode by this method.
|
|
*
|
|
* @return The Raw Green sensor value.
|
|
*/
|
|
uint16_t HiTechnicColorSensor::GetRawGreen()
|
|
{
|
|
uint16_t rawGreen = 0;
|
|
|
|
if(m_mode == kActive)
|
|
{
|
|
SetMode(kRaw);
|
|
}
|
|
if (m_i2c)
|
|
{
|
|
m_i2c->Read(kRawGreenRegister, sizeof(rawGreen), (uint8_t *)&rawGreen);
|
|
}
|
|
return rawGreen;
|
|
}
|
|
|
|
/**
|
|
* Get the Raw Blue value.
|
|
*
|
|
* Gets the (0-65536) raw blue value from the sensor.
|
|
*
|
|
* The sensor must be in raw or passive mode to access the regular RGB data
|
|
* if the sensor is not in raw or passive mode, it will be placed into raw
|
|
* mode by this method.
|
|
*
|
|
* @return The Raw Blue sensor value.
|
|
*/
|
|
uint16_t HiTechnicColorSensor::GetRawBlue()
|
|
{
|
|
uint16_t rawBlue = 0;
|
|
|
|
if(m_mode == kActive)
|
|
{
|
|
SetMode(kRaw);
|
|
}
|
|
if (m_i2c)
|
|
{
|
|
m_i2c->Read(kRawBlueRegister, sizeof(rawBlue), (uint8_t *)&rawBlue);
|
|
}
|
|
return rawBlue;
|
|
}
|
|
|
|
/**
|
|
* Get the raw value of all three colors from a single sensor reading.
|
|
* Using this method ensures that all three values come from the
|
|
* same sensor reading, using the individual color methods provides
|
|
* no such guarantee.
|
|
*
|
|
* Gets the (0-65536) raw color values from the sensor.
|
|
*
|
|
* The sensor must be in raw or passive mode to access the regular RGB data
|
|
* if the sensor is not in raw or passive mode, it will be placed into raw
|
|
* mode by this method.
|
|
*
|
|
* @return An RGB object with the raw sensor values.
|
|
*/
|
|
HiTechnicColorSensor::RGB HiTechnicColorSensor::GetRawRGB()
|
|
{
|
|
uint8_t colors[6] = {0,0,0,0,0,0};
|
|
RGB result;
|
|
|
|
if(m_mode != kActive)
|
|
{
|
|
SetMode(kActive);
|
|
}
|
|
if(m_i2c)
|
|
{
|
|
m_i2c->Read(kRedRegister, sizeof(colors), (uint8_t*)&colors);
|
|
}
|
|
|
|
result.red = (colors[0]<<8) + colors[1];
|
|
result.green = (colors[2]<<8) + colors[3];
|
|
result.blue = (colors[4]<<8) + colors[5];
|
|
|
|
return result;
|
|
}
|
|
|
|
/**
|
|
* Set the Mode of the color sensor
|
|
* This method is used to set the color sensor to one of the three modes,
|
|
* active, passive or raw. The sensor defaults to active mode which uses the
|
|
* internal LED and returns an interpreted color value and 3 8-bit RGB channel
|
|
* values. Raw mode uses the internal LED and returns 3 16-bit RGB channel values.
|
|
* Passive mode disables the internal LED and returns 3 16-bit RGB channel values.
|
|
* @param mode The mode to set
|
|
*/
|
|
void HiTechnicColorSensor::SetMode(tColorMode mode)
|
|
{
|
|
if(m_i2c)
|
|
{
|
|
m_i2c->Write(kModeRegister, (uint8_t)mode);
|
|
}
|
|
}
|
|
|
|
/*
|
|
* Live Window code, only does anything if live window is activated.
|
|
*/
|
|
std::string HiTechnicColorSensor::GetType()
|
|
{
|
|
return "Compass";
|
|
}
|
|
|
|
/**
|
|
* {@inheritDoc}
|
|
*/
|
|
void HiTechnicColorSensor::InitTable(ITable *subtable) {
|
|
m_table = subtable;
|
|
UpdateTable();
|
|
}
|
|
|
|
/**
|
|
* {@inheritDoc}
|
|
*/
|
|
void HiTechnicColorSensor::UpdateTable() {
|
|
if (m_table != NULL) {
|
|
m_table->PutNumber("Value", GetColor());
|
|
NumberArray* rgb = new NumberArray();
|
|
rgb->add(GetRed());
|
|
rgb->add(GetGreen());
|
|
rgb->add(GetBlue());
|
|
m_table->PutValue("RGB", *rgb);
|
|
delete rgb;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* {@inheritDoc}
|
|
*/
|
|
ITable* HiTechnicColorSensor::GetTable()
|
|
{
|
|
return m_table;
|
|
}
|
|
|
|
/**
|
|
* {@inheritDoc}
|
|
*/
|
|
void HiTechnicColorSensor::StartLiveWindowMode()
|
|
{
|
|
|
|
}
|
|
|
|
/**
|
|
* {@inheritDoc}
|
|
*/
|
|
void HiTechnicColorSensor::StopLiveWindowMode()
|
|
{
|
|
|
|
}
|