PIDController.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 "PIDController.h"
#include "Notifier.h"
#include "PIDSource.h"
#include "PIDOutput.h"
#include <math.h>
#include "Synchronized.h"
PIDController::PIDController(float Kp, float Ki, float Kd,
                                                                PIDSource *source, PIDOutput *output,
                                                                float period) :
        m_semaphore (0)
{
        m_semaphore = semBCreate(SEM_Q_PRIORITY, SEM_FULL);

        m_controlLoop = new Notifier(PIDController::CallCalculate, this);
        
        
        m_P = Kp;
        m_I = Ki;
        m_D = Kd;
        m_maximumOutput = 1.0;
        m_minimumOutput = -1.0;
        
        m_maximumInput = 0;
        m_minimumInput = 0;
        
        m_continuous = false;
        m_enabled = false;
        m_setpoint = 0;

        m_prevError = 0;
        m_totalError = 0;
        m_tolerance = .05;

        m_result = 0;
        
        m_pidInput = source;
        m_pidOutput = output;
        m_period = period;

        m_controlLoop->StartPeriodic(m_period);
}

PIDController::~PIDController()
{
        semFlush(m_semaphore);
        delete m_controlLoop;
}

void PIDController::CallCalculate(void *controller)
{
        PIDController *control = (PIDController*) controller;
        control->Calculate();
}

void PIDController::Calculate()
{
        bool enabled;
        PIDSource *pidInput;

        CRITICAL_REGION(m_semaphore)
        {
                if (m_pidInput == 0) return;
                if (m_pidOutput == 0) return;
                enabled = m_enabled;
                pidInput = m_pidInput;
        }
        END_REGION;

        if (enabled)
        {
                float input = pidInput->PIDGet();
                float result;
                PIDOutput *pidOutput;

                {
                        Synchronized sync(m_semaphore);
                        m_error = m_setpoint - input;
                        if (m_continuous)
                        {
                                if (fabs(m_error) > (m_maximumInput - m_minimumInput) / 2)
                                {
                                        if (m_error > 0)
                                        {
                                                m_error = m_error - m_maximumInput + m_minimumInput;
                                        }
                                        else
                                        {
                                                m_error = m_error + m_maximumInput - m_minimumInput;
                                        }
                                }
                        }

                        double potentialIGain = (m_totalError + m_error) * m_I;
                        if (potentialIGain < m_maximumOutput)
                        {
                                if (potentialIGain > m_minimumOutput)
                                        m_totalError += m_error;
                                else
                                        m_totalError = m_minimumOutput / m_I;
                        }
                        else
                        {
                                m_totalError = m_maximumOutput / m_I;
                        }

                        m_result = m_P * m_error + m_I * m_totalError + m_D * (m_error - m_prevError);
                        m_prevError = m_error;

                        if (m_result > m_maximumOutput) m_result = m_maximumOutput;
                        else if (m_result < m_minimumOutput) m_result = m_minimumOutput;

                        pidOutput = m_pidOutput;
                        result = m_result;
                }

                pidOutput->PIDWrite(result);
        }
}

void PIDController::SetPID(float p, float i, float d)
{
        CRITICAL_REGION(m_semaphore)
        {
                m_P = p;
                m_I = i;
                m_D = d;
        }
        END_REGION;
}

float PIDController::GetP()
{
        CRITICAL_REGION(m_semaphore)
        {
                return m_P;
        }
        END_REGION;
}

float PIDController::GetI()
{
        CRITICAL_REGION(m_semaphore)
        {
                return m_I;
        }
        END_REGION;
}

float PIDController::GetD()
{
        CRITICAL_REGION(m_semaphore)
        {
                return m_D;
        }
        END_REGION;
}

float PIDController::Get()
{
        float result;
        CRITICAL_REGION(m_semaphore)
        {
                result = m_result;
        }
        END_REGION;
        return result;
}

void PIDController::SetContinuous(bool continuous)
{
        CRITICAL_REGION(m_semaphore)
        {
                m_continuous = continuous;
        }
        END_REGION;

}

void PIDController::SetInputRange(float minimumInput, float maximumInput)
{
        CRITICAL_REGION(m_semaphore)
        {
                m_minimumInput = minimumInput;
                m_maximumInput = maximumInput;  
        }
        END_REGION;

        SetSetpoint(m_setpoint);
}

void PIDController::SetOutputRange(float minimumOutput, float maximumOutput)
{
        CRITICAL_REGION(m_semaphore)
        {
                m_minimumOutput = minimumOutput;
                m_maximumOutput = maximumOutput;
        }
        END_REGION;
}

void PIDController::SetSetpoint(float setpoint)
{
        CRITICAL_REGION(m_semaphore)
        {
                if (m_maximumInput > m_minimumInput)
                {
                        if (setpoint > m_maximumInput)
                                m_setpoint = m_maximumInput;
                        else if (setpoint < m_minimumInput)
                                m_setpoint = m_minimumInput;
                        else
                                m_setpoint = setpoint;
                }
                else
                {
                        m_setpoint = setpoint;
                }
        }
        END_REGION;     
}

float PIDController::GetSetpoint()
{
        float setpoint;
        CRITICAL_REGION(m_semaphore)
        {
                setpoint = m_setpoint;
        }
        END_REGION;
        return setpoint;
}

float PIDController::GetError()
{
        float error;
        CRITICAL_REGION(m_semaphore)
        {
                error = m_error;
        }
        END_REGION;
        return error;
}

/*
 * Set the percentage error which is considered tolerable for use with
 * OnTarget.
 * @param percentage error which is tolerable
 */
void PIDController::SetTolerance(float percent)
{
        CRITICAL_REGION(m_semaphore)
        {
                m_tolerance = percent;
        }
        END_REGION;
}

/*
 * 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.
 */
bool PIDController::OnTarget()
{
        bool temp;
        CRITICAL_REGION(m_semaphore)
        {
                temp = fabs(m_error) < (m_tolerance / 100 * 
                        (m_maximumInput - m_minimumInput));
        }
        END_REGION;
        return temp;
}

void PIDController::Enable()
{
        CRITICAL_REGION(m_semaphore)
        {                       
                m_enabled = true;
        }
        END_REGION;     
}
void PIDController::Disable()
{
        CRITICAL_REGION(m_semaphore)
        {
                m_pidOutput->PIDWrite(0);
                m_enabled = false;
        }
        END_REGION;
}

bool PIDController::IsEnabled()
{
        bool enabled;
        CRITICAL_REGION(m_semaphore)
        {
                enabled = m_enabled;
        }
        END_REGION;
        return enabled;
}

void PIDController::Reset()
{
        Disable();

        CRITICAL_REGION(m_semaphore)
        {
                m_prevError = 0;
                m_totalError = 0;
                m_result = 0;
        }
        END_REGION;
}