Alex Pirciu
/
BFMC
a
include/Controllers/pidcontroller.inl
- Committer:
- alexpirciu
- Date:
- 2019-03-28
- Revision:
- 1:ceee5a608e7c
File content as of revision 1:ceee5a608e7c:
/** @brief Class constructor * * Constructor method * * @param f_kp proportional factor * @param f_ki integral factor * @param f_kd derivative factor * @param f_tf derivative time filter constant * @param f_dt sample time */ template<class T> CPidController<T>::CPidController(T f_kp ,T f_ki ,T f_kd ,T f_tf ,T f_dt) :m_pidTf() ,m_dt(f_dt) { linalg::CMatrix<T,1,3> l_numPid({ (f_kd+f_tf*f_kp)/f_tf , (f_tf*f_dt*f_ki+f_dt*f_kp-2*f_tf*f_kp-2*f_kd)/f_tf , (f_kd+f_tf*f_kp+f_dt*f_dt*f_ki-f_dt*f_tf*f_ki-f_dt*f_kp)/f_tf}); linalg::CMatrix<T,1,3> l_denPid({ 1.0,-(2*f_tf-f_dt)/f_tf,(f_tf-f_dt)/f_tf }); m_pidTf.setNum(l_numPid.transpose()); m_pidTf.setDen(l_denPid.transpose()); } // template<class T> // CPidController<T>::CPidController( CPidSystemmodelType f_pid // ,T f_dt) // :m_pidTf(f_pid) // ,m_dt(f_dt){ // } /** @brief Operator * * @param f_input input function * \return control value */ template<class T> T CPidController<T>::calculateControl(const T& f_input) { return m_pidTf(f_input); } /** @brief Serial callback method * * Serial callback attaching serial callback to controller object * * @param obj PID controller object * @param a string to read data from * @param b string to write data to * \return None */ template<class T> void CPidController<T>::staticSerialCallback(void* obj,char const * a, char * b) { CPidController* self = static_cast<CPidController*>(obj); self->serialCallback(a,b); } /** @brief Reset to zero all memory of the controller. * */ template<class T> void CPidController<T>::clear() { m_pidTf.clearMemmory(); } /** @brief Set the parameter of the controller * * * @param f_kp proportional factor * @param f_ki integral factor * @param f_kd derivative factor * @param f_tf derivative time filter constant */ template<class T> void CPidController<T>::setController( T f_kp, T f_ki, T f_kd, T f_tf) { linalg::CMatrix<T,1,3> l_numPid({ (f_kd+f_tf*f_kp)/f_tf , (f_tf*m_dt*f_ki+m_dt*f_kp-2*f_tf*f_kp-2*f_kd)/f_tf , (f_kd+f_tf*f_kp+m_dt*m_dt*f_ki-m_dt*f_tf*f_ki-m_dt*f_kp)/f_tf }); linalg::CMatrix<T,1,3> l_denPid({ 1,-(2*f_tf-m_dt)/f_tf,(f_tf-m_dt)/f_tf }); m_pidTf.setNum(l_numPid.transpose()); m_pidTf.setDen(l_denPid.transpose()); } /** @brief Serial callback method setting controller to values received * * * @param a string to read data from * @param b string to write data to */ template<class T> void CPidController<T>::serialCallback(char const * a, char * b) { float l_kp,l_ki,l_kd,l_tf; uint32_t l_res = sscanf(a,"%f;%f;%f;%f;",&l_kp,&l_ki,&l_kd,&l_tf); if (4 == l_res) { setController(l_kp,l_ki,l_kd,l_tf); sprintf(b,"ack;;%2.5f;%2.5f;%2.5f;%2.5f;",l_kp,l_ki,l_kd,l_tf); } else { sprintf(b,"sintax error;;"); } }