DriverStationEnhancedIO.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 "DriverStationEnhancedIO.h"
#include "Synchronized.h"
#include "WPIErrors.h"
#include <strLib.h>

DriverStationEnhancedIO::DriverStationEnhancedIO()
        : m_inputValid (false)
        , m_outputValid (false)
        , m_configChanged (false)
        , m_requestEnhancedEnable (false)
{
        bzero((char*)&m_inputData, sizeof(m_inputData));
        bzero((char*)&m_outputData, sizeof(m_outputData));
        m_outputData.size = sizeof(m_outputData) - 1;
        m_outputData.id = kOutputBlockID;
        // Expected to be active low, so initialize inactive.
        m_outputData.data.fixed_digital_out = 0x3;
        m_inputDataSemaphore = semMCreate(SEM_Q_PRIORITY | SEM_DELETE_SAFE | SEM_INVERSION_SAFE);
        m_outputDataSemaphore = semMCreate(SEM_Q_PRIORITY | SEM_DELETE_SAFE | SEM_INVERSION_SAFE);
        m_encoderOffsets[0] = 0;
        m_encoderOffsets[1] = 0;
}

DriverStationEnhancedIO::~DriverStationEnhancedIO()
{
        semDelete(m_outputDataSemaphore);
        semDelete(m_inputDataSemaphore);
}

void DriverStationEnhancedIO::UpdateData()
{
        INT32 retVal;
        {
                status_block_t tempOutputData;
                Synchronized sync(m_outputDataSemaphore);
                if (m_outputValid || m_configChanged || m_requestEnhancedEnable)
                {
                        m_outputData.flags = kStatusValid;
                        if (m_requestEnhancedEnable)
                        {
                                // Someone called one of the get config APIs, but we are not in enhanced mode.
                                m_outputData.flags |= kForceEnhancedMode;
                        }
                        if (m_configChanged)
                        {
                                if (!m_outputValid)
                                {
                                        // Someone called one of the set config APIs, but we are not in enhanced mode.
                                        m_outputData.flags |= kForceEnhancedMode;
                                }
                                m_outputData.flags |= kStatusConfigChanged;
                        }
                        overrideIOConfig((char*)&m_outputData, 5);
                }
                retVal = getDynamicControlData(kOutputBlockID, (char*)&tempOutputData, sizeof(status_block_t), 5);
                if (retVal == 0)
                {
                        if (m_outputValid)
                        {
                                if (m_configChanged)
                                {
                                        // If our config change made the round trip then clear the flag.
                                        if (IsConfigEqual(tempOutputData, m_outputData))
                                        {
                                                m_configChanged = false;
                                        }
                                }
                                else
                                {
                                        // TODO: This won't work until artf1128 is fixed
                                        //if (tempOutputData.flags & kStatusConfigChanged)
                                        {
                                                // Configuration was updated on the DS, so update our local cache.
                                                MergeConfigIntoOutput(tempOutputData, m_outputData);
                                        }
                                }
                        }
                        else
                        {
                                // Initialize the local cache.
                                MergeConfigIntoOutput(tempOutputData, m_outputData);
                        }
                        m_requestEnhancedEnable = false;
                        m_outputValid = true;
                }
                else
                {
                        m_outputValid = false;
                        m_inputValid = false;
                }
        }
        {
                Synchronized sync(m_inputDataSemaphore);
                control_block_t tempInputData;
                retVal = getDynamicControlData(kInputBlockID, (char*)&tempInputData, sizeof(control_block_t), 5);
                if (retVal == 0 && tempInputData.data.api_version == kSupportedAPIVersion)
                {
                        m_inputData = tempInputData;
                        m_inputValid = true;
                }
                else
                {
                        m_outputValid = false;
                        m_inputValid = false;
                }
        }
}

void DriverStationEnhancedIO::MergeConfigIntoOutput(const status_block_t &dsOutputBlock, status_block_t &localCache)
{
        localCache.data.digital = (localCache.data.digital & dsOutputBlock.data.digital_oe) |
                (dsOutputBlock.data.digital & ~dsOutputBlock.data.digital_oe);
        localCache.data.digital_oe = dsOutputBlock.data.digital_oe;
        localCache.data.digital_pe = dsOutputBlock.data.digital_pe;
        localCache.data.pwm_period[0] = dsOutputBlock.data.pwm_period[0];
        localCache.data.pwm_period[1] = dsOutputBlock.data.pwm_period[1];
        localCache.data.enables = dsOutputBlock.data.enables;
}

bool DriverStationEnhancedIO::IsConfigEqual(const status_block_t &dsOutputBlock, const status_block_t &localCache)
{
        if (localCache.data.digital_oe != dsOutputBlock.data.digital_oe) return false;
        if ((localCache.data.digital & ~dsOutputBlock.data.digital) !=
                (dsOutputBlock.data.digital & ~dsOutputBlock.data.digital)) return false;
        if (localCache.data.digital_pe != dsOutputBlock.data.digital_pe) return false;
        if (localCache.data.pwm_period[0] != dsOutputBlock.data.pwm_period[0]) return false;
        if (localCache.data.pwm_period[1] != dsOutputBlock.data.pwm_period[1]) return false;
        if (localCache.data.enables != dsOutputBlock.data.enables) return false;
        return true;
}

double DriverStationEnhancedIO::GetAcceleration(tAccelChannel channel)
{
        if (channel < 1 || channel > 2)
        {
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 2");
                return 0.0;
        }
        if (!m_inputValid)
        {
                wpi_setWPIError(EnhancedIOMissing);
                return 0.0;
        }
        Synchronized sync(m_inputDataSemaphore);
        return (m_inputData.data.accel[channel] - kAccelOffset) / kAccelScale;
}

double DriverStationEnhancedIO::GetAnalogIn(UINT32 channel)
{
        // 3.3V is the analog reference voltage
        return GetAnalogInRatio(channel) * kAnalogInputReference;
}

double DriverStationEnhancedIO::GetAnalogInRatio(UINT32 channel)
{
        if (channel < 1 || channel > 8)
        {
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 8");
                return 0.0;
        }
        if (!m_inputValid)
        {
                wpi_setWPIError(EnhancedIOMissing);
                return 0.0;
        }
        Synchronized sync(m_inputDataSemaphore);
        return m_inputData.data.analog[channel-1] / kAnalogInputResolution;
}

double DriverStationEnhancedIO::GetAnalogOut(UINT32 channel)
{
        if (channel < 1 || channel > 2)
        {
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 2");
                return 0.0;
        }
        if (!m_outputValid)
        {
                wpi_setWPIError(EnhancedIOMissing);
                return 0.0;
        }

        Synchronized sync(m_outputDataSemaphore);
        return m_outputData.data.dac[channel-1] * kAnalogOutputReference / kAnalogOutputResolution;
}

void DriverStationEnhancedIO::SetAnalogOut(UINT32 channel, double value)
{
        if (channel < 1 || channel > 2)
        {
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 2");
                return;
        }
        if (!m_outputValid)
        {
                wpi_setWPIError(EnhancedIOMissing);
                return;
        }
        if (value < 0.0) value = 0.0;
        if (value > kAnalogOutputReference) value = kAnalogOutputReference;

        Synchronized sync(m_outputDataSemaphore);
        m_outputData.data.dac[channel-1] = (UINT8)(value / kAnalogOutputReference * kAnalogOutputResolution);
}

bool DriverStationEnhancedIO::GetButton(UINT32 channel)
{
        if (channel < 1 || channel > 6)
        {
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 6");
                return false;
        }

        return ((GetButtons() >> (channel-1)) & 1) != 0;
}

UINT8 DriverStationEnhancedIO::GetButtons()
{
        if (!m_inputValid)
        {
                wpi_setWPIError(EnhancedIOMissing);
                return 0;
        }
        Synchronized sync(m_inputDataSemaphore);
        return m_inputData.data.buttons;
}

void DriverStationEnhancedIO::SetLED(UINT32 channel, bool value)
{
        if (channel < 1 || channel > 8)
        {
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 8");
                return;
        }
        if (!m_outputValid)
        {
                wpi_setWPIError(EnhancedIOMissing);
                return;
        }
        UINT8 leds;
        Synchronized sync(m_outputDataSemaphore);
        leds = m_outputData.data.leds;

        leds &= ~(1 << (channel-1));
        if (value) leds |= 1 << (channel-1);

        m_outputData.data.leds = leds;
}

void DriverStationEnhancedIO::SetLEDs(UINT8 value)
{
        if (!m_outputValid)
        {
                wpi_setWPIError(EnhancedIOMissing);
                return;
        }
        Synchronized sync(m_outputDataSemaphore);
        m_outputData.data.leds = value;
}

bool DriverStationEnhancedIO::GetDigital(UINT32 channel)
{
        if (channel < 1 || channel > 16)
        {
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 16");
                return false;
        }

        return ((GetDigitals() >> (channel-1)) & 1) != 0;
}

UINT16 DriverStationEnhancedIO::GetDigitals()
{
        if (!m_inputValid)
        {
                wpi_setWPIError(EnhancedIOMissing);
                return 0;
        }
        Synchronized sync(m_inputDataSemaphore);
        return m_inputData.data.digital;
}

void DriverStationEnhancedIO::SetDigitalOutput(UINT32 channel, bool value)
{
        if (channel < 1 || channel > 16)
        {
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 16");
                return;
        }
        if (!m_outputValid)
        {
                wpi_setWPIError(EnhancedIOMissing);
                return;
        }
        UINT16 digital;
        Synchronized sync(m_outputDataSemaphore);

        if (m_outputData.data.digital_oe & (1 << (channel-1)))
        {
                digital = m_outputData.data.digital;
        
                digital &= ~(1 << (channel-1));
                if (value) digital |= 1 << (channel-1);
        
                m_outputData.data.digital = digital;
        }
        else
        {
                wpi_setWPIError(LineNotOutput);
        }
}

DriverStationEnhancedIO::tDigitalConfig DriverStationEnhancedIO::GetDigitalConfig(UINT32 channel)
{
        if (channel < 1 || channel > 16)
        {
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 16");
                return kUnknown;
        }
        if (!m_outputValid)
        {
                m_requestEnhancedEnable = true;
                wpi_setWPIError(EnhancedIOMissing);
                return kUnknown;
        }
        Synchronized sync(m_outputDataSemaphore);
        if ((channel >= 1) && (channel <= 4))
        {
                if (m_outputData.data.pwm_enable & (1 << (channel - 1)))
                {
                        return kPWM;
                }
        }
        if ((channel >= 15) && (channel <= 16))
        {
                if (m_outputData.data.comparator_enable & (1 << (channel - 15)))
                {
                        return kAnalogComparator;
                }
        }
        if (m_outputData.data.digital_oe & (1 << (channel - 1)))
        {
                return kOutput;
        }
        if (!(m_outputData.data.digital_pe & (1 << (channel - 1))))
        {
                return kInputFloating;
        }
        if (m_outputData.data.digital & (1 << (channel - 1)))
        {
                return kInputPullUp;
        }
        else
        {
                return kInputPullDown;
        }
}

void DriverStationEnhancedIO::SetDigitalConfig(UINT32 channel, tDigitalConfig config)
{
        if (channel < 1 || channel > 16)
        {
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 16");
                return;
        }
        if (config == kPWM && (channel < 1 || channel > 4))
        {
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel in PWM mode must be between 1 and 4");
                return;
        }
        if (config == kAnalogComparator && (channel < 15 || channel > 16))
        {
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel in Analog Comparator mode must be between 15 and 16");
                return;
        }

        Synchronized sync(m_outputDataSemaphore);
        m_configChanged = true;

        if ((channel >= 1) && (channel <= 4))
        {
                if (config == kPWM)
                {
                        m_outputData.data.pwm_enable |= 1 << (channel - 1);
                        m_outputData.data.digital &= ~(1 << (channel - 1));
                        m_outputData.data.digital_oe |= 1 << (channel - 1);
                        m_outputData.data.digital_pe &= ~(1 << (channel - 1));
                        return;
                }
                else
                {
                        m_outputData.data.pwm_enable &= ~(1 << (channel - 1));
                }
        }
        else if ((channel >= 15) && (channel <= 16))
        {
                if (config == kAnalogComparator)
                {
                        m_outputData.data.comparator_enable |= 1 << (channel - 15);
                        m_outputData.data.digital &= ~(1 << (channel - 1));
                        m_outputData.data.digital_oe &= ~(1 << (channel - 1));
                        m_outputData.data.digital_pe &= ~(1 << (channel - 1));
                        return;
                }
                else
                {
                        m_outputData.data.comparator_enable &= ~(1 << (channel - 15));
                }
        }
        if (config == kInputFloating)
        {
                m_outputData.data.digital &= ~(1 << (channel - 1));
                m_outputData.data.digital_oe &= ~(1 << (channel - 1));
                m_outputData.data.digital_pe &= ~(1 << (channel - 1));
        }
        else if (config == kInputPullUp)
        {
                m_outputData.data.digital |= 1 << (channel - 1);
                m_outputData.data.digital_oe &= ~(1 << (channel - 1));
                m_outputData.data.digital_pe |= 1 << (channel - 1);
        }
        else if (config == kInputPullDown)
        {
                m_outputData.data.digital &= ~(1 << (channel - 1));
                m_outputData.data.digital_oe &= ~(1 << (channel - 1));
                m_outputData.data.digital_pe |= 1 << (channel - 1);
        }
        else if (config == kOutput)
        {
                m_outputData.data.digital_oe |= 1 << (channel - 1);
                m_outputData.data.digital_pe &= ~(1 << (channel - 1));
        }
        else
        {
                // Something went wrong.
        }
}

double DriverStationEnhancedIO::GetPWMPeriod(tPWMPeriodChannels channels)
{
        if (channels < kPWMChannels1and2 || channels > kPWMChannels3and4)
        {
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "channels must be kPWMChannels1and2 or kPWMChannels3and4");
                return 0.0;
        }
        if (!m_outputValid)
        {
                m_requestEnhancedEnable = true;
                wpi_setWPIError(EnhancedIOMissing);
                return 0.0;
        }

        Synchronized sync(m_outputDataSemaphore);
        return m_outputData.data.pwm_period[channels] / 24000000.0;
}

void DriverStationEnhancedIO::SetPWMPeriod(tPWMPeriodChannels channels, double period)
{
        if (channels < kPWMChannels1and2 || channels > kPWMChannels3and4)
        {
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "channels must be kPWMChannels1and2 or kPWMChannels3and4");
                return;
        }

        // Convert to ticks based on the IO board's 24MHz clock
        double ticks = period * 24000000.0;
        // Limit the range of the ticks... warn if too big.
        if (ticks > 65534.0) 
        {
                wpi_setWPIError(EnhancedIOPWMPeriodOutOfRange);
                ticks = 65534.0;
        }
        else if (ticks < 0.0) ticks = 0.0;
        // Preserve the duty cycles.
        double dutyCycles[2];
        dutyCycles[0] = GetPWMOutput((channels << 1) + 1);
        dutyCycles[1] = GetPWMOutput((channels << 1) + 2);
        {
                Synchronized sync(m_outputDataSemaphore);
                // Update the period
                m_outputData.data.pwm_period[channels] = (UINT16)ticks;
                m_configChanged = true;
        }
        // Restore the duty cycles
        SetPWMOutput((channels << 1) + 1, dutyCycles[0]);
        SetPWMOutput((channels << 1) + 2, dutyCycles[1]);
}

bool DriverStationEnhancedIO::GetFixedDigitalOutput(UINT32 channel)
{
        if (channel < 1 || channel > 2)
        {
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 2");
                return 0;
        }
        if (!m_outputValid)
        {
                wpi_setWPIError(EnhancedIOMissing);
                return 0;
        }

        Synchronized sync(m_outputDataSemaphore);
        return ((m_outputData.data.fixed_digital_out >> (channel-1)) & 1) != 0;
}

void DriverStationEnhancedIO::SetFixedDigitalOutput(UINT32 channel, bool value)
{
        if (channel < 1 || channel > 2)
        {
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 2");
                return;
        }
        if (!m_outputValid)
        {
                wpi_setWPIError(EnhancedIOMissing);
                return;
        }

        UINT8 digital;
        Synchronized sync(m_outputDataSemaphore);
        digital = m_outputData.data.fixed_digital_out;

        digital &= ~(1 << (channel-1));
        if (value) digital |= 1 << (channel-1);

        m_outputData.data.fixed_digital_out = digital;
}

INT16 DriverStationEnhancedIO::GetEncoder(UINT32 encoderNumber)
{
        if (encoderNumber < 1 || encoderNumber > 2)
        {
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "encoderNumber must be between 1 and 2");
                return 0;
        }
        if (!m_inputValid)
        {
                wpi_setWPIError(EnhancedIOMissing);
                return 0;
        }

        Synchronized sync(m_inputDataSemaphore);
        return m_inputData.data.quad[encoderNumber - 1] - m_encoderOffsets[encoderNumber - 1];
}

void DriverStationEnhancedIO::ResetEncoder(UINT32 encoderNumber)
{
        if (encoderNumber < 1 || encoderNumber > 2)
        {
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "encoderNumber must be between 1 and 2");
                return;
        }
        if (!m_inputValid)
        {
                wpi_setWPIError(EnhancedIOMissing);
                return;
        }

        Synchronized sync(m_inputDataSemaphore);
        m_encoderOffsets[encoderNumber - 1] = m_inputData.data.quad[encoderNumber - 1];
}

bool DriverStationEnhancedIO::GetEncoderIndexEnable(UINT32 encoderNumber)
{
        if (encoderNumber < 1 || encoderNumber > 2)
        {
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "encoderNumber must be between 1 and 2");
                return false;
        }
        if (!m_outputValid)
        {
                m_requestEnhancedEnable = true;
                wpi_setWPIError(EnhancedIOMissing);
                return false;
        }

        Synchronized sync(m_outputDataSemaphore);
        return ((m_outputData.data.quad_index_enable >> (encoderNumber - 1)) & 1) != 0;
}

void DriverStationEnhancedIO::SetEncoderIndexEnable(UINT32 encoderNumber, bool enable)
{
        if (encoderNumber < 1 || encoderNumber > 2)
        {
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "encoderNumber must be between 1 and 2");
                return;
        }

        Synchronized sync(m_outputDataSemaphore);
        m_outputData.data.quad_index_enable &= ~(1 << (encoderNumber - 1));
        if (enable) m_outputData.data.quad_index_enable |= 1 << (encoderNumber - 1);
        m_configChanged = true;
}

double DriverStationEnhancedIO::GetTouchSlider()
{
        if (!m_inputValid)
        {
                wpi_setWPIError(EnhancedIOMissing);
                return 0.0;
        }

        Synchronized sync(m_inputDataSemaphore);
        UINT8 value = m_inputData.data.capsense_slider;
        return value == 255 ? -1.0 : value / 254.0;
}

double DriverStationEnhancedIO::GetPWMOutput(UINT32 channel)
{
        if (channel < 1 || channel > 4)
        {
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 4");
                return 0.0;
        }
        if (!m_outputValid)
        {
                wpi_setWPIError(EnhancedIOMissing);
                return 0.0;
        }

        Synchronized sync(m_outputDataSemaphore);
        return (double)m_outputData.data.pwm_compare[channel - 1] / (double)m_outputData.data.pwm_period[(channel - 1) >> 1];
}

void DriverStationEnhancedIO::SetPWMOutput(UINT32 channel, double value)
{
        if (channel < 1 || channel > 4)
        {
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 4");
                return;
        }
        if (!m_outputValid)
        {
                wpi_setWPIError(EnhancedIOMissing);
                return;
        }

        if (value > 1.0) value = 1.0;
        else if (value < 0.0) value = 0.0;
        Synchronized sync(m_outputDataSemaphore);
        m_outputData.data.pwm_compare[channel - 1] = (UINT16)(value * (double)m_outputData.data.pwm_period[(channel - 1) >> 1]);
}

UINT8 DriverStationEnhancedIO::GetFirmwareVersion()
{
        if (!m_inputValid)
        {
                m_requestEnhancedEnable = true;
                wpi_setWPIError(EnhancedIOMissing);
                return 0;
        }

        Synchronized sync(m_inputDataSemaphore);
        return m_inputData.data.fw_version;
}