From bd94845152481cce8c5c15923c5b7f1ec1f5d5d2 Mon Sep 17 00:00:00 2001
From: Jens Ahrensfeld <jens@jayfield.org>
Date: Wed, 17 Apr 2019 20:09:42 +0000
Subject: [PATCH] - Mass: fixed delay algo

git-svn-id: http://moon:8086/svn/projects/HendiControl@250 fda53097-d464-4ada-af97-ba876c37ca34
---
 Control/brewpi/mass.py | 22 ++++++++++++++++++----
 1 file changed, 18 insertions(+), 4 deletions(-)

diff --git a/Control/brewpi/mass.py b/Control/brewpi/mass.py
index 27a2654..b0a1141 100644
--- a/Control/brewpi/mass.py
+++ b/Control/brewpi/mass.py
@@ -23,21 +23,35 @@ class Mass(APlant):
 		self.theta = 0
 		self.P = 0
 
-		if self.Td > 0:
-			self.alpha = self.dt/self.Td
+		self.alpha = self.dt/1
+
+		self.Nd = int(self.Td/self.dt)
+		self.delay = np.zeros((self.Nd, 1))
+		self.ri = 0
+		self.wi = 0
 
 	def activate(self, enable):
 		pass
 
 	def process(self):
+		if self.ri >= self.delay.size:
+			self.ri = 0
+		P = self.delay[self.ri][0]
+		self.ri += 1
+
 		self.e = self.e*(1 - ((self.L*self.theta)*self.dt)/(self.M*self.C))
-		self.x = (1-self.alpha)*self.x + self.gain*self.alpha*self.P
+		self.x = (1-self.alpha)*self.x + self.gain*self.alpha*P
 		self.e += self.x
 		self.theta = self.e/(self.M*self.C)
 
 	def setPower(self, power_w):
 		k = int(power_w / 100 +0.5)
-		self.P = k * 100
+		P = k * 100
+		if self.wi >= self.delay.size:
+			self.wi = 0
+		self.delay[self.wi] = P
+		self.wi += 1
+		self.P = P
 
 	def getPower(self):
 		return self.P