* Add integrator function for error correction, #78

RWTH-EBC · May 31, 2020 · 85374c8 · 85374c8
1 parent 9d9a68d
commit 85374c8
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 1 deletion.
diff --git a/pyDMPC/ControlFramework/Modeling.py b/pyDMPC/ControlFramework/Modeling.py
@@ -310,6 +310,21 @@ def predict(self):
         self.states.outputs = [[self.modifs.linear_model_factors[0] * self.states.inputs[0] +
                                self.modifs.linear_model_factors[1] * self.states.commands[0]]]
 
+class Integrator(Model):
+
+    def __init__(self, sys_id):
+        super().__init__(sys_id)
+        self.modifs = Modifs(sys_id)
+
+    def predict(self):
+        if self.states.state_vars[2] < self.states.set_points[0] - 1:
+            self.states.commands[0] = self.states.state_vars[1] + self.states.set_points[1] - self.states.state_vars[0]
+        self.states.outputs = [[self.states.inputs[0] + 273.15]]
+
+        self.states.set_points[0] = min(max(288, 295 + self.modifs.linear_model_factors[0] * self.states.commands[0]), 303)
+        if self.states.set_points[0] < self.states.state_vars[2]: 
+            self.states.set_points[0] = self.states.state_vars[2]
+
 class FuzMod(Model):
 
     def __init__(self, sys_id):

diff --git a/pyDMPC/ControlFramework/Subsystem.py b/pyDMPC/ControlFramework/Subsystem.py
@@ -2,6 +2,7 @@
 import Modeling
 import System
 import Time
+import time
 
 class Subsystem:
 
@@ -114,6 +115,8 @@ def prepare_model(self):
             model = Modeling.LinMod(self.sys_id)
         elif self.model_type == "Fuzzy":
             model = Modeling.FuzMod(self.sys_id)
+        elif self.model_type == "Integrator":
+            model = Modeling.Integrator(self.sys_id)
         elif self.model_type == "StateSpace":
             model = Modeling.StateSpace(self.sys_id)
         return model
@@ -236,6 +239,7 @@ def calc_cost(self, command, outputs):
             for c in self.cost_rec:
                 if type(c) is scipy.interpolate.interpolate.interp1d:
                     cost += self.cost_fac[1] * c(outputs)
+                    print(c(outputs))
                 elif type(c) is list:
                     idx = self.find_nearest(np.asarray(self.inputs), outputs)
                     cost += self.cost_fac[1] * c[idx]
@@ -246,6 +250,7 @@ def calc_cost(self, command, outputs):
             cost += (self.cost_fac[2] * (outputs -
                                  self.model.states.set_points[0])**2)
 
+
         return cost
 
     def find_nearest(self, a, a0):
@@ -280,6 +285,8 @@ def interp(self, iter_real):
             else:
                 self.fin_coup_vars = self.coup_vars_send
 
+        print(f"self.fin_coup_vars: {self.fin_coup_vars}")
+
 
     def get_inputs(self):
 
@@ -323,7 +330,10 @@ def get_outputs(self):
 
     def send_commands(self):
 
-        cur_time = Time.Time.get_time()
+        cur_time = time.time()
+
+        print(f"Difference: {cur_time - self.last_write}")
+        print(f"Samp Time: {self.model.times.samp_time}")
 
         if (cur_time - self.last_write) > self.model.times.samp_time:
             self.last_write = cur_time