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)