Forked from
JEDI / base-intersemestre-jedi
1 commit behind the upstream repository.
-
HUIN Nicolas authoredHUIN Nicolas authored
controller.py 6.25 KiB
"""
File: controller.py
Description: Provides a PID controller for the robot that deals with motor speed
"""
from enum import IntEnum, unique
from motor import Motor
from sensors import HCSR04, HCSR04Event, ContinuousSensor
@unique
class IntersectionType(IntEnum):
"""
Possible intersection type encountered in the maze
"""
PathFour = 0
PathThreeLeftFront = 1
PathThreeRightFront = 2
PathThreeLeftRight = 3
PathTwoLeft = 4
PathTwoRight = 5
PathTwoFront = 6
PathOne = 7
PathZero = 8
def danger(le: HCSR04Event, fe: HCSR04Event, re: HCSR04Event):
"""
Returns True if one sensor among the three ones measures a distance
below its lower threshold, False otherwise
"""
return (le is HCSR04Event.DANGER or fe is HCSR04Event.DANGER
or re is HCSR04Event.DANGER)
def is_exit(le: HCSR04Event, fe: HCSR04Event, re: HCSR04Event):
"""
Returns True if the maze exit is reached, False otherwise
"""
#TODO
pass
def is_intersection(le: HCSR04Event, fe: HCSR04Event, re: HCSR04Event):
"""
returns True if the droid is currently in an intersection, False otherwise
"""
#TODO
pass
def is_dead_end(le: HCSR04Event, fe: HCSR04Event, re: HCSR04Event):
"""
Returns True if the droid is in a dead end, False otherwise
"""
#TODO
pass
def is_corridor(le: HCSR04Event, fe: HCSR04Event, re: HCSR04Event):
"""
Returns True if the Droid is in a corridor, False otherwise
"""
#TODO
pass
def get_intersection_type(le, fe, re):
"""
returns an IntersectionType according to the measured values on the
three sensors
"""
if is_exit(le, fe, re):
return IntersectionType.PathFour
if is_intersection(le, fe, re):
if le is HCSR04Event.NOTHING and fe is HCSR04Event.NOTHING and re is not HCSR04Event.NOTHING:
return IntersectionType.PathThreeLeftFront
if (le is not HCSR04Event.NOTHING and fe is HCSR04Event.NOTHING
and re is HCSR04Event.NOTHING):
return IntersectionType.PathThreeRightFront
if (le is HCSR04Event.NOTHING and fe is not HCSR04Event.NOTHING
and re is HCSR04Event.NOTHING):
return IntersectionType.PathThreeLeftRight
if (le is HCSR04Event.NOTHING and fe is not HCSR04Event.NOTHING
and re is not HCSR04Event.NOTHING):
return IntersectionType.PathTwoLeft
if (le is not HCSR04Event.NOTHING and fe is not HCSR04Event.NOTHING
and re is HCSR04Event.NOTHING):
return IntersectionType.PathTwoRight
else:
raise ValueError("Unknown situation")
elif is_dead_end(le, fe, re):
return IntersectionType.PathOne
elif is_corridor(le, fe, re):
return IntersectionType.PathTwoFront
else:
return IntersectionType.PathZero
class Controller:
"""
Class to control motors according to sensed values.
"""
def __init__(self,
motors: Motor,
lsensor: ContinuousSensor,
fsensor: ContinuousSensor,
rsensor: ContinuousSensor,
speed=0.5):
self.motors = motors
self.speed = speed
self.left_sensor = lsensor
self.front_sensor = fsensor
self.right_sensor = rsensor
def start(self):
"""
Starts the threads for computing the distance of the sensor
"""
for sensor in [self.left_sensor, self.front_sensor, self.right_sensor]:
sensor.start()
def stop(self):
"""
Starts the threads for computing the distance of the sensor
"""
for sensor in [self.left_sensor, self.front_sensor, self.right_sensor]:
sensor.stop()
def _get_event(self, sensor: ContinuousSensor, moving=True):
"""
Returns a HCSR04Event according to the distance measured by the sensor
:param bool moving: Indicates if the robot is moving
"""
distance = self._get_robust_distance(sensor, moving)
if distance < sensor.low:
return (HCSR04Event.DANGER, distance)
if distance < sensor.high:
return (HCSR04Event.WALL, distance)
return (HCSR04Event.NOTHING, distance)
def _get_robust_distance(self, sensor: HCSR04, moving=True):
"""
Robust measurement on sensor, stops motors if necessary and restart
them according to an acceptable measurement.
"""
#TODO Implement a robust way to read the distance from the sensor
return sensor.get_distance()
class PIDController(Controller):
"""
A PIDController manages the speed of the motors according to the PID paradigm.
Read https://en.wikipedia.org/wiki/PID_controller and
https://projects.raspberrypi.org/en/projects/robotPID/5
"""
def __init__(self,
motors: Motor,
lsensor: ContinuousSensor,
fsensor: ContinuousSensor,
rsensor: ContinuousSensor,
speed=0.7,
target=20.,
KP=0.05,
KD=0.005,
KI=0.,
pid_sample_time=0.01):
Controller.__init__(self,
motors,
lsensor,
fsensor,
rsensor,
speed=speed)
self.m1_speed = self.speed
self.m2_speed = self.speed
self.target = target
# https://en.wikipedia.org/wiki/PID_controller
self.pid_sample_time = pid_sample_time
self.last_sample = 0.0
self.KP = KP
self.KD = KD
self.KI = KI
self.prev_error = 0.0
self.sum_error = 0.0
def adjust(self):
"""
Adjusts motors speeds to compensate error on objective function
(namely a distance on left or right wall)
"""
#TODO Implement
pass
def go_to_the_next_intersection(self):
"""
Drives the droid 'safely' to the next intersection.
Safely meaning that the droids to not meet a wall and stops when it detects
an IntersectionType that is not PathTwoFront, i.e., the droid is not in
a corridor anymore
"""
#TODO Implement
pass