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 "Timer.h"
#include "WPIErrors.h"

const UINT32 Gyro::kOversampleBits;
const UINT32 Gyro::kAverageBits;
const float Gyro::kSamplesPerSecond;
const float Gyro::kCalibrationSampleTime;
const float Gyro::kDefaultVoltsPerDegreePerSecond;

void Gyro::InitGyro()
{
        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 count;
        m_analog->GetAccumulatorOutput(&value, &count);
        
        UINT32 center = (UINT32)((float)value / (float)count + .5);

        m_offset = ((float)value / (float)count) - (float)center;
        
        m_analog->SetAccumulatorCenter(center);
        m_analog->SetAccumulatorDeadband(0); 
        m_analog->ResetAccumulator();
}

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

Gyro::Gyro(UINT32 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 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;
}


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

double Gyro::PIDGet()
{
        return GetAngle();
}