droids.py

"""
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