diff --git a/controller.py b/controller.py index 164d981e793fcec6be5c4b1a200649734fdef468..8489020858952289855b54215713448b04cefa10 100644 --- a/controller.py +++ b/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: - 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 + """ + 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 + if self.error_log == []: self.error_log.append(e) - sleep(pid_sample_time) + 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) + 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): """ diff --git a/droids.py b/droids.py index 26f1ed6f0c35c83587bbbdc3ab344d069700aebf..7d19b6836760fac3372f4cf6b0735f546245f111 100644 --- a/droids.py +++ b/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]]: