HiTechnicColorSensor.cpp

/*----------------------------------------------------------------------------*/
/* 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;

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);
                }
                
                nUsageReporting::report(nUsageReporting::kResourceType_HiTechnicColorSensor, moduleNumber - 1);
        }
}

HiTechnicColorSensor::~HiTechnicColorSensor()
{
        delete m_i2c;
        m_i2c = NULL;
}

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;
}

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;
}

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;
}

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;
}

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;
}

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;
}

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;
}

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;
}

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;
}

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";
}

void HiTechnicColorSensor::InitTable(ITable *subtable) {
    m_table = subtable;
    UpdateTable();
}

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;
    }
}

ITable* HiTechnicColorSensor::GetTable()
{
    return m_table;
}

void HiTechnicColorSensor::StartLiveWindowMode()
{
        
}

void HiTechnicColorSensor::StopLiveWindowMode()
{
        
}