Skip to content
Snippets Groups Projects
Select Git revision
  • master default protected
  • simon+
  • simon
3 results

droids.py

Blame
  • Forked from JEDI / base-intersemestre-jedi
    2 commits behind, 4 commits ahead of the upstream repository.
    droids.py 6.42 KiB
    """
    File: droids.py
    Description: Provides classes for controlling the robot, either by hand, or autonomously.
    """
    
    #!/usr/bin/python3
    
    import sys
    import tty
    import termios
    from enum import IntEnum, unique
    import RPi.GPIO as GPIO
    from motor import Motor
    from sensors import HCSR04, ContinuousSensor
    from controller import PIDController
    from pins import RaspiPin, RaspiPinError
    
    
    class Droid:
        """
        Base class for the robot that provides basic pin matching
        """
    
        def __init__(self,
                     battery_voltage=9.0,
                     motor_voltage=6.0,
                     warn=True,
                     speed=0.5):
            GPIO.setmode(GPIO.BCM)
            GPIO.setwarnings(warn)
            self.speed = speed
            self.motors = Motor(battery_voltage, motor_voltage)
    
        def __del__(self):
            self._cleanup()
    
        def _set_led1(self, state):
            GPIO.output(RaspiPin.LED1_PIN, state)
    
        def _set_led2(self, state):
            GPIO.output(RaspiPin.LED2_PIN, state)
    
        def _set_oc1(self, state):
            GPIO.output(RaspiPin.OC1_PIN, state)
    
        def _set_oc2(self, state):
            GPIO.output(RaspiPin.OC2_PIN, state)
    
        def _cleanup(self):
            GPIO.cleanup()
    
        def forward(self):
            """
            Sets the motor forward
            """
            self.motors.forward(speed=self.speed)
    
        def backward(self):
            """
            Sets the motor backward
            """
            self.motors.backward(speed=self.speed)
    
        def stop(self):
            """
            Stops the motors
            """
            self.motors.stop()
    
        def right(self):
            """
            Spins the robot to the right
            """
            self.motors.right(speed=self.speed)
    
        def left(self):
            """
            Spins the robot to the left
            """
            self.motors.left(speed=self.speed)
    
    
    @unique
    class RoverKey(IntEnum):
        """
        Possible keys that can be read from the terminal
        """
        UP = 0
        DOWN = 1
        RIGHT = 2
        LEFT = 3
        STOP = 4
    
    
    class RoverDroid(Droid):
        """
        RoverDroid provides a terminal controller for the robot.
        The robot can be controlled by using the keyboard.
        """
    
        def __init__(self,
                     battery_voltage=9.0,
                     motor_voltage=6.0,
                     warn=True,
                     speed=0.5):
            Droid.__init__(self, battery_voltage, motor_voltage, warn, speed)
    
        def _read_char(self):
            """
            Read chars from keyboard
            """
            fd = sys.stdin.fileno()
            old_settings = termios.tcgetattr(fd)
            try:
                tty.setraw(sys.stdin.fileno())
                ch = sys.stdin.read(1)
            finally:
                termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
            if ch == '0x03':
                raise KeyboardInterrupt
            return ch
    
        def _read_key(self):
            """
            Converts keys from keyboard to directions to the droid
            """
            c1 = self._read_char()
            if ord(c1) != 0x1b:
                # escape char
                return RoverKey.STOP
            c2 = self._read_char()
            if ord(c2) != 0x5b:
                # [ char
                raise KeyboardInterrupt
            c3 = self._read_char()
            return RoverKey(ord(c3) - 65)  # 0=Up, 1=Down, 2=Right, 3=Left arrows
    
        def run(self):
            """
            Continuously read keys stroke from the keyboard and applies
            directions to current droid
            """
            try:
                print("Use the arrow keys to move the robot")
                print("Press CTRL-c to quit the program")
                while True:
                    key = self._read_key()
                    if key is RoverKey.UP:
                        print("ROVER", "FORWARD")
                        self.forward()
                    elif key is RoverKey.DOWN:
                        print("ROVER", "BACKWARD")
                        self.backward()
                    elif key is RoverKey.LEFT:
                        print("ROVER", "TURN LEFT")
                        self.left()
                    elif key is RoverKey.RIGHT:
                        print("ROVER", "TURN RIGHT")
                        self.right()
                    elif key is RoverKey.STOP:
                        print("ROVER", "STOP")
                        self.stop()
                    else:
                        print("ROVER", "UNKNOWN VALUE:", key)
                        raise KeyboardInterrupt
            except KeyboardInterrupt:
                self._cleanup()
    
    
    class Droid3Sensors(Droid):
        """
        Droid3Sensors provides the exploration of a maze with a controller to drives
        motors according to sensors
        """
    
        def __init__(self,
                     battery_voltage: float = 9.0,
                     motor_voltage: float = 6.0,
                     warn: bool = True,
                     speed: bool = 0.5,
                     low: float = 15.,
                     high: float = 50.,
                     target: float = 20.,
                     KP: float = 0.05,
                     KD: float = 0.005,
                     KI: float = 0.,
                     pid_sample_time: float = 0.01,
                     sensor_sampling_rate: float = 500 * 1e-3):
            Droid.__init__(self, battery_voltage, motor_voltage, warn, speed)
            print("Robot 3 Sensors with controller", "speed=%02f" % speed)
            self.left_sensor = ContinuousSensor(
                HCSR04("LEFT",
                       RaspiPin.ECHO_PIN_3,
                       RaspiPin.TRIGGER_PIN_3,
                       low=low,
                       high=high), sensor_sampling_rate)
            self.front_sensor = ContinuousSensor(
                HCSR04("FRONT",
                       RaspiPin.ECHO_PIN_1,
                       RaspiPin.TRIGGER_PIN_1,
                       low=2 * low,
                       high=high), sensor_sampling_rate)
            self.right_sensor = ContinuousSensor(
                HCSR04("RIGHT",
                       RaspiPin.ECHO_PIN_2,
                       RaspiPin.TRIGGER_PIN_2,
                       low=low,
                       high=high), sensor_sampling_rate)
            self.controller = PIDController(self.motors,
                                            self.left_sensor,
                                            self.front_sensor,
                                            self.right_sensor,
                                            speed=speed,
                                            target=target,
                                            KP=KP,
                                            KD=KD,
                                            KI=KI,
                                            pid_sample_time=pid_sample_time)
    
        def explore(self):
            """
            Explores maze until the exit is found
            """
            #TODO : Implement the maze exploration
            pass