def terminate_all(self) -> None:
self.run_event.clear()
for thread in self.all_threads:
thread.join()
log.info("Thread run status: {}: Status: {}".format(
thread.getName(), str(thread.is_alive())))
self.serial_manager.close_serial()
def get_version_info(self):
"""
Getting and displaying information about current system installed on mirto robot
:return:
"""
sys_info_service = self.robot.all_services.get("sys_info")
if sys_info_service is not None:
log.info("System version info: %s" % sys_info_service.system_version)
else:
log.warning("Service get_version_info is not enabled!")
def play_tone(self, frequency, duration):
"""
This function allows to play sound with specific frequency for give duration.
:return: None
"""
tone = self.robot.all_services.get("tone")
if tone is not None:
tone.play_tone(frequency, duration)
log.info("Finish playing tone")
else:
log.warning("Service tone is not enabled!")
def __init__(self, debug: bool = False) -> None:
self.isReady = False
self.conn = SerialConnection()
self.ports = self.conn.list_available_ports()
self.selected_port = self.ports[0]
self.debug = debug
if self.debug:
log.debug("Available ports: {}".format(self.ports))
if not self.isReady:
self.open_serial()
log.info("Serial port: %s opened successfully" % self.selected_port)
def stop_motors(self) -> None:
"""
Sending speed value 0 to both motor will cause robot to stop
:return:
"""
motor_1 = self.robot.all_services.get('motor_1')
motor_2 = self.robot.all_services.get('motor_2')
if motor_1 is not None or motor_1 is not None:
motor_1.stop_motor()
motor_2.stop_motor()
log.info("Motors stopped")
else:
log.warning("One of the motors is not enabled!")
def initialize_services(self, services_to_run: dict) -> None:
"""
Function is taking input argument (services_to_run: dict) and based on these arguments is starting services
:return:
"""
# Add version info service
sys_info = SystemInfo(name="System Info",
serial_manager=self.serial_manager)
sys_info.request_sys_info()
self.all_services.update({'sys_info': sys_info})
encoder_run_status = services_to_run.get("encoders")
if encoder_run_status[0]:
# Add encoders service
encoders = Encoders(name="Encoders",
serial_manager=self.serial_manager,
debug=encoder_run_status[1])
encoders.enable()
self.all_services.update({'encoders': encoders})
log.info("Service encoders enabled. Debug mode: {}".format(
encoder_run_status[1]))
motors_run_status = services_to_run.get("motors")
if motors_run_status[0]:
# Add motors service
motor_1 = Motor(name="Left Motor",
motor_id=0,
serial_manager=self.serial_manager,
debug=motors_run_status[1])
motor_2 = Motor(name="Right Motor",
motor_id=1,
serial_manager=self.serial_manager,
debug=motors_run_status[1])
self.all_services.update({'motor_1': motor_1})
self.all_services.update({'motor_2': motor_2})
log.info("Service motors enabled. Debug mode: {}".format(
motors_run_status[1]))
ir_sensors_run_status = services_to_run.get("ir_sensors")
if ir_sensors_run_status[0]:
# Add IR sensor service
ir_sensors = IRSensors(name="IR Sensors",
ir_id=1,
serial_manager=self.serial_manager,
debug=ir_sensors_run_status[1])
ir_sensors.set_reporting_interval(10)
self.all_services.update({'ir_sensors': ir_sensors})
log.info("IR service enabled. Debug mode: {}".format(
ir_sensors_run_status[1]))
tone_run_status = services_to_run.get("tone")
if tone_run_status[0]:
# Add tone service
tone = Tone(name="Tone",
serial_manager=self.serial_manager,
debug=tone_run_status[1])
self.all_services.update({"tone": tone})
log.info("Tone service enabled. Debug mode: {}".format(
tone_run_status[1]))
def set_motors(self, speed0: int, speed1: int) -> None:
"""
Input is a speed value which is send to robot. Range: -100 -> 100
:param speed0:
:param speed1:
:return:
"""
motor_1 = self.robot.all_services.get('motor_1')
motor_2 = self.robot.all_services.get('motor_2')
if motor_1 or motor_1 is not None:
motor_1.set_motor(speed0)
motor_2.set_motor(speed1)
log.info("Setting motor: '{}': {} motor:'{}': {}".format(motor_1.name, speed0, motor_2.name, speed1))
else:
log.warning("One of the motors is not enabled!")
def initialize_main(self, services_to_run: dict) -> None:
self.run_event = threading.Event()
self.run_event.set()
main_thread = threading.Thread(
name='Teensy msgs receiver',
target=self.serial_manager.conn.receive_data,
args=(self.run_event, self.run_services, self.terminate_all))
self.all_threads = [main_thread]
# Start all threads
for thread in self.all_threads:
try:
thread.start()
time.sleep(1)
log.info("Thread: %s set up successfully" % thread.getName())
except Exception as error:
log.error("Could not create a thread %s" % error)
# Init all services
self.initialize_services(services_to_run)
# This delay allows to make sure all background services will run, before any task
sleep(2)
def test_encoders(self, interval: float=0.1, time_to_finish: int=10) -> None:
end_time = time.time() + time_to_finish
while time.time() < end_time:
print(self.get_left_encoder_values(True), self.get_right_encoder_values(True))
sleep(interval)
log.info("Finish encoder test")
def test_play_tone(self):
self.play_tone(440, 2000)
sleep(1)
log.info("Finished test for tone service")
def close_serial(self) -> None:
self.conn.close()
self.isReady = False
log.info("Closing serial port %s" % self.selected_port)