Diff: include/Controllers/pidcontroller.inl

Revision:

1:ceee5a608e7c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/include/Controllers/pidcontroller.inl	Thu Mar 28 07:44:42 2019 +0000
@@ -0,0 +1,114 @@
+/** @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;;");
+    }
+}