def __init__(self, interface: RevvyControl, sound_paths, sw_version):
self._interface = interface
self._start_time = time.time()
# read versions
hw = interface.get_hardware_version()
fw = interface.get_firmware_version()
sw = sw_version
print('Hardware: {}\nFirmware: {}\nFramework: {}'.format(hw, fw, sw))
self._version = RobotVersion(hw, fw, sw)
setup = {
Version('1.0'): setup_sound_v1,
Version('1.1'): setup_sound_v1,
Version('2.0'): setup_sound_v2,
}
play = {
Version('1.0'): play_sound_v1,
Version('1.1'): play_sound_v1,
Version('2.0'): play_sound_v2,
}
self._ring_led = RingLed(interface)
self._sound = Sound(setup[hw], play[hw], sound_paths or {})
self._status = RobotStatusIndicator(interface)
self._status_updater = McuStatusUpdater(interface)
self._battery = BatteryStatus(0, 0, 0)
self._imu = IMU()
def _motor_config_changed(motor: PortInstance, config_name):
callback = None if config_name == 'NotConfigured' else motor.update_status
self._status_updater.set_slot(mcu_updater_slots["motors"][motor.id], callback)
def _sensor_config_changed(sensor: PortInstance, config_name):
callback = None if config_name == 'NotConfigured' else sensor.update_status
self._status_updater.set_slot(mcu_updater_slots["sensors"][sensor.id], callback)
self._motor_ports = create_motor_port_handler(interface, Motors)
for port in self._motor_ports:
port.on_config_changed(_motor_config_changed)
self._sensor_ports = create_sensor_port_handler(interface, Sensors)
for port in self._sensor_ports:
port.on_config_changed(_sensor_config_changed)
self._drivetrain = DifferentialDrivetrain(interface, self._motor_ports.port_count)
def __enter__(self):
self._comm_interface = self._bus_factory()
self._robot_control = self._comm_interface.create_application_control()
self._bootloader_control = self._comm_interface.create_bootloader_control(
)
self._stopwatch = Stopwatch()
# read versions
while True:
try:
self._hw_version = self._robot_control.get_hardware_version()
self._fw_version = self._robot_control.get_firmware_version()
break
except OSError:
try:
self._hw_version = self._bootloader_control.get_hardware_version(
)
self._fw_version = Version('0.0.0')
break
except OSError:
self._log('Failed to read robot version')
from revvy.firmware_updater import update_firmware
update_firmware(os.path.join('data', 'firmware'), self)
# read version again in case it was updated
self._fw_version = self._robot_control.get_firmware_version()
self._log(f'Hardware: {self._hw_version}')
self._log(f'Firmware: {self._fw_version}')
setup = {
Version('1.0'): SoundControlV1,
Version('1.1'): SoundControlV1,
Version('2.0'): SoundControlV2,
}
self._ring_led = RingLed(self._robot_control)
self._sound = Sound(setup[self._hw_version](),
self._assets.category_loader('sounds'))
self._status = RobotStatusIndicator(self._robot_control)
self._status_updater = McuStatusUpdater(self._robot_control)
self._battery = BatteryStatus(0, 0, 0)
self._imu = IMU()
def _set_updater(slot_name, port, config_name):
if config_name is None:
self._status_updater.disable_slot(slot_name)
else:
self._status_updater.enable_slot(slot_name, port.update_status)
self._motor_ports = create_motor_port_handler(self._robot_control)
for port in self._motor_ports:
port.on_config_changed.add(
partial(_set_updater, f'motor_{port.id}'))
self._sensor_ports = create_sensor_port_handler(self._robot_control)
for port in self._sensor_ports:
port.on_config_changed.add(
partial(_set_updater, f'sensor_{port.id}'))
self._drivetrain = DifferentialDrivetrain(self._robot_control,
self._imu)
self.update_status = self._status_updater.read
self.ping = self._robot_control.ping
return self
class Robot(RobotInterface):
@staticmethod
def _default_bus_factory() -> RevvyTransportBase:
from revvy.hardware_dependent.rrrc_transport_i2c import RevvyTransportI2C
return RevvyTransportI2C(1)
def __init__(self, bus_factory=None):
if bus_factory is None:
bus_factory = self._default_bus_factory
self._bus_factory = bus_factory
self._assets = Assets()
self._assets.add_source(os.path.join('data', 'assets'))
self._log = get_logger('Robot')
def __enter__(self):
self._comm_interface = self._bus_factory()
self._robot_control = self._comm_interface.create_application_control()
self._bootloader_control = self._comm_interface.create_bootloader_control(
)
self._stopwatch = Stopwatch()
# read versions
while True:
try:
self._hw_version = self._robot_control.get_hardware_version()
self._fw_version = self._robot_control.get_firmware_version()
break
except OSError:
try:
self._hw_version = self._bootloader_control.get_hardware_version(
)
self._fw_version = Version('0.0.0')
break
except OSError:
self._log('Failed to read robot version')
from revvy.firmware_updater import update_firmware
update_firmware(os.path.join('data', 'firmware'), self)
# read version again in case it was updated
self._fw_version = self._robot_control.get_firmware_version()
self._log(f'Hardware: {self._hw_version}')
self._log(f'Firmware: {self._fw_version}')
setup = {
Version('1.0'): SoundControlV1,
Version('1.1'): SoundControlV1,
Version('2.0'): SoundControlV2,
}
self._ring_led = RingLed(self._robot_control)
self._sound = Sound(setup[self._hw_version](),
self._assets.category_loader('sounds'))
self._status = RobotStatusIndicator(self._robot_control)
self._status_updater = McuStatusUpdater(self._robot_control)
self._battery = BatteryStatus(0, 0, 0)
self._imu = IMU()
def _set_updater(slot_name, port, config_name):
if config_name is None:
self._status_updater.disable_slot(slot_name)
else:
self._status_updater.enable_slot(slot_name, port.update_status)
self._motor_ports = create_motor_port_handler(self._robot_control)
for port in self._motor_ports:
port.on_config_changed.add(
partial(_set_updater, f'motor_{port.id}'))
self._sensor_ports = create_sensor_port_handler(self._robot_control)
for port in self._sensor_ports:
port.on_config_changed.add(
partial(_set_updater, f'sensor_{port.id}'))
self._drivetrain = DifferentialDrivetrain(self._robot_control,
self._imu)
self.update_status = self._status_updater.read
self.ping = self._robot_control.ping
return self
def __exit__(self, exc_type, exc_val, exc_tb):
self._comm_interface.close()
@property
def assets(self):
return self._assets
@property
def robot_control(self):
return self._robot_control
@property
def bootloader_control(self):
return self._bootloader_control
@property
def hw_version(self) -> Version:
return self._hw_version
@property
def fw_version(self) -> Version:
return self._fw_version
@property
def battery(self):
return self._battery
@property
def imu(self):
return self._imu
@property
def status(self):
return self._status
@property
def motors(self):
return self._motor_ports
@property
def sensors(self):
return self._sensor_ports
@property
def drivetrain(self):
return self._drivetrain
@property
def led(self):
return self._ring_led
@property
def sound(self):
return self._sound
def play_tune(self, name):
self._sound.play_tune(name)
def time(self):
return self._stopwatch.elapsed
def reset(self):
self._log('reset()')
self._ring_led.start_animation(RingLed.BreathingGreen)
self._status_updater.reset()
def _process_battery_slot(data):
assert len(data) == 4
main_status, main_percentage, _, motor_percentage = data
self._battery = BatteryStatus(chargerStatus=main_status,
main=main_percentage,
motor=motor_percentage)
self._status_updater.enable_slot("battery", _process_battery_slot)
self._status_updater.enable_slot("axl", self._imu.update_axl_data)
self._status_updater.enable_slot("gyro", self._imu.update_gyro_data)
self._status_updater.enable_slot("yaw", self._imu.update_yaw_angles)
# TODO: do something useful with the reset signal
self._status_updater.enable_slot(
"reset", lambda _: self._log('MCU reset detected'))
self._drivetrain.reset()
self._motor_ports.reset()
self._sensor_ports.reset()
self._sound.reset_volume()
self._status.robot_status = RobotStatus.NotConfigured
self._status.update()
def stop(self):
self._sound.wait()
class Robot:
def __init__(self, interface: RevvyControl, sound_paths, sw_version):
self._interface = interface
self._start_time = time.time()
# read versions
hw = interface.get_hardware_version()
fw = interface.get_firmware_version()
sw = sw_version
print('Hardware: {}\nFirmware: {}\nFramework: {}'.format(hw, fw, sw))
self._version = RobotVersion(hw, fw, sw)
setup = {
Version('1.0'): setup_sound_v1,
Version('1.1'): setup_sound_v1,
Version('2.0'): setup_sound_v2,
}
play = {
Version('1.0'): play_sound_v1,
Version('1.1'): play_sound_v1,
Version('2.0'): play_sound_v2,
}
self._ring_led = RingLed(interface)
self._sound = Sound(setup[hw], play[hw], sound_paths or {})
self._status = RobotStatusIndicator(interface)
self._status_updater = McuStatusUpdater(interface)
self._battery = BatteryStatus(0, 0, 0)
self._imu = IMU()
def _motor_config_changed(motor: PortInstance, config_name):
callback = None if config_name == 'NotConfigured' else motor.update_status
self._status_updater.set_slot(mcu_updater_slots["motors"][motor.id], callback)
def _sensor_config_changed(sensor: PortInstance, config_name):
callback = None if config_name == 'NotConfigured' else sensor.update_status
self._status_updater.set_slot(mcu_updater_slots["sensors"][sensor.id], callback)
self._motor_ports = create_motor_port_handler(interface, Motors)
for port in self._motor_ports:
port.on_config_changed(_motor_config_changed)
self._sensor_ports = create_sensor_port_handler(interface, Sensors)
for port in self._sensor_ports:
port.on_config_changed(_sensor_config_changed)
self._drivetrain = DifferentialDrivetrain(interface, self._motor_ports.port_count)
@property
def start_time(self):
return self._start_time
@property
def version(self):
return self._version
@property
def battery(self):
return self._battery
@property
def imu(self):
return self._imu
@property
def status(self):
return self._status
@property
def motors(self):
return self._motor_ports
@property
def sensors(self):
return self._sensor_ports
@property
def drivetrain(self):
return self._drivetrain
@property
def led_ring(self):
return self._ring_led
@property
def sound(self):
return self._sound
def update_status(self):
self._status_updater.read()
def reset(self):
self._ring_led.set_scenario(RingLed.BreathingGreen)
self._status_updater.reset()
def _process_battery_slot(data):
assert len(data) == 4
main_status = data[0]
main_percentage = data[1]
# motor_status = data[2]
motor_percentage = data[3]
self._battery = BatteryStatus(chargerStatus=main_status, main=main_percentage, motor=motor_percentage)
self._status_updater.set_slot(mcu_updater_slots["battery"], _process_battery_slot)
self._status_updater.set_slot(mcu_updater_slots["axl"], self._imu.update_axl_data)
self._status_updater.set_slot(mcu_updater_slots["gyro"], self._imu.update_gyro_data)
self._status_updater.set_slot(mcu_updater_slots["yaw"], self._imu.update_yaw_angles)
self._drivetrain.reset()
self._motor_ports.reset()
self._sensor_ports.reset()
self._status.robot_status = RobotStatus.NotConfigured
self._status.update()