smooth_pid.hpp

Go to the documentation of this file.

#ifndef FANG_ROBOTICS_MCB_TRAP_SMOOTH_PID_HPP
#define FANG_ROBOTICS_MCB_TRAP_SMOOTH_PID_HPP
#include "util/chrono/simple_timer.hpp"
#include "wrap/units/units_alias.hpp"
#include "system/assert/fang_assert.hpp"
#include <cmath>
 
#include "tap/algorithms/math_user_utils.hpp"
#include "tap/algorithms/smooth_pid.hpp"
 
namespace trap
{
    namespace algorithms
    {
        template<typename ErrorType, typename ControlType, typename TimeUnit>
        class SmoothPid
        {
        public:
            //A bit of cheating
            using Config = tap::algorithms::SmoothPidConfig;
 
            SmoothPid(const Config& config, ErrorType initialLastError = ErrorType{0.0})
            : m_smoothPid{config}, m_lastError{initialLastError}
            {
            }
 
            ControlType runController(ErrorType error)
            {
                const TimeUnit deltaTime{m_runControllerTimer.getDurationAndReset()};
                return runController(error, deltaTime);
            }
 
            ControlType runController(const ErrorType& error, const TimeUnit& deltaTime)
            {
                const ErrorType deltaError{error - m_lastError};
                const double errorDerivative{static_cast<double>(deltaError) / static_cast<double>(deltaTime)};
                //Prevents oscillations by filtering out higher frequencies
                const double cleanErrorDerivative{tap::algorithms::lowPassFilter(m_lastDerivative, errorDerivative, k_lowPassAlpha)};
 
                m_lastDerivative = errorDerivative;
                m_lastError = error;
 
                return runController(error, cleanErrorDerivative, deltaTime);
            }
 
            ControlType runController(ErrorType error, double errorDerivative, TimeUnit deltaTime)
            {
                float result {m_smoothPid.runController(static_cast<float>(error), static_cast<float>(errorDerivative), static_cast<float>(deltaTime))};
                FANG_ASSERT(!std::isnan(result), "Cannot be nan");
                static float testResult{};
                testResult = result;
                return ControlType{result};
            }
 
        private:
            static constexpr double k_lowPassAlpha{0.0};
            tap::algorithms::SmoothPid m_smoothPid;
            ErrorType m_lastError{0};
            double m_lastDerivative{0};
            fang::chrono::SimpleTimer m_runControllerTimer{};
        };
 
    }
}
#endif