Alex Pirciu / Mbed 2 deprecated BFMC

a

Dependencies:   mbed mbed-rtos

src/Controllers/controller.cpp@1:ceee5a608e7c, 2019-03-28 (annotated)

Committer:
alexpirciu
Date:
Thu Mar 28 07:44:42 2019 +0000
Revision:
1:ceee5a608e7c
assa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alexpirciu 1:ceee5a608e7c 1 /**
alexpirciu 1:ceee5a608e7c 2 ******************************************************************************
alexpirciu 1:ceee5a608e7c 3 * @file Controller.cpp
alexpirciu 1:ceee5a608e7c 4 * @author RBRO/PJ-IU
alexpirciu 1:ceee5a608e7c 5 * @version V1.0.0
alexpirciu 1:ceee5a608e7c 6 * @date day-month-year
alexpirciu 1:ceee5a608e7c 7 * @brief This file contains the class implementation for the controller
alexpirciu 1:ceee5a608e7c 8 * functionality.
alexpirciu 1:ceee5a608e7c 9 ******************************************************************************
alexpirciu 1:ceee5a608e7c 10 */
alexpirciu 1:ceee5a608e7c 11
alexpirciu 1:ceee5a608e7c 12 #include <Controllers/controller.hpp>
alexpirciu 1:ceee5a608e7c 13
alexpirciu 1:ceee5a608e7c 14 namespace controllers{
alexpirciu 1:ceee5a608e7c 15 /**
alexpirciu 1:ceee5a608e7c 16 * @brief Construct a new CControllerSiso::CControllerSiso object
alexpirciu 1:ceee5a608e7c 17 *
alexpirciu 1:ceee5a608e7c 18 * @param f_encoder Reference to the encoder getter interface.
alexpirciu 1:ceee5a608e7c 19 * @param f_pid Reference to the controller interface.
alexpirciu 1:ceee5a608e7c 20 * @param f_converter [Optional] Pointer to the converter interface.
alexpirciu 1:ceee5a608e7c 21 */
alexpirciu 1:ceee5a608e7c 22 CControllerSiso::CControllerSiso(encoders::IEncoderGetter& f_encoder
alexpirciu 1:ceee5a608e7c 23 ,ControllerType<double>& f_pid
alexpirciu 1:ceee5a608e7c 24 ,controllers::IConverter* f_converter)
alexpirciu 1:ceee5a608e7c 25 :m_encoder(f_encoder)
alexpirciu 1:ceee5a608e7c 26 ,m_pid(f_pid)
alexpirciu 1:ceee5a608e7c 27 ,m_converter(f_converter)
alexpirciu 1:ceee5a608e7c 28 ,m_control_sup(0.5)
alexpirciu 1:ceee5a608e7c 29 ,m_control_inf(-0.5)
alexpirciu 1:ceee5a608e7c 30 ,m_ref_abs_inf(10.0)
alexpirciu 1:ceee5a608e7c 31 ,m_mes_abs_inf(30.0)
alexpirciu 1:ceee5a608e7c 32 ,m_mes_abs_sup(300.0)
alexpirciu 1:ceee5a608e7c 33 {
alexpirciu 1:ceee5a608e7c 34 }
alexpirciu 1:ceee5a608e7c 35
alexpirciu 1:ceee5a608e7c 36 /**
alexpirciu 1:ceee5a608e7c 37 * @brief Set the reference signal value.
alexpirciu 1:ceee5a608e7c 38 *
alexpirciu 1:ceee5a608e7c 39 * @param f_RefRps The value of the reference signal
alexpirciu 1:ceee5a608e7c 40 */
alexpirciu 1:ceee5a608e7c 41 void CControllerSiso::setRef(double f_RefRps)
alexpirciu 1:ceee5a608e7c 42 {
alexpirciu 1:ceee5a608e7c 43 m_RefRps=f_RefRps;
alexpirciu 1:ceee5a608e7c 44 }
alexpirciu 1:ceee5a608e7c 45
alexpirciu 1:ceee5a608e7c 46 /** \brief Get the value of reference signal.
alexpirciu 1:ceee5a608e7c 47 *
alexpirciu 1:ceee5a608e7c 48 */
alexpirciu 1:ceee5a608e7c 49 double CControllerSiso::getRef()
alexpirciu 1:ceee5a608e7c 50 {
alexpirciu 1:ceee5a608e7c 51 return m_RefRps;
alexpirciu 1:ceee5a608e7c 52 }
alexpirciu 1:ceee5a608e7c 53
alexpirciu 1:ceee5a608e7c 54 /** @brief Get the value of the control signal calculated last time.
alexpirciu 1:ceee5a608e7c 55 *
alexpirciu 1:ceee5a608e7c 56 */
alexpirciu 1:ceee5a608e7c 57 double CControllerSiso::get()
alexpirciu 1:ceee5a608e7c 58 {
alexpirciu 1:ceee5a608e7c 59 return m_u;
alexpirciu 1:ceee5a608e7c 60 }
alexpirciu 1:ceee5a608e7c 61
alexpirciu 1:ceee5a608e7c 62 /** @brief Get the value of the error between the measured and reference signal.
alexpirciu 1:ceee5a608e7c 63 *
alexpirciu 1:ceee5a608e7c 64 */
alexpirciu 1:ceee5a608e7c 65 double CControllerSiso::getError()
alexpirciu 1:ceee5a608e7c 66 {
alexpirciu 1:ceee5a608e7c 67 return m_error;
alexpirciu 1:ceee5a608e7c 68 }
alexpirciu 1:ceee5a608e7c 69
alexpirciu 1:ceee5a608e7c 70 /** @brief Clear the memory of the controller.
alexpirciu 1:ceee5a608e7c 71 *
alexpirciu 1:ceee5a608e7c 72 */
alexpirciu 1:ceee5a608e7c 73 void CControllerSiso::clear()
alexpirciu 1:ceee5a608e7c 74 {
alexpirciu 1:ceee5a608e7c 75 m_pid.clear();
alexpirciu 1:ceee5a608e7c 76 }
alexpirciu 1:ceee5a608e7c 77
alexpirciu 1:ceee5a608e7c 78
alexpirciu 1:ceee5a608e7c 79 /**
alexpirciu 1:ceee5a608e7c 80 * @brief It calculates the next value of the control signal, by utilizing the given interfaces.
alexpirciu 1:ceee5a608e7c 81 *
alexpirciu 1:ceee5a608e7c 82 * @return true control works fine
alexpirciu 1:ceee5a608e7c 83 * @return false appeared an error
alexpirciu 1:ceee5a608e7c 84 */
alexpirciu 1:ceee5a608e7c 85 bool CControllerSiso::control()
alexpirciu 1:ceee5a608e7c 86 {
alexpirciu 1:ceee5a608e7c 87 // Mesurment speed value
alexpirciu 1:ceee5a608e7c 88 float l_MesRps = m_encoder.getSpeedRps();
alexpirciu 1:ceee5a608e7c 89 bool l_isAbs = m_encoder.isAbs();
alexpirciu 1:ceee5a608e7c 90 float l_ref;
alexpirciu 1:ceee5a608e7c 91
alexpirciu 1:ceee5a608e7c 92 // Check the measured value and the superior limit for avoid over control state.
alexpirciu 1:ceee5a608e7c 93 // In this case deactivete the controller.
alexpirciu 1:ceee5a608e7c 94 if(std::abs(l_MesRps) > m_mes_abs_sup){
alexpirciu 1:ceee5a608e7c 95 m_RefRps = 0.0;
alexpirciu 1:ceee5a608e7c 96 m_u = 0.0;
alexpirciu 1:ceee5a608e7c 97 error("@PIDA:CControllerSiso: To high speed!;;\r\n");
alexpirciu 1:ceee5a608e7c 98 return false;
alexpirciu 1:ceee5a608e7c 99 }
alexpirciu 1:ceee5a608e7c 100 // Check the inferior limits of reference signal and measured signal for standing state.
alexpirciu 1:ceee5a608e7c 101 // Inactivate the controller
alexpirciu 1:ceee5a608e7c 102 if(std::abs(m_RefRps) < m_ref_abs_inf && std::abs(l_MesRps) < m_mes_abs_inf ){
alexpirciu 1:ceee5a608e7c 103 m_u = 0.0;
alexpirciu 1:ceee5a608e7c 104 m_error = 0.0;
alexpirciu 1:ceee5a608e7c 105 return true;
alexpirciu 1:ceee5a608e7c 106 }
alexpirciu 1:ceee5a608e7c 107
alexpirciu 1:ceee5a608e7c 108 // Check measured value is oriantated or absolute
alexpirciu 1:ceee5a608e7c 109 if ( l_isAbs ){
alexpirciu 1:ceee5a608e7c 110 l_ref = std::abs(m_RefRps);
alexpirciu 1:ceee5a608e7c 111 } else{
alexpirciu 1:ceee5a608e7c 112 l_ref = m_RefRps;
alexpirciu 1:ceee5a608e7c 113 }
alexpirciu 1:ceee5a608e7c 114 float l_error=l_ref-l_MesRps;
alexpirciu 1:ceee5a608e7c 115 float l_v_control = m_pid.calculateControl(l_error);
alexpirciu 1:ceee5a608e7c 116 float l_pwm_control = converter(l_v_control);
alexpirciu 1:ceee5a608e7c 117
alexpirciu 1:ceee5a608e7c 118 m_u=l_pwm_control;
alexpirciu 1:ceee5a608e7c 119 m_error=l_error;
alexpirciu 1:ceee5a608e7c 120
alexpirciu 1:ceee5a608e7c 121 // Check measured value is oriantated or absolute.
alexpirciu 1:ceee5a608e7c 122 if(m_RefRps<0 && l_isAbs)
alexpirciu 1:ceee5a608e7c 123 {
alexpirciu 1:ceee5a608e7c 124 m_u=m_u*-1.0;
alexpirciu 1:ceee5a608e7c 125 }
alexpirciu 1:ceee5a608e7c 126 return true;
alexpirciu 1:ceee5a608e7c 127 }
alexpirciu 1:ceee5a608e7c 128
alexpirciu 1:ceee5a608e7c 129 /** @brief
alexpirciu 1:ceee5a608e7c 130 *
alexpirciu 1:ceee5a608e7c 131 * Apply the converter interface to change the measurment unit.
alexpirciu 1:ceee5a608e7c 132 *
alexpirciu 1:ceee5a608e7c 133 * @param f_u Input control signal
alexpirciu 1:ceee5a608e7c 134 * @return Converted control signal
alexpirciu 1:ceee5a608e7c 135 */
alexpirciu 1:ceee5a608e7c 136 double CControllerSiso::converter(double f_u)
alexpirciu 1:ceee5a608e7c 137 {
alexpirciu 1:ceee5a608e7c 138 double l_pwm=f_u;
alexpirciu 1:ceee5a608e7c 139 // Convert the control signal from V to PWM
alexpirciu 1:ceee5a608e7c 140 if(m_converter!=NULL){
alexpirciu 1:ceee5a608e7c 141 l_pwm = (*m_converter)(f_u);
alexpirciu 1:ceee5a608e7c 142 }
alexpirciu 1:ceee5a608e7c 143
alexpirciu 1:ceee5a608e7c 144 // Check the pwm control signal and limits
alexpirciu 1:ceee5a608e7c 145 if( l_pwm < m_control_inf ){
alexpirciu 1:ceee5a608e7c 146 l_pwm = m_control_inf;
alexpirciu 1:ceee5a608e7c 147 } else if( l_pwm > m_control_sup ){
alexpirciu 1:ceee5a608e7c 148 l_pwm = m_control_sup;
alexpirciu 1:ceee5a608e7c 149 }
alexpirciu 1:ceee5a608e7c 150 return l_pwm;
alexpirciu 1:ceee5a608e7c 151 }
alexpirciu 1:ceee5a608e7c 152
alexpirciu 1:ceee5a608e7c 153 }; // namespace controllers