PWM.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 "PWM.h"

#include "DigitalModule.h"
#include "Resource.h"
#include "Utility.h"
#include "WPIErrors.h"

const UINT32 PWM::kDefaultPwmPeriod;
const UINT32 PWM::kDefaultMinPwmHigh;
const INT32 PWM::kPwmDisabled;
static Resource *allocated = NULL;

void PWM::InitPWM(UINT8 moduleNumber, UINT32 channel)
{
        char buf[64];
        Resource::CreateResourceObject(&allocated, tDIO::kNumSystems * kPwmChannels);
        if (!CheckPWMModule(moduleNumber))
        {
                snprintf(buf, 64, "Digital Module %d", moduleNumber);
                wpi_setWPIErrorWithContext(ModuleIndexOutOfRange, buf);
                return;
        }
        if (!CheckPWMChannel(channel))
        {
                snprintf(buf, 64, "PWM Channel %d", channel);
                wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf);
                return;
        }

        snprintf(buf, 64, "PWM %d (Module: %d)", channel, moduleNumber);
        if (allocated->Allocate((moduleNumber - 1) * kPwmChannels + channel - 1, buf) == ~0ul)
        {
                CloneError(allocated);
                return;
        }
        m_channel = channel;
        m_module = DigitalModule::GetInstance(moduleNumber);
        m_module->SetPWM(m_channel, kPwmDisabled);
        m_eliminateDeadband = false;
}

PWM::PWM(UINT8 moduleNumber, UINT32 channel)
        : m_module(NULL)
{
        InitPWM(moduleNumber, channel);
}

PWM::PWM(UINT32 channel)
        : m_module(NULL)
{
        InitPWM(GetDefaultDigitalModule(), channel);
}

PWM::~PWM()
{
        if (m_module)
        {
                m_module->SetPWM(m_channel, kPwmDisabled);
                allocated->Free((m_module->GetNumber() - 1) * kPwmChannels + m_channel - 1);
        }
}

void PWM::EnableDeadbandElimination(bool eliminateDeadband)
{
        if (StatusIsFatal()) return;
        m_eliminateDeadband = eliminateDeadband;
}

void PWM::SetBounds(INT32 max, INT32 deadbandMax, INT32 center, INT32 deadbandMin, INT32 min)
{
        if (StatusIsFatal()) return;
        m_maxPwm = max;
        m_deadbandMaxPwm = deadbandMax;
        m_centerPwm = center;
        m_deadbandMinPwm = deadbandMin;
        m_minPwm = min;
}

UINT32 PWM::GetModuleNumber()
{
        return m_module->GetNumber();
}

void PWM::SetPosition(float pos)
{
        if (StatusIsFatal()) return;
        if (pos < 0.0)
        {
                pos = 0.0;
        }
        else if (pos > 1.0)
        {
                pos = 1.0;
        }

        INT32 rawValue;
        // note, need to perform the multiplication below as floating point before converting to int
        rawValue = (INT32)( (pos * (float) GetFullRangeScaleFactor()) + GetMinNegativePwm());

        wpi_assert((rawValue >= GetMinNegativePwm()) && (rawValue <= GetMaxPositivePwm()));
        wpi_assert(rawValue != kPwmDisabled);

        // send the computed pwm value to the FPGA
        SetRaw((UINT8)rawValue);
}

float PWM::GetPosition()
{
        if (StatusIsFatal()) return 0.0;
        INT32 value = GetRaw();
        if (value < GetMinNegativePwm())
        {
                return 0.0;
        }
        else if (value > GetMaxPositivePwm())
        {
                return 1.0;
        }
        else
        {
                return (float)(value - GetMinNegativePwm()) / (float)GetFullRangeScaleFactor();
        }
}

void PWM::SetSpeed(float speed)
{
        if (StatusIsFatal()) return;
        // clamp speed to be in the range 1.0 >= speed >= -1.0
        if (speed < -1.0)
        {
                speed = -1.0;
        }
        else if (speed > 1.0)
        {
                speed = 1.0;
        }

        // calculate the desired output pwm value by scaling the speed appropriately
        INT32 rawValue;
        if (speed == 0.0)
        {
                rawValue = GetCenterPwm();
        }
        else if (speed > 0.0)
        {
                rawValue = (INT32)(speed * ((float)GetPositiveScaleFactor()) +
                                                                        ((float) GetMinPositivePwm()) + 0.5);
        }
        else
        {
                rawValue = (INT32)(speed * ((float)GetNegativeScaleFactor()) +
                                                                        ((float) GetMaxNegativePwm()) + 0.5);
        }

        // the above should result in a pwm_value in the valid range
        wpi_assert((rawValue >= GetMinNegativePwm()) && (rawValue <= GetMaxPositivePwm()));
        wpi_assert(rawValue != kPwmDisabled);

        // send the computed pwm value to the FPGA
        SetRaw((UINT8)rawValue);
}

float PWM::GetSpeed()
{
        if (StatusIsFatal()) return 0.0;
        INT32 value = GetRaw();
        if (value > GetMaxPositivePwm())
        {
                return 1.0;
        }
        else if (value < GetMinNegativePwm())
        {
                return -1.0;
        }
        else if (value > GetMinPositivePwm())
        {
                return (float)(value - GetMinPositivePwm()) / (float)GetPositiveScaleFactor();
        }
        else if (value < GetMaxNegativePwm())
        {
                return (float)(value - GetMaxNegativePwm()) / (float)GetNegativeScaleFactor();
        }
        else
        {
                return 0.0;
        }
}

void PWM::SetRaw(UINT8 value)
{
        if (StatusIsFatal()) return;
        m_module->SetPWM(m_channel, value);
}

UINT8 PWM::GetRaw()
{
        if (StatusIsFatal()) return 0;
        return m_module->GetPWM(m_channel);
}

void PWM::SetPeriodMultiplier(PeriodMultiplier mult)
{
        if (StatusIsFatal()) return;
        switch(mult)
        {
        case kPeriodMultiplier_4X:
                m_module->SetPWMPeriodScale(m_channel, 3); // Squelch 3 out of 4 outputs
                break;
        case kPeriodMultiplier_2X:
                m_module->SetPWMPeriodScale(m_channel, 1); // Squelch 1 out of 2 outputs
                break;
        case kPeriodMultiplier_1X:
                m_module->SetPWMPeriodScale(m_channel, 0); // Don't squelch any outputs
                break;
        default:
                wpi_assert(false);
        }
}