Select Git revision
Forked from
JEDI / base-intersemestre-jedi
Source project has a limited visibility.
droids.py 9.35 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
from controller import IntersectionType, get_intersection_type
import threading
from time import sleep
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.controller.adjust()
print("Going forward with motor speeds of", self.controller.m1_speed, 'and', self.controller.m2_speed)
self.motors.forward(lspeed=self.controller.m1_speed, rspeed=self.controller.m2_speed)
def backward(self):
"""
Sets the motor backward
"""
#self.controller.error_log = [] #Not sure
#self.controller.adjust()
self.motors.backward(lspeed=self.controller.m1_speed, rspeed=self.controller.m2_speed)
def stop(self):
"""
Stops the motors
"""
self.controller.error_log = [] #Not sure
self.motors.stop()
def right(self):
"""
Spins the robot to the right
"""
self.controller.error_log = []
self.motors.right(speed=self.speed)
def left(self):
"""
Spins the robot to the left
"""
self.controller.error_log = []
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)
self.controller.start()
def forward(self):
"""
Sets the motor forward
"""
self.controller.adjust(MotorDirection(0))
print("Going forward with motor speeds of", self.controller.m1_speed, 'and', self.controller.m2_speed)
self.motors.forward(lspeed=self.controller.m1_speed, rspeed=self.controller.m2_speed)
def backward(self):
"""
Sets the motor backward
"""
#self.controller.error_log = [] #Not sure
self.controller.adjust(MotorDirection(1))
self.motors.backward(lspeed=self.controller.m1_speed, rspeed=self.controller.m2_speed)
def explore(self):
"""
Explores maze until the exit is found
"""
#TODO : Implement the maze exploration
def print_distance_and_state(self):
#print("Distance and states prints :")
while True:
#print("Distances ...")
e = []
for i in range(3):
#d = self.controller._get_robust_distance(self.controller._get_sensor(name=i))
s = self.controller._get_event(self.controller._get_sensor(name=i))
e.append(s[0])
#print(("Left", "Front", "Right")[i], "sensor distance :", d)
#print(("Left", "Front", "Right")[i], "sensor state :", s)
print("Events :", e)
print("Intersection type :", get_intersection_type(e[0], e[1], e[2]))
sleep(1)
def really_get_intersection_type(self):
e=[]
for i in range(3):
e.append(self.controller._get_event(self.controller._get_sensor(name=i))[0])
return get_intersection_type(e[0], e[1], e[2])
#thread = threading.Thread(
# target=print_distance_and_state(self), args=(self, ))
#thread.start()
sleep(0.5)
print("Exploring...")
intersection = really_get_intersection_type(self)
print("Intersection type :", intersection)
while not intersection is IntersectionType(0):
if intersection in [IntersectionType(i) for i in [0,1,2,6]]:
self.forward()
elif intersection in [IntersectionType(i) for i in [3,5]]:
self.right()
elif intersection is IntersectionType(4):
self.left()
intersection = really_get_intersection_type(self)
print ("Intersection type :", intersection)
self.stop()
print("Exploring finished")