.. _program_listing_file_src_LouLib_Controllers_PIDController.cpp:

Program Listing for File PIDController.cpp
==========================================

|exhale_lsh| :ref:`Return to documentation for file <file_src_LouLib_Controllers_PIDController.cpp>` (``src/LouLib/Controllers/PIDController.cpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: 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