Diff: RST.cpp

Revision:

1:d3e12393b71d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RST.cpp	Thu Jan 12 16:04:37 2017 +0000
@@ -0,0 +1,53 @@
+#include "RST.h"
+
+RST::RST(float* R, float* S, float* T, RST_aditionalData SP):m_R(R), m_S(S), m_T(T), m_SP(SP)
+{
+    //
+}
+
+float RST::ComputeCommand(float inputADC)
+{
+    static float last_references[MAX_GRADE];
+    static float last_outputs[MAX_GRADE];
+    static float last_commands[MAX_GRADE];
+    float R_component = 0;
+    float T_component = 0;
+    float S_component = 0;
+    float outputPWM;
+    
+    for(int i=0;i<(m_SP.gradR+1);i++)
+    {
+        last_references[0] = m_SP.setpoint;
+        R_component = R_component + m_R[i]*last_references[i];
+    }
+    for(int i=0;i<m_SP.gradR;i++)
+    {
+        last_references[i+1] = last_references[i];
+    }
+    for(int i=0;i<(m_SP.gradT+1);i++)
+    {
+        last_outputs[0] = inputADC;
+        T_component = T_component + m_T[i]*last_outputs[i];
+    }
+    for(int i=0;i<m_SP.gradT;i++)
+    {
+        last_outputs[i+1] = last_outputs[i];
+    }
+    for(int i=1;i<(m_SP.gradS+1);i++)
+    {
+        S_component = S_component + last_commands[i]*m_S[i];
+    }
+    
+    outputPWM = T_component - R_component - S_component;
+    last_commands[0] = outputPWM;
+    
+    for(int i=0;i<m_SP.gradS;i++)
+    {
+        last_commands[i+1] = last_commands[i];
+    }
+    
+    return outputPWM;
+}
+
+
+