From 39a4aac8814afe9289b150f8bdbd0ed4f4e6ea7a Mon Sep 17 00:00:00 2001 From: Murad Abu-Khalaf Date: Fri, 26 Mar 2021 14:54:38 -0400 Subject: [PATCH] clarify controller sign, rename error calculation to reflect block diagram 1 in paper --- testClosedLoopController.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/testClosedLoopController.py b/testClosedLoopController.py index 089009e..6b6ffe8 100755 --- a/testClosedLoopController.py +++ b/testClosedLoopController.py @@ -169,11 +169,14 @@ def sensorCallback(weak_self, image): def PControl(self, img_err): if self.PGain is None: - K = np.ones((3, 128, 128)) + K = np.ones((3, 128, 128)) / 50.0 self.PGain = K K = self.PGain - u = -np.sum(K * (img_err) / 50.0) + f = np.sum(K * img_err ) + f_bar = 0 + f_tilda = f_bar - f + u = f_tilda #print(u) #u = np.clip(u, -0.5, 0.5) #print(u) @@ -208,7 +211,7 @@ def PControl(self, img_err): self.distance = np.concatenate((self.distance, [self.row]), axis=0) return u - def getErrorSignal(self, yref): + def getErrorSignal_BlockDiagram1(self, yref): #if self.img_ref is None: net = self.NN #print(self.img) @@ -228,7 +231,6 @@ def getErrorSignal(self, yref): #print(img_err) return img_err - def main(): """ The main method implements the game loop. @@ -258,7 +260,7 @@ def main(): display.blit(envCar.surface, (0, 0)) if envCar.img is not None: - img_err = envCar.getErrorSignal(10) + img_err = envCar.getErrorSignal_BlockDiagram1(10) envCar.PControl(img_err) pygame.display.flip()