dimensional_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 "units.h"
 
#include "tap/algorithms/smooth_pid.hpp"
 
namespace trap
{
    namespace algorithms
    {
        template<typename RawControlUnit, typename RawErrorUnit, typename RawTimeUnit>
        class DimensionalSmoothPid
        {
        public:
            using ControlUnit = units::unit_t<RawControlUnit>;
            using ErrorUnit = units::unit_t<RawErrorUnit>;
            using TimeUnit = units::unit_t<RawTimeUnit>;
 
            using PUnit = units::compound_unit<RawControlUnit, units::inverse<RawErrorUnit>>;
            using IUnit = units::compound_unit<RawControlUnit, units::inverse<RawErrorUnit>, units::inverse<RawTimeUnit>>;
            using DUnit = units::compound_unit<RawControlUnit, RawTimeUnit, units::inverse<RawErrorUnit>>;
            
            using ErrorDerivativeUnit = units::compound_unit<RawErrorUnit, units::inverse<RawTimeUnit>>;
 
            using IntegralAccumulationUnit = units::compound_unit<RawErrorUnit, RawTimeUnit>;
            struct Config
            {
            public:
                PUnit p;
                IUnit i;
                DUnit d;
                IntegralAccumulationUnit maxIntegralAccumulation;
                ControlUnit maxOutput;
                ErrorUnit errorDerivativeFloor;
                ErrorUnit errorDeadzone = ErrorUnit{0.0};
                float tQDerivativeKalman = 1.0f;
                float tRDerivativeKalman = 0.0f;
                float tQProportionalKalman = 1.0f;
                float tRProportionalKalman = 0.0f;
 
                Config(double p, double i, double d, double maxIntegralAccumulation, double maxOutput, double errorDerivativeFloor,
                       double errorDeadzone = 0.0, double tQDerivativeKalman = 1.0, double tRDerivativeKalman = 0.0, tRProportionalKalman = 1.0)
                : Config{PUnit{p} IUnit{i}, DUnit{d}, IntegralAccumulationUnit{maxIntegralAccumulation}, ControlUnit{maxOutput}, ErrorUnit{errorDerivativeFloor},
                  ErrorUnit{errorDeadzone}, tQDerivativeKalman, tRDerivativeKalman, tQProportionalKalman, tRProportionalKalman}
                {}
            }
 
            DimensionalSmoothPid(const Config& config);
            runController(ErrorUnit error);
            runController(ErrorUnit error, ErrorDerivativeUnit errorDerivative, TimeUnit deltaTime);
        private:
            tap::algorithms::SmoothPid m_smoothPid;
        };
    }
}
#endif