Counter.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 "Counter.h"
#include "AnalogTrigger.h"
#include "DigitalInput.h"
#include "Resource.h"
#include "WPIErrors.h"

static Resource *counters = NULL;

void Counter::InitCounter(Mode mode)
{
        Resource::CreateResourceObject(&counters, tCounter::kNumSystems);
        UINT32 index = counters->Allocate("Counter");
        if (index == ~0ul)
        {
                CloneError(counters);
                return;
        }
        m_index = index;
        tRioStatusCode localStatus = NiFpga_Status_Success;
        m_counter = tCounter::create(m_index, &localStatus);
        m_counter->writeConfig_Mode(mode, &localStatus);
        m_upSource = NULL;
        m_downSource = NULL;
        m_allocatedUpSource = false;
        m_allocatedDownSource = false;
        m_counter->writeTimerConfig_AverageSize(1, &localStatus);
        wpi_setError(localStatus);
}

Counter::Counter() :
        m_upSource(NULL),
        m_downSource(NULL),
        m_counter(NULL)
{
        InitCounter();
}

Counter::Counter(DigitalSource *source) :
        m_upSource(NULL),
        m_downSource(NULL),
        m_counter(NULL)
{
        InitCounter();
        SetUpSource(source);
        ClearDownSource();
}

Counter::Counter(DigitalSource &source) :
        m_upSource(NULL),
        m_downSource(NULL),
        m_counter(NULL)
{
        InitCounter();
        SetUpSource(&source);
        ClearDownSource();
}

Counter::Counter(UINT32 channel) :
        m_upSource(NULL),
        m_downSource(NULL),
        m_counter(NULL)
{
        InitCounter();
        SetUpSource(channel);
        ClearDownSource();
}

Counter::Counter(UINT8 moduleNumber, UINT32 channel) :
        m_upSource(NULL),
        m_downSource(NULL),
        m_counter(NULL)
{
        InitCounter();
        SetUpSource(moduleNumber, channel);
        ClearDownSource();
}

Counter::Counter(AnalogTrigger *trigger) :
        m_upSource(NULL),
        m_downSource(NULL),
        m_counter(NULL)
{
        InitCounter();
        SetUpSource(trigger->CreateOutput(AnalogTriggerOutput::kState));
        ClearDownSource();
        m_allocatedUpSource = true;
}

Counter::Counter(AnalogTrigger &trigger) :
        m_upSource(NULL),
        m_downSource(NULL),
        m_counter(NULL)
{
        InitCounter();
        SetUpSource(trigger.CreateOutput(AnalogTriggerOutput::kState));
        ClearDownSource();
        m_allocatedUpSource = true;
}

Counter::Counter(EncodingType encodingType, DigitalSource *upSource, DigitalSource *downSource, bool inverted) :
        m_upSource(NULL),
        m_downSource(NULL),
        m_counter(NULL)
{
        if (encodingType != k1X && encodingType != k2X)
        {
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "Counter only supports 1X and 2X quadrature decoding.");
                return;
        }
        InitCounter(kExternalDirection);
        SetUpSource(upSource);
        SetDownSource(downSource);
        tRioStatusCode localStatus = NiFpga_Status_Success;

        if (encodingType == k1X)
        {
                SetUpSourceEdge(true, false);
                m_counter->writeTimerConfig_AverageSize(1, &localStatus);
        }
        else
        {
                SetUpSourceEdge(true, true);
                m_counter->writeTimerConfig_AverageSize(2, &localStatus);
        }

        wpi_setError(localStatus);
        SetDownSourceEdge(inverted, true);
}

Counter::~Counter()
{
        SetUpdateWhenEmpty(true);
        if (m_allocatedUpSource)
        {
                delete m_upSource;
                m_upSource = NULL;
        }
        if (m_allocatedDownSource)
        {
                delete m_downSource;
                m_downSource = NULL;
        }
        delete m_counter;
        m_counter = NULL;
        counters->Free(m_index);
}

void Counter::SetUpSource(UINT8 moduleNumber, UINT32 channel)
{
        if (StatusIsFatal()) return;
        SetUpSource(new DigitalInput(moduleNumber, channel));
        m_allocatedUpSource = true;
}

void Counter::SetUpSource(UINT32 channel)
{
        if (StatusIsFatal()) return;
        SetUpSource(GetDefaultDigitalModule(), channel);
        m_allocatedUpSource = true;
}

void Counter::SetUpSource(AnalogTrigger *analogTrigger, AnalogTriggerOutput::Type triggerType)
{
        if (StatusIsFatal()) return;
        SetUpSource(analogTrigger->CreateOutput(triggerType));
        m_allocatedUpSource = true;
}

void Counter::SetUpSource(AnalogTrigger &analogTrigger, AnalogTriggerOutput::Type triggerType)
{
        SetUpSource(&analogTrigger, triggerType);
}

void Counter::SetUpSource(DigitalSource *source)
{
        if (StatusIsFatal()) return;
        if (m_allocatedUpSource)
        {
                delete m_upSource;
                m_upSource = NULL;
                m_allocatedUpSource = false;
        }
        m_upSource = source;
        if (m_upSource->StatusIsFatal())
        {
                CloneError(m_upSource);
        }
        else
        {
                tRioStatusCode localStatus = NiFpga_Status_Success;
                m_counter->writeConfig_UpSource_Module(source->GetModuleForRouting(), &localStatus);
                m_counter->writeConfig_UpSource_Channel(source->GetChannelForRouting(), &localStatus);
                m_counter->writeConfig_UpSource_AnalogTrigger(source->GetAnalogTriggerForRouting(), &localStatus);
        
                if(m_counter->readConfig_Mode(&localStatus) == kTwoPulse ||
                                m_counter->readConfig_Mode(&localStatus) == kExternalDirection)
                {
                        SetUpSourceEdge(true, false);
                }
                m_counter->strobeReset(&localStatus);
                wpi_setError(localStatus);
        }
}

void Counter::SetUpSource(DigitalSource &source)
{
        SetUpSource(&source);
}

void Counter::SetUpSourceEdge(bool risingEdge, bool fallingEdge)
{
        if (StatusIsFatal()) return;
        if (m_upSource == NULL)
        {
                wpi_setWPIErrorWithContext(NullParameter, "Must set non-NULL UpSource before setting UpSourceEdge");
        }
        tRioStatusCode localStatus = NiFpga_Status_Success;
        m_counter->writeConfig_UpRisingEdge(risingEdge, &localStatus);
        m_counter->writeConfig_UpFallingEdge(fallingEdge, &localStatus);
        wpi_setError(localStatus);
}

void Counter::ClearUpSource()
{
        if (StatusIsFatal()) return;
        if (m_allocatedUpSource)
        {
                delete m_upSource;
                m_upSource = NULL;
                m_allocatedUpSource = false;
        }
        tRioStatusCode localStatus = NiFpga_Status_Success;
        bool state = m_counter->readConfig_Enable(&localStatus);
        m_counter->writeConfig_Enable(false, &localStatus);
        m_counter->writeConfig_UpFallingEdge(false, &localStatus);
        m_counter->writeConfig_UpRisingEdge(false, &localStatus);
        // Index 0 of digital is always 0.
        m_counter->writeConfig_UpSource_Channel(0, &localStatus);
        m_counter->writeConfig_UpSource_AnalogTrigger(false, &localStatus);
        m_counter->writeConfig_Enable(state, &localStatus);
        wpi_setError(localStatus);
}

void Counter::SetDownSource(UINT32 channel)
{
        if (StatusIsFatal()) return;
        SetDownSource(new DigitalInput(channel));
        m_allocatedDownSource = true;
}

void Counter::SetDownSource(UINT8 moduleNumber, UINT32 channel)
{
        if (StatusIsFatal()) return;
        SetDownSource(new DigitalInput(moduleNumber, channel));
        m_allocatedDownSource = true;
}

void Counter::SetDownSource(AnalogTrigger *analogTrigger, AnalogTriggerOutput::Type triggerType)
{
        if (StatusIsFatal()) return;
        SetDownSource(analogTrigger->CreateOutput(triggerType));
        m_allocatedDownSource = true;
}

void Counter::SetDownSource(AnalogTrigger &analogTrigger, AnalogTriggerOutput::Type triggerType)
{
        SetDownSource(&analogTrigger, triggerType);
}

void Counter::SetDownSource(DigitalSource *source)
{
        if (StatusIsFatal()) return;
        if (m_allocatedDownSource)
        {
                delete m_downSource;
                m_downSource = NULL;
                m_allocatedDownSource = false;
        }
        m_downSource = source;
        if (m_downSource->StatusIsFatal())
        {
                CloneError(m_downSource);
        }
        else
        {
                tRioStatusCode localStatus = NiFpga_Status_Success;
                unsigned char mode = m_counter->readConfig_Mode(&localStatus);
                if (mode != kTwoPulse && mode != kExternalDirection)
                {
                        wpi_setWPIErrorWithContext(ParameterOutOfRange, "Counter only supports DownSource in TwoPulse and ExternalDirection modes.");
                        return;
                }
                m_counter->writeConfig_DownSource_Module(source->GetModuleForRouting(), &localStatus);
                m_counter->writeConfig_DownSource_Channel(source->GetChannelForRouting(), &localStatus);
                m_counter->writeConfig_DownSource_AnalogTrigger(source->GetAnalogTriggerForRouting(), &localStatus);
        
                SetDownSourceEdge(true, false);
                m_counter->strobeReset(&localStatus);
                wpi_setError(localStatus);
        }
}

void Counter::SetDownSource(DigitalSource &source)
{
        SetDownSource(&source);
}

void Counter::SetDownSourceEdge(bool risingEdge, bool fallingEdge)
{
        if (StatusIsFatal()) return;
        if (m_downSource == NULL)
        {
                wpi_setWPIErrorWithContext(NullParameter, "Must set non-NULL DownSource before setting DownSourceEdge");
        }
        tRioStatusCode localStatus = NiFpga_Status_Success;
        m_counter->writeConfig_DownRisingEdge(risingEdge, &localStatus);
        m_counter->writeConfig_DownFallingEdge(fallingEdge, &localStatus);
        wpi_setError(localStatus);
}

void Counter::ClearDownSource()
{
        if (StatusIsFatal()) return;
        if (m_allocatedDownSource)
        {
                delete m_downSource;
                m_downSource = NULL;
                m_allocatedDownSource = false;
        }
        tRioStatusCode localStatus = NiFpga_Status_Success;
        bool state = m_counter->readConfig_Enable(&localStatus);
        m_counter->writeConfig_Enable(false, &localStatus);
        m_counter->writeConfig_DownFallingEdge(false, &localStatus);
        m_counter->writeConfig_DownRisingEdge(false, &localStatus);
        // Index 0 of digital is always 0.
        m_counter->writeConfig_DownSource_Channel(0, &localStatus);
        m_counter->writeConfig_DownSource_AnalogTrigger(false, &localStatus);
        m_counter->writeConfig_Enable(state, &localStatus);
        wpi_setError(localStatus);
}

void Counter::SetUpDownCounterMode()
{
        if (StatusIsFatal()) return;
        tRioStatusCode localStatus = NiFpga_Status_Success;
        m_counter->writeConfig_Mode(kTwoPulse, &localStatus);
        wpi_setError(localStatus);
}

void Counter::SetExternalDirectionMode()
{
        if (StatusIsFatal()) return;
        tRioStatusCode localStatus = NiFpga_Status_Success;
        m_counter->writeConfig_Mode(kExternalDirection, &localStatus);
        wpi_setError(localStatus);
}

void Counter::SetSemiPeriodMode(bool highSemiPeriod)
{
        if (StatusIsFatal()) return;
        tRioStatusCode localStatus = NiFpga_Status_Success;
        m_counter->writeConfig_Mode(kSemiperiod, &localStatus);
        m_counter->writeConfig_UpRisingEdge(highSemiPeriod, &localStatus);
        SetUpdateWhenEmpty(false);
        wpi_setError(localStatus);
}

void Counter::SetPulseLengthMode(float threshold)
{
        if (StatusIsFatal()) return;
        tRioStatusCode localStatus = NiFpga_Status_Success;
        m_counter->writeConfig_Mode(kPulseLength, &localStatus);
        m_counter->writeConfig_PulseLengthThreshold((UINT32)(threshold * 1.0e6) * kSystemClockTicksPerMicrosecond, &localStatus);
        wpi_setError(localStatus);
}

void Counter::Start()
{
        if (StatusIsFatal()) return;
        tRioStatusCode localStatus = NiFpga_Status_Success;
        m_counter->writeConfig_Enable(1, &localStatus);
        wpi_setError(localStatus);
}

INT32 Counter::Get()
{
        if (StatusIsFatal()) return 0;
        tRioStatusCode localStatus = NiFpga_Status_Success;
        INT32 value = m_counter->readOutput_Value(&localStatus);
        wpi_setError(localStatus);
        return value;
}

void Counter::Reset()
{
        if (StatusIsFatal()) return;
        tRioStatusCode localStatus = NiFpga_Status_Success;
        m_counter->strobeReset(&localStatus);
        wpi_setError(localStatus);
}

void Counter::Stop()
{
        if (StatusIsFatal()) return;
        tRioStatusCode localStatus = NiFpga_Status_Success;
        m_counter->writeConfig_Enable(0, &localStatus);
        wpi_setError(localStatus);
}

/*
 * Get the Period of the most recent count.
 * Returns the time interval of the most recent count. This can be used for velocity calculations
 * to determine shaft speed.
 * @returns The period of the last two pulses in units of seconds.
 */
double Counter::GetPeriod()
{
        if (StatusIsFatal()) return 0.0;
        tRioStatusCode localStatus = NiFpga_Status_Success;
        tCounter::tTimerOutput output = m_counter->readTimerOutput(&localStatus);
        double period;
        if (output.Stalled)
        {
                // Return infinity
                double zero = 0.0;
                period = 1.0 / zero;
        }
        else
        {
                // output.Period is a fixed point number that counts by 2 (24 bits, 25 integer bits)
                period = (double)(output.Period << 1) / (double)output.Count;
        }
        wpi_setError(localStatus);
        return period * 1.0e-6;
}

void Counter::SetMaxPeriod(double maxPeriod)
{
        if (StatusIsFatal()) return;
        tRioStatusCode localStatus = NiFpga_Status_Success;
        m_counter->writeTimerConfig_StallPeriod((UINT32)(maxPeriod * 1.0e6), &localStatus);
        wpi_setError(localStatus);
}

void Counter::SetUpdateWhenEmpty(bool enabled)
{
        if (StatusIsFatal()) return;
        tRioStatusCode localStatus = NiFpga_Status_Success;
        m_counter->writeTimerConfig_UpdateWhenEmpty(enabled, &localStatus);
        wpi_setError(localStatus);
}

bool Counter::GetStopped()
{
        if (StatusIsFatal()) return false;
        tRioStatusCode localStatus = NiFpga_Status_Success;
        return m_counter->readTimerOutput_Stalled(&localStatus);
        wpi_setError(localStatus);
}

bool Counter::GetDirection()
{
        if (StatusIsFatal()) return false;
        tRioStatusCode localStatus = NiFpga_Status_Success;
        bool value = m_counter->readOutput_Direction(&localStatus);
        wpi_setError(localStatus);
        return value;
}

void Counter::SetReverseDirection(bool reverseDirection)
{
        if (StatusIsFatal()) return;
        tRioStatusCode localStatus = NiFpga_Status_Success;
        if (m_counter->readConfig_Mode(&localStatus) == kExternalDirection)
        {
                if (reverseDirection)
                        SetDownSourceEdge(true, true);
                else
                        SetDownSourceEdge(false, true);
        }
        wpi_setError(localStatus);
}