ackermannmodel.cpp

00001 /**
00002   ******************************************************************************
00003   * @file    AckermannModel.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 Ackermann model.
00008   ******************************************************************************
00009  */
00010 
00011 #include <Examples/SystemModels/ackermannmodel.hpp>
00012 
00013 /* Specify model type */
00014 using ackermannmodeltype = systemmodels::nlti::mimo::CDiscreteTimeSystemModel<double,2,10,5>;
00015 
00016 /** \brief  Constructor for the CAckermannModel class
00017  *
00018  *  Constructor method
00019  *
00020  *  \param[in] f_states       reference to initial system states 
00021  *  \param[in] f_dt           sample time
00022  *  \param[in] f_gamma        reduction factor from motor to wheel
00023  *  \param[in] f_wb           wheel base
00024  *  \param[in] f_b            motor viscous friction constant
00025  *  \param[in] f_J            moment of inertia of the rotor
00026  *  \param[in] f_K            motor torque constant
00027  *  \param[in] f_R            electric resistance 
00028  *  \param[in] f_L            electric inductance
00029  * 
00030  */
00031 examples::systemmodels::ackermannmodel::CAckermannModel::CAckermannModel(
00032                                      const CStatesType&     f_states
00033                                     ,const double            f_dt     
00034                                     ,const double            f_gamma
00035                                     ,const double            f_wb
00036                                     ,const double            f_b
00037                                     ,const double            f_J
00038                                     ,const double            f_K
00039                                     ,const double            f_R
00040                                     ,const double            f_L)
00041     :CSystemModelType<double,2,10,5>(f_states,f_dt)
00042     ,m_gamma(f_gamma)
00043     ,m_wb(f_wb)
00044     ,m_bJ(f_b/f_J)
00045     ,m_KJ(f_K/f_J)
00046     ,m_KL(f_K/f_L)
00047     ,m_RL(f_R/f_L)
00048     ,m_L(f_L)
00049 {
00050 }
00051 
00052 /** \brief  Constructor for the CAckermannModel class
00053  *
00054  *  Constructor method
00055  *
00056  *  \param[in] f_dt           sample time
00057  *  \param[in] f_gamma        reduction factor from motor to wheel
00058  *  \param[in] f_wb           wheel base
00059  *  \param[in] f_b            motor viscous friction constant
00060  *  \param[in] f_J            moment of inertia of the rotor
00061  *  \param[in] f_K            motor torque constant
00062  *  \param[in] f_R            electric resistance 
00063  *  \param[in] f_L            electric inductance
00064  * 
00065  */
00066 examples::systemmodels::ackermannmodel::CAckermannModel::CAckermannModel(
00067                                      const double      f_dt     
00068                                     ,const double      f_gamma
00069                                     ,const double      f_wb
00070                                     ,const double      f_b
00071                                     ,const double      f_J
00072                                     ,const double      f_K
00073                                     ,const double      f_R
00074                                     ,const double      f_L)
00075     :CSystemModelType<double,2,10,5>(f_dt)
00076     ,m_gamma(f_gamma)
00077     ,m_wb(f_wb)
00078     ,m_bJ(f_b/f_J)
00079     ,m_KJ(f_K/f_J)
00080     ,m_KL(f_K/f_L)
00081     ,m_RL(f_R/f_L)
00082     ,m_L(f_L)
00083 {
00084 }
00085 
00086 /** \brief  Update method
00087  *
00088  *  Method for updating system states.
00089  *
00090  *  \param[in] f_input        reference to input type object
00091  *  \return    system states
00092  */
00093 ackermannmodeltype::CStatesType 
00094     examples::systemmodels::ackermannmodel::CAckermannModel::update(
00095       const CInputType&       f_input)
00096 {
00097     CState l_states=m_states;
00098     CInput l_input=f_input;
00099     l_states.x()+=m_dt*l_states.x_dot();
00100     l_states.y()+=m_dt*l_states.y_dot();
00101 
00102     l_states.x_dot_prev()=l_states.x_dot();
00103     l_states.y_dot_prev()=l_states.y_dot();
00104 
00105     l_states.x_dot()=m_gamma*l_states.omega()*cos(l_states.teta_rad());
00106     l_states.y_dot()=m_gamma*l_states.omega()*sin(l_states.teta_rad());
00107 
00108     double l_alpha_rad=l_input.alpha()*DEG2RAD;
00109     l_states.teta_rad_dot()=m_gamma*l_states.omega()*tan(l_alpha_rad)/m_wb;
00110     l_states.teta_rad()+=m_dt*l_states.teta_rad_dot();
00111 
00112     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
00113     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;
00114     l_states.omega()=omega_k_1;
00115     m_states=l_states;
00116     return l_states;
00117 }
00118 
00119 /** \brief  Calculate output method
00120  *
00121  *  Method for calculating output depending on input.
00122  *
00123  *  \param[in] f_input        reference to input type object
00124  *  \return    systemoutputs
00125  */
00126 ackermannmodeltype::COutputType 
00127     examples::systemmodels::ackermannmodel::CAckermannModel::calculateOutput(
00128       const CInputType&  f_input)
00129 {
00130     CState l_states=m_states;
00131     // CInput l_input=f_input;
00132     COutput l_outputs(COutputType::zeros());
00133     l_outputs.x_ddot()=l_states.x_dot()-l_states.x_dot_prev();
00134     l_outputs.y_ddot()=l_states.y_dot()-l_states.y_dot_prev();
00135     l_outputs.teta_rad_dot()=l_states.teta_rad_dot();
00136     l_outputs.speed()=l_states.omega()*m_gamma;
00137     // output.alpha()=f_input.alpha();
00138     l_outputs.i()=l_states.i();
00139     m_outputs=l_outputs;
00140     return l_outputs;
00141 }