Plein plein de modifs !!!

controller.py

@@ -4,7 +4,7 @@ Description: Provides a PID controller for the robot that deals with motor speed
 """
 
 from enum import IntEnum, unique
-from motor import Motor
+from motor import Motor, MotorDirection
 from sensors import HCSR04, HCSR04Event, ContinuousSensor
 from time import sleep
 from math import inf
@@ -170,8 +170,8 @@ class Controller:
             #print("Finite distance")
             return sum([i for i in measures if i!=inf]) / 1
 
-    ###TESTING
-    def _get_sensor(self, name:int):
+    #TO DELETE
+    def _get_sensor(self, name:int): #Useless
         if name==0:
             #print("Returning left sensor")
             return self.left_sensor
@@ -199,7 +199,7 @@ class PIDController(Controller):
                  target=20.,
                  KP=0.001,
                  KD=0., #0.005
-                 KI=0., # 0.05
+                 KI=0.05, # 0.05
                  pid_sample_time=0.5):
         Controller.__init__(self,
                             motors,
@@ -220,29 +220,61 @@ class PIDController(Controller):
         #self.sum_error = 0.0
         self.error_log = []
 
-    def adjust(self):
+    def adjust(self, direction:MotorDirection = MotorDirection(0)):
         """
         Adjusts motors speeds to compensate error on objective function
         (namely a distance on left or right wall)
         """
         #TODO Implement
-        while this.error_log == []:
+        """
+        while self.error_log == []:
             distance_left = self._get_robust_distance(self.left_sensor)
             distance_right = self._get_robust_distance(self.right_sensor)
             if distance_left < inf and distance_right < inf:
                 e = distance_right - distance_left
                 self.error_log.append(e)
-        while True:
+        """
+        if direction == MotorDirection(0):
+            sign = 1
+        elif direction == MotorDirection(1):
+            sign = -1
+        else:
+            print("Wrong direction")
+            pass
+
         distance_left = self._get_robust_distance(self.left_sensor)
         distance_right = self._get_robust_distance(self.right_sensor)
         if distance_left < inf and distance_right < inf:
             e = distance_right - distance_left
-                self.m1_speed = min(0.7, max(0.3, 0.5 + self.KP*e)) 
-                # + self.KI*sum(self.error_log)/len(self.error_log) + self.KD*(e-self.error_log[-1])/pid_sample_time
-                self.m2_speed = min(0.7, max(0.3, 0.5 - self.KP*e)) 
+            if self.error_log == []:
+                self.error_log.append(e)
+            print("Error =", e)
+            print("Error log :", self.error_log)
+            P = self.KP*e
+            I = self.KI*sum(self.error_log)/len(self.error_log)
+            D = self.KD*(e-self.error_log[-1])/pid_sample_time)))
+            print("PID factors :\n\tP =", P, "\n\tI =", I, "\n\tD =", D)
+            self.m1_speed = min(0.7, max(0.3, 
+                    0.5 + sign*(
+                    self.KP*e
+                    + self.KI*sum(self.error_log)/len(self.error_log)
+                    + self.KD*(e-self.error_log[-1])/pid_sample_time)))
+            self.m2_speed = min(0.7, max(0.3, 
+                    0.5 - sign*(
+                    self.KP*e
+                    + self.KI*sum(self.error_log)/len(self.error_log)
+                    + self.KD*(e-self.error_log[-1])/pid_sample_time)))
             # - self.KI*sum(self.error_log)/len(self.error_log) - self.KD*(e-self.error_log[-1])/pid_sample_time
             self.error_log.append(e)
-            sleep(pid_sample_time)
+        else:
+            self.m1_speed = min(0.7, max(0.3, 
+                    0.5 + sign*(
+                    self.KI*sum(self.error_log)/len(self.error_log)
+                    + self.KD*(e-self.error_log[-1])/pid_sample_time)))
+            self.m2_speed = min(0.7, max(0.3, 
+                    0.5 - sign*(
+                    self.KI*sum(self.error_log)/len(self.error_log)
+                    + self.KD*(e-self.error_log[-1])/pid_sample_time)))
 
     def go_to_the_next_intersection(self):
         """

droids.py

@@ -55,6 +55,7 @@ class Droid:
         """
         Sets the motor forward
         """
+        #self.controller.adjust()
         print("Going forward with motor speeds of", self.controller.m1_speed, 'and', self.controller.m2_speed)
         self.motors.forward(lspeed=self.controller.m1_speed, rspeed=self.controller.m2_speed)
 
@@ -62,24 +63,29 @@ class Droid:
         """
         Sets the motor backward
         """
+        #self.controller.error_log = [] #Not sure
+        #self.controller.adjust()
         self.motors.backward(lspeed=self.controller.m1_speed, rspeed=self.controller.m2_speed)
 
     def stop(self):
         """
         Stops the motors
         """
+        self.controller.error_log = [] #Not sure
         self.motors.stop()
 
     def right(self):
         """
         Spins the robot to the right
         """
+        self.controller.error_log = []
         self.motors.right(speed=self.speed)
 
     def left(self):
         """
         Spins the robot to the left
         """
+        self.controller.error_log = []
         self.motors.left(speed=self.speed)
 
 
@@ -221,6 +227,21 @@ class Droid3Sensors(Droid):
                                         pid_sample_time=pid_sample_time)
         self.controller.start()
 
+    def forward(self):
+        """
+        Sets the motor forward
+        """
+        self.controller.adjust(MotorDirection(0))
+        print("Going forward with motor speeds of", self.controller.m1_speed, 'and', self.controller.m2_speed)
+        self.motors.forward(lspeed=self.controller.m1_speed, rspeed=self.controller.m2_speed)
+
+    def backward(self):
+        """
+        Sets the motor backward
+        """
+        #self.controller.error_log = [] #Not sure
+        self.controller.adjust(MotorDirection(1))
+        self.motors.backward(lspeed=self.controller.m1_speed, rspeed=self.controller.m2_speed)
     def explore(self):
         """
         Explores maze until the exit is found
@@ -255,11 +276,10 @@ class Droid3Sensors(Droid):
 
         sleep(0.5)
         print("Exploring...")
+
         intersection = really_get_intersection_type(self)
         print("Intersection type :", intersection)
-        #while intersection in [IntersectionType(i) for i in [0,1,2,6]]:
         while not intersection is IntersectionType(0):
-            self.controller.adjust()
             if intersection in [IntersectionType(i) for i in [0,1,2,6]]:
                 self.forward()
             elif intersection in [IntersectionType(i) for i in [3,5]]: