DigitalModule.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 "DigitalModule.h"
#include "I2C.h"
#include "PWM.h"
#include "Resource.h"
#include "Synchronized.h"
#include "WPIErrors.h"
#include <math.h>
#include <taskLib.h>

static Resource *DIOChannels = NULL;
static Resource *DO_PWMGenerators[tDIO::kNumSystems] = {NULL};

DigitalModule* DigitalModule::GetInstance(uint8_t moduleNumber)
{
        if (CheckDigitalModule(moduleNumber))
        {
                return (DigitalModule *)GetModule(nLoadOut::kModuleType_Digital, moduleNumber);
        }

        // If this wasn't caught before now, make sure we say what's wrong before we crash
        char buf[64];
        snprintf(buf, 64, "Digital Module %d", moduleNumber);
        wpi_setGlobalWPIErrorWithContext(ModuleIndexOutOfRange, buf);

        return NULL;
}

DigitalModule::DigitalModule(uint8_t moduleNumber)
        : Module(nLoadOut::kModuleType_Digital, moduleNumber)
        , m_fpgaDIO (NULL)
{
        Resource::CreateResourceObject(&DIOChannels, tDIO::kNumSystems * kDigitalChannels);
        Resource::CreateResourceObject(&DO_PWMGenerators[m_moduleNumber - 1], tDIO::kNumDO_PWMDutyCycleElements);
        tRioStatusCode localStatus = NiFpga_Status_Success;
        m_fpgaDIO = tDIO::create(m_moduleNumber - 1, &localStatus);
        wpi_setError(localStatus);

        // Make sure that the 9403 IONode has had a chance to initialize before continuing.
        while(m_fpgaDIO->readLoopTiming(&localStatus) == 0) taskDelay(1);
        
        if (m_fpgaDIO->readLoopTiming(&localStatus) != kExpectedLoopTiming)
        {
                char err[128];
                sprintf(err, "DIO LoopTiming: %d, expecting: %lu\n", m_fpgaDIO->readLoopTiming(&localStatus), kExpectedLoopTiming);
                wpi_setWPIErrorWithContext(LoopTimingError, err);
        }
        
    //Calculate the length, in ms, of one DIO loop
    double loopTime = m_fpgaDIO->readLoopTiming(&localStatus)/(kSystemClockTicksPerMicrosecond*1e3);
    
        m_fpgaDIO->writePWMConfig_Period((uint16_t) (PWM::kDefaultPwmPeriod/loopTime + .5), &localStatus);
        m_fpgaDIO->writePWMConfig_MinHigh((uint16_t) ((PWM::kDefaultPwmCenter-PWM::kDefaultPwmStepsDown*loopTime)/loopTime + .5), &localStatus);

        // Ensure that PWM output values are set to OFF
        for (uint32_t pwm_index = 1; pwm_index <= kPwmChannels; pwm_index++)
        {
                SetPWM(pwm_index, PWM::kPwmDisabled);
                SetPWMPeriodScale(pwm_index, 3); // Set all to 4x by default.
        }

        // Turn off all relay outputs.
        m_fpgaDIO->writeSlowValue_RelayFwd(0, &localStatus);
        m_fpgaDIO->writeSlowValue_RelayRev(0, &localStatus);
        wpi_setError(localStatus);

        // Create a semaphore to protect changes to the digital output values
        m_digitalSemaphore = semMCreate(SEM_Q_PRIORITY | SEM_DELETE_SAFE | SEM_INVERSION_SAFE);

        // Create a semaphore to protect changes to the relay values
        m_relaySemaphore = semMCreate(SEM_Q_PRIORITY | SEM_DELETE_SAFE | SEM_INVERSION_SAFE);

        // Create a semaphore to protect changes to the DO PWM config
        m_doPwmSemaphore = semMCreate(SEM_Q_PRIORITY | SEM_DELETE_SAFE | SEM_INVERSION_SAFE);

        AddToSingletonList();
}

DigitalModule::~DigitalModule()
{
        semDelete(m_doPwmSemaphore);
        m_doPwmSemaphore = NULL;
        semDelete(m_relaySemaphore);
        m_relaySemaphore = NULL;
        semDelete(m_digitalSemaphore);
        m_digitalSemaphore = NULL;
        delete m_fpgaDIO;
}

void DigitalModule::SetPWM(uint32_t channel, uint8_t value)
{
        CheckPWMChannel(channel);
        tRioStatusCode localStatus = NiFpga_Status_Success;
        m_fpgaDIO->writePWMValue(channel - 1, value, &localStatus);
        wpi_setError(localStatus);
}

uint8_t DigitalModule::GetPWM(uint32_t channel)
{
        CheckPWMChannel(channel);
        tRioStatusCode localStatus = NiFpga_Status_Success;
        return m_fpgaDIO->readPWMValue(channel - 1, &localStatus);
        wpi_setError(localStatus);
}

void DigitalModule::SetPWMPeriodScale(uint32_t channel, uint32_t squelchMask)
{
        CheckPWMChannel(channel);
        tRioStatusCode localStatus = NiFpga_Status_Success;
        m_fpgaDIO->writePWMPeriodScale(channel - 1, squelchMask, &localStatus);
        wpi_setError(localStatus);
}

void DigitalModule::SetRelayForward(uint32_t channel, bool on)
{
        tRioStatusCode localStatus = NiFpga_Status_Success;
        CheckRelayChannel(channel);
        {
                Synchronized sync(m_relaySemaphore);
                uint8_t forwardRelays = m_fpgaDIO->readSlowValue_RelayFwd(&localStatus);
                if (on)
                        forwardRelays |= 1 << (channel - 1);
                else
                        forwardRelays &= ~(1 << (channel - 1));
                m_fpgaDIO->writeSlowValue_RelayFwd(forwardRelays, &localStatus);
        }
        wpi_setError(localStatus);
}

void DigitalModule::SetRelayReverse(uint32_t channel, bool on)
{
        tRioStatusCode localStatus = NiFpga_Status_Success;
        CheckRelayChannel(channel);
        {
                Synchronized sync(m_relaySemaphore);
                uint8_t reverseRelays = m_fpgaDIO->readSlowValue_RelayRev(&localStatus);
                if (on)
                        reverseRelays |= 1 << (channel - 1);
                else
                        reverseRelays &= ~(1 << (channel - 1));
                m_fpgaDIO->writeSlowValue_RelayRev(reverseRelays, &localStatus);
        }
        wpi_setError(localStatus);
}

bool DigitalModule::GetRelayForward(uint32_t channel)
{
        tRioStatusCode localStatus = NiFpga_Status_Success;
        uint8_t forwardRelays = m_fpgaDIO->readSlowValue_RelayFwd(&localStatus);
        wpi_setError(localStatus);
        return (forwardRelays & (1 << (channel - 1))) != 0;
}

uint8_t DigitalModule::GetRelayForward()
{
        tRioStatusCode localStatus = NiFpga_Status_Success;
        uint8_t forwardRelays = m_fpgaDIO->readSlowValue_RelayFwd(&localStatus);
        wpi_setError(localStatus);
        return forwardRelays;
}

bool DigitalModule::GetRelayReverse(uint32_t channel)
{
        tRioStatusCode localStatus = NiFpga_Status_Success;
        uint8_t reverseRelays = m_fpgaDIO->readSlowValue_RelayRev(&localStatus);
        wpi_setError(localStatus);
        return (reverseRelays & (1 << (channel - 1))) != 0;
        
}

uint8_t DigitalModule::GetRelayReverse()
{
        tRioStatusCode localStatus = NiFpga_Status_Success;
        uint8_t reverseRelays = m_fpgaDIO->readSlowValue_RelayRev(&localStatus);
        wpi_setError(localStatus);
        return reverseRelays;   
}


bool DigitalModule::AllocateDIO(uint32_t channel, bool input)
{
        char buf[64];
        snprintf(buf, 64, "DIO %d (Module %d)", channel, m_moduleNumber);
        if (DIOChannels->Allocate(kDigitalChannels * (m_moduleNumber - 1) + channel - 1, buf) == ~0ul) return false;
        tRioStatusCode localStatus = NiFpga_Status_Success;
        {
                Synchronized sync(m_digitalSemaphore);
                uint32_t bitToSet = 1 << (RemapDigitalChannel(channel - 1));
                uint32_t outputEnable = m_fpgaDIO->readOutputEnable(&localStatus);
                uint32_t outputEnableValue;
                if (input)
                {
                        outputEnableValue = outputEnable & (~bitToSet); // clear the bit for read
                }
                else
                {
                        outputEnableValue = outputEnable | bitToSet; // set the bit for write
                }
                m_fpgaDIO->writeOutputEnable(outputEnableValue, &localStatus);
        }
        wpi_setError(localStatus);
        return true;
}

void DigitalModule::FreeDIO(uint32_t channel)
{
        DIOChannels->Free(kDigitalChannels * (m_moduleNumber - 1) + channel - 1);
}

void DigitalModule::SetDIO(uint32_t channel, short value)
{
        if (value != 0 && value != 1)
        {
                wpi_setWPIError(NonBinaryDigitalValue);
                if (value != 0)
                        value = 1;
        }
        tRioStatusCode localStatus = NiFpga_Status_Success;
        {
                Synchronized sync(m_digitalSemaphore);
                uint16_t currentDIO = m_fpgaDIO->readDO(&localStatus);
                if(value == 0)
                {
                        currentDIO = currentDIO & ~(1 << RemapDigitalChannel(channel - 1));
                }
                else if (value == 1)
                {
                        currentDIO = currentDIO | (1 << RemapDigitalChannel(channel - 1));
                } 
                m_fpgaDIO->writeDO(currentDIO, &localStatus);
        }
        wpi_setError(localStatus);
}

bool DigitalModule::GetDIO(uint32_t channel)
{
        tRioStatusCode localStatus = NiFpga_Status_Success;
        uint32_t currentDIO = m_fpgaDIO->readDI(&localStatus);
        wpi_setError(localStatus);

        //Shift 00000001 over channel-1 places.
        //AND it against the currentDIO
        //if it == 0, then return false
        //else return true
        return ((currentDIO >> RemapDigitalChannel(channel - 1)) & 1) != 0;
}

uint16_t DigitalModule::GetDIO()
{
        tRioStatusCode localStatus = NiFpga_Status_Success;
        uint32_t currentDIO = m_fpgaDIO->readDI(&localStatus);
        wpi_setError(localStatus);
        return currentDIO;
}

bool DigitalModule::GetDIODirection(uint32_t channel)
{
        tRioStatusCode localStatus = NiFpga_Status_Success;
        uint32_t currentOutputEnable = m_fpgaDIO->readOutputEnable(&localStatus);
        wpi_setError(localStatus);
        
        //Shift 00000001 over channel-1 places.
        //AND it against the currentOutputEnable
        //if it == 0, then return false
        //else return true
        return ((currentOutputEnable >> RemapDigitalChannel(channel - 1)) & 1) != 0;
}

uint16_t DigitalModule::GetDIODirection()
{
        tRioStatusCode localStatus = NiFpga_Status_Success;
        uint32_t currentOutputEnable = m_fpgaDIO->readOutputEnable(&localStatus);
        wpi_setError(localStatus);
        return currentOutputEnable;
}

void DigitalModule::Pulse(uint32_t channel, float pulseLength)
{
        uint16_t mask = 1 << RemapDigitalChannel(channel - 1);
        tRioStatusCode localStatus = NiFpga_Status_Success;
        m_fpgaDIO->writePulseLength((uint8_t)(1.0e9 * pulseLength / (m_fpgaDIO->readLoopTiming(&localStatus) * 25)), &localStatus);
        m_fpgaDIO->writePulse(mask, &localStatus);
        wpi_setError(localStatus);
}

bool DigitalModule::IsPulsing(uint32_t channel)
{
        uint16_t mask = 1 << RemapDigitalChannel(channel - 1);
        tRioStatusCode localStatus = NiFpga_Status_Success;
        uint16_t pulseRegister = m_fpgaDIO->readPulse(&localStatus);
        wpi_setError(localStatus);
        return (pulseRegister & mask) != 0;
}

bool DigitalModule::IsPulsing()
{
        tRioStatusCode localStatus = NiFpga_Status_Success;
        uint16_t pulseRegister = m_fpgaDIO->readPulse(&localStatus);
        wpi_setError(localStatus);
        return pulseRegister != 0;
}

uint32_t DigitalModule::AllocateDO_PWM()
{
        char buf[64];
        snprintf(buf, 64, "DO_PWM (Module: %d)", m_moduleNumber);
        return DO_PWMGenerators[(m_moduleNumber - 1)]->Allocate(buf);
}

void DigitalModule::FreeDO_PWM(uint32_t pwmGenerator)
{
        if (pwmGenerator == ~0ul) return;
        DO_PWMGenerators[(m_moduleNumber - 1)]->Free(pwmGenerator);
}

void DigitalModule::SetDO_PWMRate(float rate)
{
        // Currently rounding in the log rate domain... heavy weight toward picking a higher freq.
        // TODO: Round in the linear rate domain.
        tRioStatusCode localStatus = NiFpga_Status_Success;
        uint8_t pwmPeriodPower = (uint8_t)(log(1.0 / (m_fpgaDIO->readLoopTiming(&localStatus) * 0.25E-6 * rate))/log(2.0) + 0.5);
        m_fpgaDIO->writeDO_PWMConfig_PeriodPower(pwmPeriodPower, &localStatus);
        wpi_setError(localStatus);
}

void DigitalModule::SetDO_PWMOutputChannel(uint32_t pwmGenerator, uint32_t channel)
{
        if (pwmGenerator == ~0ul) return;
        tRioStatusCode localStatus = NiFpga_Status_Success;
        switch(pwmGenerator)
        {
        case 0:
                m_fpgaDIO->writeDO_PWMConfig_OutputSelect_0(RemapDigitalChannel(channel - 1), &localStatus);
                break;
        case 1:
                m_fpgaDIO->writeDO_PWMConfig_OutputSelect_1(RemapDigitalChannel(channel - 1), &localStatus);
                break;
        case 2:
                m_fpgaDIO->writeDO_PWMConfig_OutputSelect_2(RemapDigitalChannel(channel - 1), &localStatus);
                break;
        case 3:
                m_fpgaDIO->writeDO_PWMConfig_OutputSelect_3(RemapDigitalChannel(channel - 1), &localStatus);
                break;
        }
        wpi_setError(localStatus);
}

void DigitalModule::SetDO_PWMDutyCycle(uint32_t pwmGenerator, float dutyCycle)
{
        if (pwmGenerator == ~0ul) return;
        if (dutyCycle > 1.0) dutyCycle = 1.0;
        if (dutyCycle < 0.0) dutyCycle = 0.0;
        float rawDutyCycle = 256.0 * dutyCycle;
        if (rawDutyCycle > 255.5) rawDutyCycle = 255.5;
        tRioStatusCode localStatus = NiFpga_Status_Success;
        {
                Synchronized sync(m_doPwmSemaphore);
                uint8_t pwmPeriodPower = m_fpgaDIO->readDO_PWMConfig_PeriodPower(&localStatus);
                if (pwmPeriodPower < 4)
                {
                        // The resolution of the duty cycle drops close to the highest frequencies.
                        rawDutyCycle = rawDutyCycle / pow(2.0, 4 - pwmPeriodPower);
                }
                m_fpgaDIO->writeDO_PWMDutyCycle(pwmGenerator, (uint8_t)rawDutyCycle, &localStatus);
        }
        wpi_setError(localStatus);
}

uint16_t DigitalModule::GetLoopTiming()
{
        tRioStatusCode localStatus = NiFpga_Status_Success;
        uint16_t timing = m_fpgaDIO->readLoopTiming(&localStatus);
        wpi_setError(localStatus);
        
        return timing;
}

I2C* DigitalModule::GetI2C(uint32_t address)
{
        return new I2C(this, address);
}