Select Git revision
Forked from
JEDI / base-intersemestre-jedi
2 commits behind, 4 commits ahead of the upstream repository.
ROUE Simon, Claude, Yves authored
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