Alex Pirciu / Mbed 2 deprecated BFMC

a

Dependencies:   mbed mbed-rtos

include/Controllers/pidcontroller.inl@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 /** @brief Class constructor
alexpirciu 1:ceee5a608e7c 2 *
alexpirciu 1:ceee5a608e7c 3 * Constructor method
alexpirciu 1:ceee5a608e7c 4 *
alexpirciu 1:ceee5a608e7c 5 * @param f_kp proportional factor
alexpirciu 1:ceee5a608e7c 6 * @param f_ki integral factor
alexpirciu 1:ceee5a608e7c 7 * @param f_kd derivative factor
alexpirciu 1:ceee5a608e7c 8 * @param f_tf derivative time filter constant
alexpirciu 1:ceee5a608e7c 9 * @param f_dt sample time
alexpirciu 1:ceee5a608e7c 10 */
alexpirciu 1:ceee5a608e7c 11 template<class T>
alexpirciu 1:ceee5a608e7c 12 CPidController<T>::CPidController(T f_kp
alexpirciu 1:ceee5a608e7c 13 ,T f_ki
alexpirciu 1:ceee5a608e7c 14 ,T f_kd
alexpirciu 1:ceee5a608e7c 15 ,T f_tf
alexpirciu 1:ceee5a608e7c 16 ,T f_dt)
alexpirciu 1:ceee5a608e7c 17 :m_pidTf()
alexpirciu 1:ceee5a608e7c 18 ,m_dt(f_dt)
alexpirciu 1:ceee5a608e7c 19 {
alexpirciu 1:ceee5a608e7c 20 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});
alexpirciu 1:ceee5a608e7c 21 linalg::CMatrix<T,1,3> l_denPid({ 1.0,-(2*f_tf-f_dt)/f_tf,(f_tf-f_dt)/f_tf });
alexpirciu 1:ceee5a608e7c 22
alexpirciu 1:ceee5a608e7c 23 m_pidTf.setNum(l_numPid.transpose());
alexpirciu 1:ceee5a608e7c 24 m_pidTf.setDen(l_denPid.transpose());
alexpirciu 1:ceee5a608e7c 25 }
alexpirciu 1:ceee5a608e7c 26
alexpirciu 1:ceee5a608e7c 27 // template<class T>
alexpirciu 1:ceee5a608e7c 28 // CPidController<T>::CPidController( CPidSystemmodelType f_pid
alexpirciu 1:ceee5a608e7c 29 // ,T f_dt)
alexpirciu 1:ceee5a608e7c 30 // :m_pidTf(f_pid)
alexpirciu 1:ceee5a608e7c 31 // ,m_dt(f_dt){
alexpirciu 1:ceee5a608e7c 32
alexpirciu 1:ceee5a608e7c 33 // }
alexpirciu 1:ceee5a608e7c 34
alexpirciu 1:ceee5a608e7c 35
alexpirciu 1:ceee5a608e7c 36 /** @brief Operator
alexpirciu 1:ceee5a608e7c 37 *
alexpirciu 1:ceee5a608e7c 38 * @param f_input input function
alexpirciu 1:ceee5a608e7c 39 * \return control value
alexpirciu 1:ceee5a608e7c 40 */
alexpirciu 1:ceee5a608e7c 41 template<class T>
alexpirciu 1:ceee5a608e7c 42 T CPidController<T>::calculateControl(const T& f_input)
alexpirciu 1:ceee5a608e7c 43 {
alexpirciu 1:ceee5a608e7c 44 return m_pidTf(f_input);
alexpirciu 1:ceee5a608e7c 45 }
alexpirciu 1:ceee5a608e7c 46
alexpirciu 1:ceee5a608e7c 47 /** @brief Serial callback method
alexpirciu 1:ceee5a608e7c 48 *
alexpirciu 1:ceee5a608e7c 49 * Serial callback attaching serial callback to controller object
alexpirciu 1:ceee5a608e7c 50 *
alexpirciu 1:ceee5a608e7c 51 * @param obj PID controller object
alexpirciu 1:ceee5a608e7c 52 * @param a string to read data from
alexpirciu 1:ceee5a608e7c 53 * @param b string to write data to
alexpirciu 1:ceee5a608e7c 54 * \return None
alexpirciu 1:ceee5a608e7c 55 */
alexpirciu 1:ceee5a608e7c 56 template<class T>
alexpirciu 1:ceee5a608e7c 57 void CPidController<T>::staticSerialCallback(void* obj,char const * a, char * b)
alexpirciu 1:ceee5a608e7c 58 {
alexpirciu 1:ceee5a608e7c 59 CPidController* self = static_cast<CPidController*>(obj);
alexpirciu 1:ceee5a608e7c 60 self->serialCallback(a,b);
alexpirciu 1:ceee5a608e7c 61 }
alexpirciu 1:ceee5a608e7c 62
alexpirciu 1:ceee5a608e7c 63 /** @brief Reset to zero all memory of the controller.
alexpirciu 1:ceee5a608e7c 64 *
alexpirciu 1:ceee5a608e7c 65 */
alexpirciu 1:ceee5a608e7c 66 template<class T>
alexpirciu 1:ceee5a608e7c 67 void CPidController<T>::clear()
alexpirciu 1:ceee5a608e7c 68 {
alexpirciu 1:ceee5a608e7c 69 m_pidTf.clearMemmory();
alexpirciu 1:ceee5a608e7c 70 }
alexpirciu 1:ceee5a608e7c 71
alexpirciu 1:ceee5a608e7c 72 /** @brief Set the parameter of the controller
alexpirciu 1:ceee5a608e7c 73 *
alexpirciu 1:ceee5a608e7c 74 *
alexpirciu 1:ceee5a608e7c 75 * @param f_kp proportional factor
alexpirciu 1:ceee5a608e7c 76 * @param f_ki integral factor
alexpirciu 1:ceee5a608e7c 77 * @param f_kd derivative factor
alexpirciu 1:ceee5a608e7c 78 * @param f_tf derivative time filter constant
alexpirciu 1:ceee5a608e7c 79 */
alexpirciu 1:ceee5a608e7c 80 template<class T>
alexpirciu 1:ceee5a608e7c 81 void CPidController<T>::setController(
alexpirciu 1:ceee5a608e7c 82 T f_kp,
alexpirciu 1:ceee5a608e7c 83 T f_ki,
alexpirciu 1:ceee5a608e7c 84 T f_kd,
alexpirciu 1:ceee5a608e7c 85 T f_tf)
alexpirciu 1:ceee5a608e7c 86 {
alexpirciu 1:ceee5a608e7c 87 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 });
alexpirciu 1:ceee5a608e7c 88 linalg::CMatrix<T,1,3> l_denPid({ 1,-(2*f_tf-m_dt)/f_tf,(f_tf-m_dt)/f_tf });
alexpirciu 1:ceee5a608e7c 89
alexpirciu 1:ceee5a608e7c 90 m_pidTf.setNum(l_numPid.transpose());
alexpirciu 1:ceee5a608e7c 91 m_pidTf.setDen(l_denPid.transpose());
alexpirciu 1:ceee5a608e7c 92 }
alexpirciu 1:ceee5a608e7c 93
alexpirciu 1:ceee5a608e7c 94 /** @brief Serial callback method setting controller to values received
alexpirciu 1:ceee5a608e7c 95 *
alexpirciu 1:ceee5a608e7c 96 *
alexpirciu 1:ceee5a608e7c 97 * @param a string to read data from
alexpirciu 1:ceee5a608e7c 98 * @param b string to write data to
alexpirciu 1:ceee5a608e7c 99 */
alexpirciu 1:ceee5a608e7c 100 template<class T>
alexpirciu 1:ceee5a608e7c 101 void CPidController<T>::serialCallback(char const * a, char * b)
alexpirciu 1:ceee5a608e7c 102 {
alexpirciu 1:ceee5a608e7c 103 float l_kp,l_ki,l_kd,l_tf;
alexpirciu 1:ceee5a608e7c 104 uint32_t l_res = sscanf(a,"%f;%f;%f;%f;",&l_kp,&l_ki,&l_kd,&l_tf);
alexpirciu 1:ceee5a608e7c 105 if (4 == l_res)
alexpirciu 1:ceee5a608e7c 106 {
alexpirciu 1:ceee5a608e7c 107 setController(l_kp,l_ki,l_kd,l_tf);
alexpirciu 1:ceee5a608e7c 108 sprintf(b,"ack;;%2.5f;%2.5f;%2.5f;%2.5f;",l_kp,l_ki,l_kd,l_tf);
alexpirciu 1:ceee5a608e7c 109 }
alexpirciu 1:ceee5a608e7c 110 else
alexpirciu 1:ceee5a608e7c 111 {
alexpirciu 1:ceee5a608e7c 112 sprintf(b,"sintax error;;");
alexpirciu 1:ceee5a608e7c 113 }
alexpirciu 1:ceee5a608e7c 114 }