Skip to content
Snippets Groups Projects
Forked from JEDI / base-intersemestre-jedi
1 commit behind the upstream repository.
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