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 "NetworkCommunication/UsageReporting.h"
#include "Resource.h"
#include "WPIErrors.h"
#include "LiveWindow/LiveWindow.h"

static Resource *quadEncoders = NULL;

void Encoder::InitEncoder(bool reverseDirection, EncodingType encodingType)
{
        m_table = NULL;
        m_encodingType = encodingType;
        tRioStatusCode localStatus = NiFpga_Status_Success;
        switch (encodingType)
        {
                case k4X:
                {
                        Resource::CreateResourceObject(&quadEncoders, tEncoder::kNumSystems);
                        uint32_t 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);
                        m_index = m_counter->GetIndex();
                        break;
                }
        }
        m_distancePerPulse = 1.0;
        m_pidSource = kDistance;
        wpi_setError(localStatus);

        nUsageReporting::report(nUsageReporting::kResourceType_Encoder, m_index, encodingType);
        LiveWindow::GetInstance()->AddSensor("Encoder", m_aSource->GetModuleForRouting(), m_aSource->GetChannelForRouting(), this);
}

Encoder::Encoder(uint8_t aModuleNumber, uint32_t aChannel,
                                                uint8_t bModuleNumber, uint32_t 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_t aChannel, uint32_t 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_t Encoder::GetRaw()
{
        if (StatusIsFatal()) return 0;
        int32_t 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_t Encoder::Get()
{
        if (StatusIsFatal()) return 0;
        return (int32_t) (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_t)(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::SetSamplesToAverage(int samplesToAverage)
{
        tRioStatusCode localStatus = NiFpga_Status_Success;
        if (samplesToAverage < 1 || samplesToAverage > 127)
        {
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "Average counter values must be between 1 and 127");
        }
        switch (m_encodingType) {
                case k4X:
                        m_encoder->writeTimerConfig_AverageSize(samplesToAverage, &localStatus);
                        break;
                case k1X:
                case k2X:
                        m_counter->SetSamplesToAverage(samplesToAverage);
                        break;
        }
        wpi_setError(localStatus);
}
    
int Encoder::GetSamplesToAverage()
{
        tRioStatusCode localStatus = NiFpga_Status_Success;
        int result = 1;
        switch (m_encodingType) {
                case k4X:
                        result = m_encoder->readTimerConfig_AverageSize(&localStatus);
                        break;
                case k1X:
                case k2X:
                        result = m_counter->GetSamplesToAverage();
                        break;
        }
        wpi_setError(localStatus);
        return result;
}



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

void Encoder::UpdateTable() {
        if (m_table != NULL) {
        m_table->PutNumber("Speed", GetRate());
        m_table->PutNumber("Distance", GetDistance());
        m_table->PutNumber("Distance per Tick", m_distancePerPulse);
        }
}

void Encoder::StartLiveWindowMode() {
        
}

void Encoder::StopLiveWindowMode() {
        
}

std::string Encoder::GetSmartDashboardType() {
        if (m_encodingType == k4X)
                return "Quadrature Encoder";
        else
                return "Encoder";
}

void Encoder::InitTable(ITable *subTable) {
        m_table = subTable;
        UpdateTable();
}

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