Diff: RST.cpp

Revision:

0:b5906c81772b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RST.cpp	Sun Feb 05 16:31:58 2017 +0000
@@ -0,0 +1,74 @@
+#include "RST.hpp"
+
+RST::RST() {
+    R = 0;
+    S = 0;
+    T = 0;
+}
+
+float RST::calculateCmd()
+{
+    static float last_references[ControllerParams::MAX_ORD];
+    static float last_outputs[ControllerParams::MAX_ORD];
+    static float last_commands[ControllerParams::MAX_ORD];
+    
+    float R_component;
+    float S_component;
+    float T_component;
+    float outputPWM;
+    
+    
+    for(int i=0;i<(ordR+1);i++)
+    {
+        last_references[0] = this->ref;
+        R_component = R_component + R[i]*last_references[i];
+    }
+    for(int i=0;i<ordR;i++)
+    {
+        last_references[i+1] = last_references[i];
+    }
+    for(int i=0;i<(ordT+1);i++)
+    {
+        last_outputs[0] = this->out;
+        T_component = T_component + T[i]*last_outputs[i];
+    }
+    for(int i=0;i<ordT;i++)
+    {
+        last_outputs[i+1] = last_outputs[i];
+    }
+    for(int i=1;i<(ordS+1);i++)
+    {
+        S_component = S_component + last_commands[i]*S[i];
+    }
+    
+    outputPWM = T_component - R_component - S_component;
+    
+    for(int i=0;i<ordS;i++)
+    {
+        last_commands[i+1] = last_commands[i];
+    }
+    
+    return outputPWM; 
+}
+
+void RST::updateParams(ControllerParams &cp) {
+    ordR = cp.ordR;
+    ordS = cp.ordS;
+    ordT = cp.ordT; 
+
+    R = new float[ordR+1];
+    S = new float[ordS+1];
+    T = new float[ordT+1];
+    
+    int i;
+    for (i=0; i<ordR+1; i++) R[i] = cp.R[i];
+    for (i=0; i<ordS+1; i++) S[i] = cp.S[i];
+    for (i=0; i<ordT+1; i++) T[i] = cp.T[i];
+}
+
+
+RST::~RST() {
+    delete [] R;
+    delete [] S;
+    delete [] T;
+}