controller.cpp

00001 /**
00002   ******************************************************************************
00003   * @file    Controller.cpp
00004   * @author  RBRO/PJ-IU
00005   * @version V1.0.0
00006   * @date    day-month-year
00007   * @brief   This file contains the class implementation for the controller
00008   *          functionality.
00009   ******************************************************************************
00010  */
00011 
00012 #include <Controllers/controller.hpp>
00013 
00014 namespace controllers{
00015     /**
00016      * @brief Construct a new CControllerSiso::CControllerSiso object
00017      * 
00018      * @param f_encoder Reference to the encoder getter interface.
00019      * @param f_pid     Reference to the controller interface.
00020      * @param f_converter [Optional] Pointer to the converter interface. 
00021      */
00022     CControllerSiso::CControllerSiso(encoders::IEncoderGetter&          f_encoder
00023                             ,ControllerType<double>&                    f_pid
00024                             ,controllers::IConverter*                       f_converter)
00025         :m_encoder(f_encoder)
00026         ,m_pid(f_pid)
00027         ,m_converter(f_converter)
00028         ,m_control_sup(0.5)
00029         ,m_control_inf(-0.5)
00030         ,m_ref_abs_inf(10.0)
00031         ,m_mes_abs_inf(30.0)
00032         ,m_mes_abs_sup(300.0)
00033     {
00034     }
00035 
00036     /**
00037      * @brief Set the reference signal value.
00038      * 
00039      * @param f_RefRps The value of the reference signal
00040      */
00041     void CControllerSiso::setRef(double f_RefRps)
00042     {
00043         m_RefRps=f_RefRps;
00044     }
00045 
00046     /** \brief  Get the value of reference signal.
00047      *
00048      */
00049     double CControllerSiso::getRef()
00050     {
00051         return m_RefRps;
00052     }
00053 
00054     /** @brief  Get the value of the control signal calculated last time.
00055      * 
00056      */
00057     double CControllerSiso::get()
00058     {
00059         return m_u;
00060     }
00061 
00062     /** @brief  Get the value of the error between the measured and reference signal. 
00063      *
00064      */
00065     double CControllerSiso::getError()
00066     {
00067         return m_error;
00068     }
00069 
00070     /** @brief  Clear the memory of the controller. 
00071      *
00072      */
00073     void CControllerSiso::clear()
00074     {
00075         m_pid.clear();
00076     }
00077 
00078 
00079     /**
00080      * @brief It calculates the next value of the control signal, by utilizing the given interfaces.
00081      * 
00082      * @return true control works fine
00083      * @return false appeared an error
00084      */
00085     bool CControllerSiso::control()
00086     {
00087         // Mesurment speed value
00088         float  l_MesRps = m_encoder.getSpeedRps();
00089         bool   l_isAbs = m_encoder.isAbs();
00090         float  l_ref;
00091 
00092         // Check the measured value and the superior limit for avoid over control state.
00093         // In this case deactivete the controller. 
00094         if(std::abs(l_MesRps) > m_mes_abs_sup){
00095             m_RefRps = 0.0;
00096             m_u = 0.0;
00097             error("@PIDA:CControllerSiso: To high speed!;;\r\n");
00098             return false;
00099         }
00100         // Check the inferior limits of reference signal and measured signal for standing state.
00101         // Inactivate the controller
00102         if(std::abs(m_RefRps) < m_ref_abs_inf && std::abs(l_MesRps) < m_mes_abs_inf ){
00103             m_u = 0.0;
00104             m_error = 0.0;
00105             return true; 
00106         }
00107 
00108         // Check measured value is oriantated or absolute
00109         if ( l_isAbs ){
00110             l_ref = std::abs(m_RefRps);
00111         } else{ 
00112             l_ref = m_RefRps;
00113         }
00114         float l_error=l_ref-l_MesRps;
00115         float l_v_control = m_pid.calculateControl(l_error);
00116         float l_pwm_control = converter(l_v_control);
00117         
00118         m_u=l_pwm_control;
00119         m_error=l_error;
00120         
00121         // Check measured value is oriantated or absolute.
00122         if(m_RefRps<0 && l_isAbs)
00123         {
00124             m_u=m_u*-1.0;
00125         }
00126         return true;
00127     }
00128 
00129     /** @brief  
00130      *
00131      * Apply the converter interface to change the measurment unit.
00132      *
00133      * @param f_u                  Input control signal
00134      * @return                     Converted control signal
00135      */
00136     double CControllerSiso::converter(double f_u)
00137     {
00138         double l_pwm=f_u;
00139         // Convert the control signal from V to PWM
00140         if(m_converter!=NULL){
00141             l_pwm = (*m_converter)(f_u);
00142         }
00143 
00144         // Check the pwm control signal and limits
00145         if( l_pwm < m_control_inf ){
00146             l_pwm  = m_control_inf;
00147         } else if( l_pwm > m_control_sup ){
00148             l_pwm  = m_control_sup;
00149         }
00150         return l_pwm;
00151     }
00152 
00153 }; //  namespace controllers