Gyro.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 "Gyro.h"
#include "AnalogChannel.h"
#include "AnalogModule.h"
#include "NetworkCommunication/UsageReporting.h"
#include "Timer.h"
#include "WPIErrors.h"
#include "LiveWindow/LiveWindow.h"

const uint32_t Gyro::kOversampleBits;
const uint32_t Gyro::kAverageBits;
constexpr float Gyro::kSamplesPerSecond;
constexpr float Gyro::kCalibrationSampleTime;
constexpr float Gyro::kDefaultVoltsPerDegreePerSecond;

void Gyro::InitGyro()
{
        m_table = NULL;
        if (!m_analog->IsAccumulatorChannel())
        {
                wpi_setWPIErrorWithContext(ParameterOutOfRange,
                                "moduleNumber and/or channel (must be accumulator channel)");
                if (m_channelAllocated)
                {
                        delete m_analog;
                        m_analog = NULL;
                }
                return;
        }

        m_voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond;
        m_analog->SetAverageBits(kAverageBits);
        m_analog->SetOversampleBits(kOversampleBits);
        float sampleRate = kSamplesPerSecond * 
                (1 << (kAverageBits + kOversampleBits));
        m_analog->GetModule()->SetSampleRate(sampleRate);
        Wait(1.0);

        m_analog->InitAccumulator();
        Wait(kCalibrationSampleTime);

        INT64 value;
        uint32_t count;
        m_analog->GetAccumulatorOutput(&value, &count);

        m_center = (uint32_t)((float)value / (float)count + .5);

        m_offset = ((float)value / (float)count) - (float)m_center;

        m_analog->SetAccumulatorCenter(m_center);
        m_analog->SetAccumulatorDeadband(0); 
        m_analog->ResetAccumulator();
        
        SetPIDSourceParameter(kAngle);

        nUsageReporting::report(nUsageReporting::kResourceType_Gyro, m_analog->GetChannel(), m_analog->GetModuleNumber() - 1);
        LiveWindow::GetInstance()->AddSensor("Gyro", m_analog->GetModuleNumber(), m_analog->GetChannel(), this);
}

Gyro::Gyro(uint8_t moduleNumber, uint32_t channel)
{
        m_analog = new AnalogChannel(moduleNumber, channel);
        m_channelAllocated = true;
        InitGyro();
}

Gyro::Gyro(uint32_t channel)
{
        m_analog = new AnalogChannel(channel);
        m_channelAllocated = true;
        InitGyro();
}

Gyro::Gyro(AnalogChannel *channel)
{
        m_analog = channel;
        m_channelAllocated = false;
        if (channel == NULL)
        {
                wpi_setWPIError(NullParameter);
        }
        else
        {
                InitGyro();
        }
}

Gyro::Gyro(AnalogChannel &channel)
{
        m_analog = &channel;
        m_channelAllocated = false;
        InitGyro();
}

void Gyro::Reset()
{
        m_analog->ResetAccumulator();
}

Gyro::~Gyro()
{
        if (m_channelAllocated)
                delete m_analog;
}

float Gyro::GetAngle( void )
{
        INT64 rawValue;
        uint32_t count;
        m_analog->GetAccumulatorOutput(&rawValue, &count);

        INT64 value = rawValue - (INT64)((float)count * m_offset);

        double scaledValue = value * 1e-9 * (double)m_analog->GetLSBWeight() * (double)(1 << m_analog->GetAverageBits()) /
                (m_analog->GetModule()->GetSampleRate() * m_voltsPerDegreePerSecond);

        return (float)scaledValue;
}


double Gyro::GetRate( void )
{
        return (m_analog->GetAverageValue() - ((double)m_center + m_offset)) * 1e-9 * m_analog->GetLSBWeight() 
                        / ((1 << m_analog->GetOversampleBits()) * m_voltsPerDegreePerSecond);
}


void Gyro::SetSensitivity( float voltsPerDegreePerSecond )
{
        m_voltsPerDegreePerSecond = voltsPerDegreePerSecond;
}

void Gyro::SetPIDSourceParameter(PIDSourceParameter pidSource)
{
        if(pidSource == 0 || pidSource > 2)
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "Gyro pidSource");
    m_pidSource = pidSource;
}

double Gyro::PIDGet()
{
        switch(m_pidSource){
        case kRate:
                return GetRate();
        case kAngle:
                return GetAngle();
        default:
                return 0;
        }
}

void Gyro::UpdateTable() {
        if (m_table != NULL) {
                m_table->PutNumber("Value", GetAngle());
        }
}

void Gyro::StartLiveWindowMode() {
        
}

void Gyro::StopLiveWindowMode() {
        
}

std::string Gyro::GetSmartDashboardType() {
        return "Gyro";
}

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

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