Diff: RSTController.cpp

Revision:

3:b6e4e5529a52

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RSTController.cpp	Mon May 08 13:05:45 2017 +0000
@@ -0,0 +1,88 @@
+#include "RSTController.hpp"
+
+RSTController::RSTController() {
+	R = 0;
+	S = 0;
+	T = 0;
+}
+
+void RSTController::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];
+    }
+    
+    cmd = outputPWM;
+}
+
+void RSTController::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];
+}
+
+RSTController& RSTController::operator=(const RSTController& rst) {
+	
+	this->ordR = rst.ordR;
+	this->ordS = rst.ordS;
+	this->ordT = rst.ordT;
+
+	int i;
+	for(i = 0; i<ordR+1; i++)	this->R[i] = rst.R[i]; 	
+	for(i = 0; i<ordS+1; i++)	this->S[i] = rst.S[i]; 	
+	for(i = 0; i<ordT+1; i++)	this->T[i] = rst.T[i]; 	
+	
+	return *this;
+}
+
+
+RSTController::~RSTController() {
+	delete [] R;
+	delete [] S;
+	delete [] T;
+}