PIDSubsystem.cpp

/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2011. 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 "Commands/PIDSubsystem.h"
#include "PIDController.h"
#include "float.h"

// XXX max and min are not used?

PIDSubsystem::PIDSubsystem(const char *name, double p, double i, double d) :
        Subsystem(name)
{
        m_controller = new PIDController(p, i, d, this, this);
}

PIDSubsystem::PIDSubsystem(const char *name, double p, double i, double d, double f) :
        Subsystem(name)
{
        m_controller = new PIDController(p, i, d, f, this, this);
}

PIDSubsystem::PIDSubsystem(const char *name, double p, double i, double d, double f,
        double period) :
        Subsystem(name)
{
        m_controller = new PIDController(p, i, d, f, this, this, period);
}

PIDSubsystem::PIDSubsystem(double p, double i, double d) :
        Subsystem("PIDSubsystem")
{
        m_controller = new PIDController(p, i, d, this, this);
}

PIDSubsystem::PIDSubsystem(double p, double i, double d, double f) :
        Subsystem("PIDSubsystem")
{
        m_controller = new PIDController(p, i, d, f, this, this);
}

PIDSubsystem::PIDSubsystem(double p, double i, double d, double f, double period) :
        Subsystem("PIDSubsystem")
{
        m_controller = new PIDController(p, i, d, f, this, this, period);
}

PIDSubsystem::~PIDSubsystem()
{
        delete m_controller;
}

void PIDSubsystem::Enable()
{
        m_controller->Enable();
}

void PIDSubsystem::Disable()
{
        m_controller->Disable();
}


PIDController *PIDSubsystem::GetPIDController()
{
        return m_controller;
}

void PIDSubsystem::SetSetpoint(double setpoint)
{
        m_controller->SetSetpoint(setpoint);
}

void PIDSubsystem::SetSetpointRelative(double deltaSetpoint)
{
        SetSetpoint(GetSetpoint() + deltaSetpoint);
}

double PIDSubsystem::GetSetpoint()
{
        return m_controller->GetSetpoint();
}

void PIDSubsystem::SetInputRange(float minimumInput, float maximumInput)
{
        m_controller->SetInputRange(minimumInput, maximumInput);
}

/*
 * Set the absolute error which is considered tolerable for use with
 * OnTarget.
 * @param percentage error which is tolerable
 */
void PIDSubsystem::SetAbsoluteTolerance(float absValue) {
        m_controller->SetAbsoluteTolerance(absValue);
}

/*
 * Set the percentage error which is considered tolerable for use with
 * OnTarget.
 * @param percentage error which is tolerable
 */
void PIDSubsystem::SetPercentTolerance(float percent) {
        m_controller->SetPercentTolerance(percent);
}

/*
 * Return true if the error is within the percentage of the total input range,
 * determined by SetTolerance. This asssumes that the maximum and minimum input
 * were set using SetInput. Use OnTarget() in the IsFinished() method of commands
 * that use this subsystem.
 * 
 * Currently this just reports on target as the actual value passes through the setpoint.
 * Ideally it should be based on being within the tolerance for some period of time.
 * 
 * @return true if the error is within the percentage tolerance of the input range
 */
bool PIDSubsystem::OnTarget()
{
        return m_controller->OnTarget();
}

double PIDSubsystem::GetPosition()
{
        return ReturnPIDInput();
}

void PIDSubsystem::PIDWrite(float output)
{
        UsePIDOutput(output);
}

double PIDSubsystem::PIDGet()
{
        return ReturnPIDInput();
}


std::string PIDSubsystem::GetSmartDashboardType(){
        return "PIDCommand";
}
void PIDSubsystem::InitTable(ITable* table){
        m_controller->InitTable(table);
        Subsystem::InitTable(table);
}