def initialize_robot(self):
"""Initialize robot interfaces (sensors and actuators) and its brain from configuration"""
self.stop_interfaces()
self.actuators = Actuators(self.configuration.actuators)
self.sensors = Sensors(self.configuration.sensors)
self.brains = Brains(self.sensors, self.actuators,
self.configuration.brain_path, self.controller)
self.__wait_gazebo()
def __init__(self, config):
self._motors = Motors(config)
self._sensors = Sensors(config)
def left_speed():
return self._sensors.speed_left
def right_speed():
return self._sensors.speed_right
def left_ticks():
return self._sensors.enc_ticks_left
def right_ticks():
return self._sensors.enc_ticks_right
self._left = Helper(speed_sensor=left_speed, ticks_sensor=left_ticks)
self._right = Helper(speed_sensor=right_speed,
ticks_sensor=right_ticks)
def __init__(self, config):
self._motors = Motors(config)
self._sensors = Sensors(config)
def left_speed():
return self._sensors.speed_left
def right_speed():
return self._sensors.speed_right
def left_ticks():
return self._sensors.enc_ticks_left
def right_ticks():
return self._sensors.enc_ticks_left
self._left = Helper(speed_sensor=left_speed,
ticks_sensor=left_ticks)
self._right = Helper(speed_sensor=right_speed,
ticks_sensor=right_ticks)
class BotController(object):
"""
This controller uses PID in a closed-loop to stabilise bot speed regardless
of surface it is on (hard, carpet, asphalt).
It also presents signed ticks and signed speed readings to the user,
courtesy of Helper class (see below).
This class expects that its on_timer() method is called at 100Hz frequency.
"""
def __init__(self, config):
self._motors = Motors(config)
self._sensors = Sensors(config)
def left_speed():
return self._sensors.speed_left
def right_speed():
return self._sensors.speed_right
def left_ticks():
return self._sensors.enc_ticks_left
def right_ticks():
return self._sensors.enc_ticks_left
self._left = Helper(speed_sensor=left_speed,
ticks_sensor=left_ticks)
self._right = Helper(speed_sensor=right_speed,
ticks_sensor=right_ticks)
def start(self):
self._sensors.start()
def stop(self):
self._motors.run(0, 0)
self._motors.close()
self._sensors.stop()
def on_timer(self):
self._sensors.read()
self._left.on_timer()
self._right.on_timer()
self._motors.run(self._left.torque, self._right.torque)
def run(self, speed_left, speed_right):
self._left.run(speed_left)
self._right.run(speed_right)
@property
def ticks(self):
return self._left.ticks, self._right.ticks
@property
def actual_speed(self):
return self._left.speed, self._right.speed
@property
def timer(self):
return self._sensors.timer
@property
def values(self):
return self._sensors.values
class BotController(object):
"""
This controller uses PID in a closed-loop to stabilise bot speed regardless
of surface it is on (hard, carpet, asphalt).
It also presents signed ticks and signed speed readings to the user,
courtesy of Helper class (see below).
This class expects that its on_timer() method is called at 100Hz frequency.
"""
def __init__(self, config):
self._motors = Motors(config)
self._sensors = Sensors(config)
def left_speed():
return self._sensors.speed_left
def right_speed():
return self._sensors.speed_right
def left_ticks():
return self._sensors.enc_ticks_left
def right_ticks():
return self._sensors.enc_ticks_right
self._left = Helper(speed_sensor=left_speed, ticks_sensor=left_ticks)
self._right = Helper(speed_sensor=right_speed,
ticks_sensor=right_ticks)
def start(self):
self._sensors.start()
def stop(self):
self._motors.run(0, 0)
self._motors.close()
self._sensors.stop()
def on_timer(self):
self._sensors.read()
self._left.on_timer()
self._right.on_timer()
self._motors.run(self._left.torque, self._right.torque)
def run(self, speed_left, speed_right):
self._left.run(speed_left)
self._right.run(speed_right)
@property
def ticks(self):
return self._left.ticks, self._right.ticks
@property
def actual_speed(self):
return self._left.speed, self._right.speed
@property
def timer(self):
return self._sensors.timer
@property
def values(self):
return self._sensors.values
class Pilot(threading.Thread):
"""This class handles the robot and its brain.
This class called Pilot that handles the initialization of the robot sensors and actuators and the
brain that will control the robot. The main logic consists of an infinite loop called every 60 milliseconds that
invoke an action from the brain.
Attributes:
controller {utils.controller.Controller} -- Controller instance of the MVC of the application
configuration {utils.configuration.Config} -- Configuration instance of the application
sensors {robot.sensors.Sensors} -- Sensors instance of the robot
actuators {robot.actuators.Actuators} -- Actuators instance of the robot
brains {brains.brains_handler.Brains} -- Brains controller instance
"""
def __init__(self, configuration, controller):
"""Constructor of the pilot class
Arguments:
configuration {utils.configuration.Config} -- Configuration instance of the application
controller {utils.controller.Controller} -- Controller instance of the MVC of the application
"""
self.controller = controller
self.controller.set_pilot(self)
self.configuration = configuration
self.stop_event = threading.Event()
self.kill_event = threading.Event()
threading.Thread.__init__(self, args=self.stop_event)
self.sensors = None
self.actuators = None
self.brains = None
self.initialize_robot()
def __wait_gazebo(self):
"""Wait for gazebo to be initialized"""
gazebo_ready = False
self.stop_event.set()
while not gazebo_ready:
try:
self.controller.pause_gazebo_simulation()
gazebo_ready = True
self.stop_event.clear()
except Exception as ex:
print(ex)
def initialize_robot(self):
"""Initialize robot interfaces (sensors and actuators) and its brain from configuration"""
self.stop_interfaces()
self.actuators = Actuators(self.configuration.actuators)
self.sensors = Sensors(self.configuration.sensors)
self.brains = Brains(self.sensors, self.actuators,
self.configuration.brain_path, self.controller)
self.__wait_gazebo()
def stop_interfaces(self):
"""Function that kill the current interfaces of the robot. For reloading purposes."""
if self.sensors:
self.sensors.kill()
if self.actuators:
self.actuators.kill()
pass
def run(self):
"""Main loop of the class. Calls a brain action every TIME_CYCLE"""
"TODO: cleanup measure of ips"
it = 0
ss = time.time()
while (not self.kill_event.is_set()):
start_time = datetime.now()
if not self.stop_event.is_set():
try:
self.brains.active_brain.execute()
except AttributeError as e:
logger.warning('No Brain selected')
logger.error(e)
dt = datetime.now() - start_time
ms = (dt.days * 24 * 60 * 60 +
dt.seconds) * 1000 + dt.microseconds / 1000.0
elapsed = time.time() - ss
if elapsed < 1:
it += 1
else:
ss = time.time()
# print(it)
it = 0
if (ms < TIME_CYCLE):
time.sleep((TIME_CYCLE - ms) / 1000.0)
logger.debug('Pilot: pilot killed.')
def stop(self):
"""Pause the main loop"""
self.stop_event.set()
def play(self):
"""Resume the main loop."""
if self.is_alive():
self.stop_event.clear()
else:
self.start()
def kill(self):
"""Destroy the main loop. For exiting"""
self.actuators.kill()
self.kill_event.set()
def reload_brain(self, brain_path):
"""Reload a brain specified by brain_path
This function is useful if one wants to change the environment of the robot (simulated world).
Arguments:
brain_path {str} -- Path to the brain module to load.
"""
self.brains.load_brain(brain_path)
from robot.motor import Motors from robot.sensors import Sensors import config import time if __name__ == '__main__': SPEED = 40 motors = Motors(config) sensors = Sensors(config) sensors.start() torque = 0 data = [] for _ in range(300): time.sleep(0.01) sensors.read() data.append((sensors.timer, sensors.speed_left, sensors.speed_right, sensors.enc_ticks_left, sensors.enc_ticks_right, torque)) if _ == 50: torque = SPEED motors.run(SPEED, SPEED)
from robot.motor import Motors
from robot.sensors import Sensors
import config
import time
if __name__ == '__main__':
SPEED = 40
motors = Motors(config)
sensors = Sensors(config)
sensors.start()
torque = 0
data = []
for _ in range(300):
time.sleep(0.01)
sensors.read()
data.append((sensors.timer, sensors.speed_left, sensors.speed_right,
sensors.enc_ticks_left, sensors.enc_ticks_right, torque))
if _ == 50:
torque = SPEED
motors.run(SPEED, SPEED)
if _ == 150:
torque = -SPEED
motors.run(-SPEED, -SPEED)
