Skip to content
Snippets Groups Projects
Commit 3285d83a authored by ROUE Simon, Claude, Yves's avatar ROUE Simon, Claude, Yves
Browse files

Plein plein de modifs !!!

parent a8584743
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
controller.py
+ 48
− 16
View file @ 3285d83a
......@@ -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):
"""
......
droids.py
+ 22
− 2
View file @ 3285d83a
......@@ -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]]:
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment