Alex Pirciu / Mbed 2 deprecated BFMC

a

Dependencies:   mbed mbed-rtos

src/Examples/SystemModels/ackermannmodel.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 AckermannModel.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 Ackermann model.
alexpirciu 1:ceee5a608e7c 8 ******************************************************************************
alexpirciu 1:ceee5a608e7c 9 */
alexpirciu 1:ceee5a608e7c 10
alexpirciu 1:ceee5a608e7c 11 #include <Examples/SystemModels/ackermannmodel.hpp>
alexpirciu 1:ceee5a608e7c 12
alexpirciu 1:ceee5a608e7c 13 /* Specify model type */
alexpirciu 1:ceee5a608e7c 14 using ackermannmodeltype = systemmodels::nlti::mimo::CDiscreteTimeSystemModel<double,2,10,5>;
alexpirciu 1:ceee5a608e7c 15
alexpirciu 1:ceee5a608e7c 16 /** \brief Constructor for the CAckermannModel class
alexpirciu 1:ceee5a608e7c 17 *
alexpirciu 1:ceee5a608e7c 18 * Constructor method
alexpirciu 1:ceee5a608e7c 19 *
alexpirciu 1:ceee5a608e7c 20 * \param[in] f_states reference to initial system states
alexpirciu 1:ceee5a608e7c 21 * \param[in] f_dt sample time
alexpirciu 1:ceee5a608e7c 22 * \param[in] f_gamma reduction factor from motor to wheel
alexpirciu 1:ceee5a608e7c 23 * \param[in] f_wb wheel base
alexpirciu 1:ceee5a608e7c 24 * \param[in] f_b motor viscous friction constant
alexpirciu 1:ceee5a608e7c 25 * \param[in] f_J moment of inertia of the rotor
alexpirciu 1:ceee5a608e7c 26 * \param[in] f_K motor torque constant
alexpirciu 1:ceee5a608e7c 27 * \param[in] f_R electric resistance
alexpirciu 1:ceee5a608e7c 28 * \param[in] f_L electric inductance
alexpirciu 1:ceee5a608e7c 29 *
alexpirciu 1:ceee5a608e7c 30 */
alexpirciu 1:ceee5a608e7c 31 examples::systemmodels::ackermannmodel::CAckermannModel::CAckermannModel(
alexpirciu 1:ceee5a608e7c 32 const CStatesType& f_states
alexpirciu 1:ceee5a608e7c 33 ,const double f_dt
alexpirciu 1:ceee5a608e7c 34 ,const double f_gamma
alexpirciu 1:ceee5a608e7c 35 ,const double f_wb
alexpirciu 1:ceee5a608e7c 36 ,const double f_b
alexpirciu 1:ceee5a608e7c 37 ,const double f_J
alexpirciu 1:ceee5a608e7c 38 ,const double f_K
alexpirciu 1:ceee5a608e7c 39 ,const double f_R
alexpirciu 1:ceee5a608e7c 40 ,const double f_L)
alexpirciu 1:ceee5a608e7c 41 :CSystemModelType<double,2,10,5>(f_states,f_dt)
alexpirciu 1:ceee5a608e7c 42 ,m_gamma(f_gamma)
alexpirciu 1:ceee5a608e7c 43 ,m_wb(f_wb)
alexpirciu 1:ceee5a608e7c 44 ,m_bJ(f_b/f_J)
alexpirciu 1:ceee5a608e7c 45 ,m_KJ(f_K/f_J)
alexpirciu 1:ceee5a608e7c 46 ,m_KL(f_K/f_L)
alexpirciu 1:ceee5a608e7c 47 ,m_RL(f_R/f_L)
alexpirciu 1:ceee5a608e7c 48 ,m_L(f_L)
alexpirciu 1:ceee5a608e7c 49 {
alexpirciu 1:ceee5a608e7c 50 }
alexpirciu 1:ceee5a608e7c 51
alexpirciu 1:ceee5a608e7c 52 /** \brief Constructor for the CAckermannModel class
alexpirciu 1:ceee5a608e7c 53 *
alexpirciu 1:ceee5a608e7c 54 * Constructor method
alexpirciu 1:ceee5a608e7c 55 *
alexpirciu 1:ceee5a608e7c 56 * \param[in] f_dt sample time
alexpirciu 1:ceee5a608e7c 57 * \param[in] f_gamma reduction factor from motor to wheel
alexpirciu 1:ceee5a608e7c 58 * \param[in] f_wb wheel base
alexpirciu 1:ceee5a608e7c 59 * \param[in] f_b motor viscous friction constant
alexpirciu 1:ceee5a608e7c 60 * \param[in] f_J moment of inertia of the rotor
alexpirciu 1:ceee5a608e7c 61 * \param[in] f_K motor torque constant
alexpirciu 1:ceee5a608e7c 62 * \param[in] f_R electric resistance
alexpirciu 1:ceee5a608e7c 63 * \param[in] f_L electric inductance
alexpirciu 1:ceee5a608e7c 64 *
alexpirciu 1:ceee5a608e7c 65 */
alexpirciu 1:ceee5a608e7c 66 examples::systemmodels::ackermannmodel::CAckermannModel::CAckermannModel(
alexpirciu 1:ceee5a608e7c 67 const double f_dt
alexpirciu 1:ceee5a608e7c 68 ,const double f_gamma
alexpirciu 1:ceee5a608e7c 69 ,const double f_wb
alexpirciu 1:ceee5a608e7c 70 ,const double f_b
alexpirciu 1:ceee5a608e7c 71 ,const double f_J
alexpirciu 1:ceee5a608e7c 72 ,const double f_K
alexpirciu 1:ceee5a608e7c 73 ,const double f_R
alexpirciu 1:ceee5a608e7c 74 ,const double f_L)
alexpirciu 1:ceee5a608e7c 75 :CSystemModelType<double,2,10,5>(f_dt)
alexpirciu 1:ceee5a608e7c 76 ,m_gamma(f_gamma)
alexpirciu 1:ceee5a608e7c 77 ,m_wb(f_wb)
alexpirciu 1:ceee5a608e7c 78 ,m_bJ(f_b/f_J)
alexpirciu 1:ceee5a608e7c 79 ,m_KJ(f_K/f_J)
alexpirciu 1:ceee5a608e7c 80 ,m_KL(f_K/f_L)
alexpirciu 1:ceee5a608e7c 81 ,m_RL(f_R/f_L)
alexpirciu 1:ceee5a608e7c 82 ,m_L(f_L)
alexpirciu 1:ceee5a608e7c 83 {
alexpirciu 1:ceee5a608e7c 84 }
alexpirciu 1:ceee5a608e7c 85
alexpirciu 1:ceee5a608e7c 86 /** \brief Update method
alexpirciu 1:ceee5a608e7c 87 *
alexpirciu 1:ceee5a608e7c 88 * Method for updating system states.
alexpirciu 1:ceee5a608e7c 89 *
alexpirciu 1:ceee5a608e7c 90 * \param[in] f_input reference to input type object
alexpirciu 1:ceee5a608e7c 91 * \return system states
alexpirciu 1:ceee5a608e7c 92 */
alexpirciu 1:ceee5a608e7c 93 ackermannmodeltype::CStatesType
alexpirciu 1:ceee5a608e7c 94 examples::systemmodels::ackermannmodel::CAckermannModel::update(
alexpirciu 1:ceee5a608e7c 95 const CInputType& f_input)
alexpirciu 1:ceee5a608e7c 96 {
alexpirciu 1:ceee5a608e7c 97 CState l_states=m_states;
alexpirciu 1:ceee5a608e7c 98 CInput l_input=f_input;
alexpirciu 1:ceee5a608e7c 99 l_states.x()+=m_dt*l_states.x_dot();
alexpirciu 1:ceee5a608e7c 100 l_states.y()+=m_dt*l_states.y_dot();
alexpirciu 1:ceee5a608e7c 101
alexpirciu 1:ceee5a608e7c 102 l_states.x_dot_prev()=l_states.x_dot();
alexpirciu 1:ceee5a608e7c 103 l_states.y_dot_prev()=l_states.y_dot();
alexpirciu 1:ceee5a608e7c 104
alexpirciu 1:ceee5a608e7c 105 l_states.x_dot()=m_gamma*l_states.omega()*cos(l_states.teta_rad());
alexpirciu 1:ceee5a608e7c 106 l_states.y_dot()=m_gamma*l_states.omega()*sin(l_states.teta_rad());
alexpirciu 1:ceee5a608e7c 107
alexpirciu 1:ceee5a608e7c 108 double l_alpha_rad=l_input.alpha()*DEG2RAD;
alexpirciu 1:ceee5a608e7c 109 l_states.teta_rad_dot()=m_gamma*l_states.omega()*tan(l_alpha_rad)/m_wb;
alexpirciu 1:ceee5a608e7c 110 l_states.teta_rad()+=m_dt*l_states.teta_rad_dot();
alexpirciu 1:ceee5a608e7c 111
alexpirciu 1:ceee5a608e7c 112 double omega_k_1=(1-m_dt*m_bJ)*l_states.omega()+m_dt*m_KJ*l_states.i();//next state of the motor's rotation speed
alexpirciu 1:ceee5a608e7c 113 l_states.i()=-1*m_dt*m_KL*l_states.omega()+(1-m_dt*m_RL)*l_states.i()+m_dt*l_input.v()/m_L;
alexpirciu 1:ceee5a608e7c 114 l_states.omega()=omega_k_1;
alexpirciu 1:ceee5a608e7c 115 m_states=l_states;
alexpirciu 1:ceee5a608e7c 116 return l_states;
alexpirciu 1:ceee5a608e7c 117 }
alexpirciu 1:ceee5a608e7c 118
alexpirciu 1:ceee5a608e7c 119 /** \brief Calculate output method
alexpirciu 1:ceee5a608e7c 120 *
alexpirciu 1:ceee5a608e7c 121 * Method for calculating output depending on input.
alexpirciu 1:ceee5a608e7c 122 *
alexpirciu 1:ceee5a608e7c 123 * \param[in] f_input reference to input type object
alexpirciu 1:ceee5a608e7c 124 * \return systemoutputs
alexpirciu 1:ceee5a608e7c 125 */
alexpirciu 1:ceee5a608e7c 126 ackermannmodeltype::COutputType
alexpirciu 1:ceee5a608e7c 127 examples::systemmodels::ackermannmodel::CAckermannModel::calculateOutput(
alexpirciu 1:ceee5a608e7c 128 const CInputType& f_input)
alexpirciu 1:ceee5a608e7c 129 {
alexpirciu 1:ceee5a608e7c 130 CState l_states=m_states;
alexpirciu 1:ceee5a608e7c 131 // CInput l_input=f_input;
alexpirciu 1:ceee5a608e7c 132 COutput l_outputs(COutputType::zeros());
alexpirciu 1:ceee5a608e7c 133 l_outputs.x_ddot()=l_states.x_dot()-l_states.x_dot_prev();
alexpirciu 1:ceee5a608e7c 134 l_outputs.y_ddot()=l_states.y_dot()-l_states.y_dot_prev();
alexpirciu 1:ceee5a608e7c 135 l_outputs.teta_rad_dot()=l_states.teta_rad_dot();
alexpirciu 1:ceee5a608e7c 136 l_outputs.speed()=l_states.omega()*m_gamma;
alexpirciu 1:ceee5a608e7c 137 // output.alpha()=f_input.alpha();
alexpirciu 1:ceee5a608e7c 138 l_outputs.i()=l_states.i();
alexpirciu 1:ceee5a608e7c 139 m_outputs=l_outputs;
alexpirciu 1:ceee5a608e7c 140 return l_outputs;
alexpirciu 1:ceee5a608e7c 141 }