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

static Resource *quadEncoders = NULL;

void Encoder::InitEncoder(bool reverseDirection, EncodingType encodingType)
{
        m_encodingType = encodingType;
        tRioStatusCode localStatus = NiFpga_Status_Success;
        switch (encodingType)
        {
        case k4X:
                Resource::CreateResourceObject(&quadEncoders, tEncoder::kNumSystems);
                UINT32 index = quadEncoders->Allocate("4X Encoder");
                if (index == ~0ul)
                {
                        CloneError(quadEncoders);
                        return;
                }
                if (m_aSource->StatusIsFatal())
                {
                        CloneError(m_aSource);
                        return;
                }
                if (m_bSource->StatusIsFatal())
                {
                        CloneError(m_bSource);
                        return;
                }
                m_index = index;
                m_encoder = tEncoder::create(m_index, &localStatus);
                m_encoder->writeConfig_ASource_Module(m_aSource->GetModuleForRouting(), &localStatus);
                m_encoder->writeConfig_ASource_Channel(m_aSource->GetChannelForRouting(), &localStatus);
                m_encoder->writeConfig_ASource_AnalogTrigger(m_aSource->GetAnalogTriggerForRouting(), &localStatus);
                m_encoder->writeConfig_BSource_Module(m_bSource->GetModuleForRouting(), &localStatus);
                m_encoder->writeConfig_BSource_Channel(m_bSource->GetChannelForRouting(), &localStatus);
                m_encoder->writeConfig_BSource_AnalogTrigger(m_bSource->GetAnalogTriggerForRouting(), &localStatus);
                m_encoder->strobeReset(&localStatus);
                m_encoder->writeConfig_Reverse(reverseDirection, &localStatus);
                m_encoder->writeTimerConfig_AverageSize(4, &localStatus);
                m_counter = NULL;
                break;
        case k1X:
        case k2X:
                m_counter = new Counter(m_encodingType, m_aSource, m_bSource, reverseDirection);
                break;
        }
        m_distancePerPulse = 1.0;
        m_pidSource = kDistance;
        wpi_setError(localStatus);
}

Encoder::Encoder(UINT8 aModuleNumber, UINT32 aChannel,
                                                UINT8 bModuleNumber, UINT32 bChannel,
                                                bool reverseDirection, EncodingType encodingType) :
        m_encoder(NULL),
        m_counter(NULL)
{
        m_aSource = new DigitalInput(aModuleNumber, aChannel);
        m_bSource = new DigitalInput(bModuleNumber, bChannel);
        InitEncoder(reverseDirection, encodingType);
        m_allocatedASource = true;
        m_allocatedBSource = true;
}

Encoder::Encoder(UINT32 aChannel, UINT32 bChannel, bool reverseDirection, EncodingType encodingType) :
        m_encoder(NULL),
        m_counter(NULL)
{
        m_aSource = new DigitalInput(aChannel);
        m_bSource = new DigitalInput(bChannel);
        InitEncoder(reverseDirection, encodingType);
        m_allocatedASource = true;
        m_allocatedBSource = true;
}

Encoder::Encoder(DigitalSource *aSource, DigitalSource *bSource, bool reverseDirection, EncodingType encodingType) :
        m_encoder(NULL),
        m_counter(NULL)
{
        m_aSource = aSource;
        m_bSource = bSource;
        m_allocatedASource = false;
        m_allocatedBSource = false;
        if (m_aSource == NULL || m_bSource == NULL)
                wpi_setWPIError(NullParameter);
        else
                InitEncoder(reverseDirection, encodingType);
}

Encoder::Encoder(DigitalSource &aSource, DigitalSource &bSource, bool reverseDirection, EncodingType encodingType) :
        m_encoder(NULL),
        m_counter(NULL)
{
        m_aSource = &aSource;
        m_bSource = &bSource;
        m_allocatedASource = false;
        m_allocatedBSource = false;
        InitEncoder(reverseDirection, encodingType);
}

Encoder::~Encoder()
{
        if (m_allocatedASource) delete m_aSource;
        if (m_allocatedBSource) delete m_bSource;
        if (m_counter)
        {
                delete m_counter;
        }
        else
        {
                quadEncoders->Free(m_index);
                delete m_encoder;
        }
}

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

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

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

INT32 Encoder::Get()
{
        if (StatusIsFatal()) return 0;
        return (INT32) (GetRaw() * DecodingScaleFactor());
}

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

double Encoder::GetPeriod()
{
        if (StatusIsFatal()) return 0.0;
        double measuredPeriod;
        if (m_counter)
        {
                measuredPeriod = m_counter->GetPeriod();
        }
        else
        {
                tRioStatusCode localStatus = NiFpga_Status_Success;
                tEncoder::tTimerOutput output = m_encoder->readTimerOutput(&localStatus);
                double value;
                if (output.Stalled)
                {
                        // Return infinity
                        double zero = 0.0;
                        value = 1.0 / zero;
                }
                else
                {
                        // output.Period is a fixed point number that counts by 2 (24 bits, 25 integer bits)
                        value = (double)(output.Period << 1) / (double)output.Count;
                }
                wpi_setError(localStatus);
                measuredPeriod = value * 1.0e-6;
        }
        return measuredPeriod / DecodingScaleFactor();
}

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

bool Encoder::GetStopped()
{
        if (StatusIsFatal()) return true;
        if (m_counter)
        {
                return m_counter->GetStopped();
        }
        else
        {
                tRioStatusCode localStatus = NiFpga_Status_Success;
                bool value = m_encoder->readTimerOutput_Stalled(&localStatus) != 0;
                wpi_setError(localStatus);
                return value;
        }
}

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

double Encoder::DecodingScaleFactor()
{
        if (StatusIsFatal()) return 0.0;
        switch (m_encodingType)
        {
        case k1X:
                return 1.0;
        case k2X:
                return 0.5;
        case k4X:
                return 0.25;
        default:
                return 0.0;
        }       
}

double Encoder::GetDistance()
{
        if (StatusIsFatal()) return 0.0;
        return GetRaw() * DecodingScaleFactor() * m_distancePerPulse;
}

double Encoder::GetRate()
{
        if (StatusIsFatal()) return 0.0;
        return (m_distancePerPulse / GetPeriod());
}

void Encoder::SetMinRate(double minRate)
{
        if (StatusIsFatal()) return;
        SetMaxPeriod(m_distancePerPulse / minRate);
}

void Encoder::SetDistancePerPulse(double distancePerPulse)
{
        if (StatusIsFatal()) return;
        m_distancePerPulse = distancePerPulse;
}

void Encoder::SetReverseDirection(bool reverseDirection)
{
        if (StatusIsFatal()) return;
        if (m_counter)
        {
                m_counter->SetReverseDirection(reverseDirection);
        }
        else
        {
                tRioStatusCode localStatus = NiFpga_Status_Success;
                m_encoder->writeConfig_Reverse(reverseDirection, &localStatus);
                wpi_setError(localStatus);
        }
}

void Encoder::SetPIDSourceParameter(PIDSourceParameter pidSource)
{
        if (StatusIsFatal()) return;
        m_pidSource = pidSource;
}

double Encoder::PIDGet()
{
        if (StatusIsFatal()) return 0.0;
        switch (m_pidSource)
        {
        case kDistance:
                return GetDistance();
        case kRate:
                return GetRate();
        default:
                return 0.0;
        }
}