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

constexpr float PWM::kDefaultPwmPeriod;
constexpr float PWM::kDefaultPwmCenter;
const int32_t PWM::kDefaultPwmStepsDown;
const int32_t PWM::kPwmDisabled;
static Resource *allocated = NULL;

void PWM::InitPWM(uint8_t moduleNumber, uint32_t channel)
{
        m_table = NULL;
        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;

        nUsageReporting::report(nUsageReporting::kResourceType_PWM, channel, moduleNumber - 1);
}

PWM::PWM(uint8_t moduleNumber, uint32_t channel)
        : m_module(NULL)
{
        InitPWM(moduleNumber, channel);
}

PWM::PWM(uint32_t 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_t max, int32_t deadbandMax, int32_t center, int32_t deadbandMin, int32_t min)
{
        if (StatusIsFatal()) return;
        m_maxPwm = max;
        m_deadbandMaxPwm = deadbandMax;
        m_centerPwm = center;
        m_deadbandMinPwm = deadbandMin;
        m_minPwm = min;
}


void PWM::SetBounds(double max, double deadbandMax, double center, double deadbandMin, double min)
{
        if (StatusIsFatal()) return;
    
        double loopTime = m_module->GetLoopTiming()/(kSystemClockTicksPerMicrosecond*1e3);
                        
    m_maxPwm = (int32_t)((max-kDefaultPwmCenter)/loopTime+kDefaultPwmStepsDown-1);
    m_deadbandMaxPwm = (int32_t)((deadbandMax-kDefaultPwmCenter)/loopTime+kDefaultPwmStepsDown-1);
    m_centerPwm = (int32_t)((center-kDefaultPwmCenter)/loopTime+kDefaultPwmStepsDown-1);
    m_deadbandMinPwm = (int32_t)((deadbandMin-kDefaultPwmCenter)/loopTime+kDefaultPwmStepsDown-1);
    m_minPwm = (int32_t)((min-kDefaultPwmCenter)/loopTime+kDefaultPwmStepsDown-1);
}

uint32_t 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_t rawValue;
        // note, need to perform the multiplication below as floating point before converting to int
        rawValue = (int32_t)( (pos * (float) GetFullRangeScaleFactor()) + GetMinNegativePwm());

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

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

float PWM::GetPosition()
{
        if (StatusIsFatal()) return 0.0;
        int32_t 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_t rawValue;
        if (speed == 0.0)
        {
                rawValue = GetCenterPwm();
        }
        else if (speed > 0.0)
        {
                rawValue = (int32_t)(speed * ((float)GetPositiveScaleFactor()) +
                                                                        ((float) GetMinPositivePwm()) + 0.5);
        }
        else
        {
                rawValue = (int32_t)(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_t)rawValue);
}

float PWM::GetSpeed()
{
        if (StatusIsFatal()) return 0.0;
        int32_t value = GetRaw();
        if (value == PWM::kPwmDisabled)
        {
                return 0.0;
        }
        else 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_t value)
{
        if (StatusIsFatal()) return;
        m_module->SetPWM(m_channel, value);
}

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


void PWM::ValueChanged(ITable* source, const std::string& key, EntryValue value, bool isNew) {
        SetSpeed(value.f);
}

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

void PWM::StartLiveWindowMode() {
        SetSpeed(0);
        if (m_table != NULL) {
                m_table->AddTableListener("Value", this, true);
        }
}

void PWM::StopLiveWindowMode() {
        SetSpeed(0);
        if (m_table != NULL) {
                m_table->RemoveTableListener(this);
        }
}

std::string PWM::GetSmartDashboardType() {
        return "Speed Controller";
}

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

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