Modifs pour rendre adjust un thread (incomplet)

controller.py

@@ -197,10 +197,10 @@ class PIDController(Controller):
                  rsensor: ContinuousSensor,
                  speed=0.7,
                  target=20.,
-                 KP=0.05,
-                 KD=0.005,
-                 KI=0.,
-                 pid_sample_time=0.01):
+                 KP=0.001,
+                 KD=0., #0.005
+                 KI=0., # 0.05
+                 pid_sample_time=0.5):
         Controller.__init__(self,
                             motors,
                             lsensor,
@@ -212,12 +212,13 @@ class PIDController(Controller):
         self.target = target
         # https://en.wikipedia.org/wiki/PID_controller
         self.pid_sample_time = pid_sample_time
-        self.last_sample = 0.0
+        #self.last_sample = 0.0
         self.KP = KP
         self.KD = KD
         self.KI = KI
-        self.prev_error = 0.0
-        self.sum_error = 0.0
+        #self.prev_error = 0.0
+        #self.sum_error = 0.0
+        self.error_log = []
 
     def adjust(self):
         """
@@ -225,13 +226,23 @@ class PIDController(Controller):
         (namely a distance on left or right wall)
         """
         #TODO Implement
+        while this.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.m1_speed = 0.5 + self.KP*e # + self.KI*... + self.KD*...
-            self.m2_speed = 0.5 - self.KP*e # - self.KI*... - self.KD*...
-        pass
+                self.error_log.append(e)
+        while True:
+            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)) 
+                # - 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)
 
     def go_to_the_next_intersection(self):
         """

droids.py

@@ -250,10 +250,10 @@ class Droid3Sensors(Droid):
             return get_intersection_type(e[0], e[1], e[2])
         
         #thread = threading.Thread(
+        #    target=print_distance_and_state(self), args=(self, ))
         #thread.start()
 
         sleep(0.5)
-        #    target=print_distance_and_state(self), args=(self, ))
         print("Exploring...")
         intersection = really_get_intersection_type(self)
         print("Intersection type :", intersection)