Program Listing for File PIDController.cpp

↰ Return to documentation for file (src/LouLib/Controllers/PIDController.cpp)

#include "PIDController.hpp"
#include "../Math/MathFunctions.hpp"

using namespace LouLib::Units::literals;

namespace LouLib {
    namespace Controllers {

        PIDController::PIDController(double _kp, double _ki, double _kd) {
            kp = _kp;
            ki = _ki;
            kd = _kd;

            error = (1<<30);
            derivative = 0;
            integral = 0;

            setTolerance(0);

            setIntegratorRange(-(1<<30), (1<<30));

            setOutputRange(-(1<<30), (1<<30));

            setDeltaTime(10_ms);

            setSetpoint(0);
        }

        void PIDController::setTolerance(double _errorTolerance, double _derivativeTolerance) {
            errorTolerance = _errorTolerance;
            derivativeTolerance = _derivativeTolerance;
        }

        void PIDController::setIntegratorRange(double _integratorRangeMin, double _integratorRangeMax) {
            integratorMin = _integratorRangeMin;
            integratorMax = _integratorRangeMax;
        }

        void PIDController::setOutputRange(double _minOutput, double _maxOutput) {
            minOutput = _minOutput;
            maxOutput = _maxOutput;
        }

        void PIDController::setDeltaTime(Units::Time _deltaTime) {
            deltaTime = _deltaTime.to(Units::SECOND);
        }

        void PIDController::setSetpoint(double _setpoint) {
            error = (1<<30);
            derivative = 0;
            integral = 0;
            setpoint = _setpoint;
        }

        bool PIDController::atSetpoint() {
            return (std::abs(error) < errorTolerance && std::abs(derivative) < derivativeTolerance);
        }

        double PIDController::getOutput() {
            double output = kp*error + ki*integral + kd*derivative;
            return Math::clamp(output, minOutput, maxOutput);
        }

        void PIDController::update(double measurement) {

            double _error = setpoint - measurement;

            if(error == (1<<30)) derivative = 0;
            else derivative = (_error - error)/deltaTime;

            if(error >= integratorMin && error <= integratorMax){
                integral += error*deltaTime;
            }else{
                integral = 0;
            }

            if(Math::signum(error) != Math::signum(_error)){
                integral = 0;
            }

            error = _error;
        }

    } // LouLib
} // Controllers