From 60f32aecfb28eda9effd276c1b78597b2df73bdb Mon Sep 17 00:00:00 2001 From: "Johannes T." <119115663+Johannes-Thiel@users.noreply.github.com> Date: Wed, 4 Sep 2024 13:19:23 +0200 Subject: [PATCH] D1 axis (#159) * first test for a d1 axis module * first prototype of D1 module * Change to one axis base class * cleanup axis merge * change to one D1 axis module * fix imports for merged axis * make D1 available as a z axis * fix import * fix import * fix imports for simulation * fix imports * fix simulation * cleanup dev output * cleanup Ui * include the endstops * reactivate safety * fix homing function * rename D1Axis to AxisD1 * cast to int * adapt move_to * cleanup * rename actual_position to position * rework parameters * config for f15 * make position a property * fix namechange * rmenove camera * fix d1 entry * add setters to the d1 * add to testbrain hardware * fix typing and add min and max position * add defaults to z axis * update f15 params * fix d1 bugs * fix config * improve config * fix imports and fix pp mode * improve clear view * fix private functions improve reset fault * better default values, add comments, fix stop * fix merge * Make lizard methods consistent with merged lizrad version --------- Co-authored-by: Pascal Schade --- config/f15_config_f15/hardware.py | 46 +++--- config/testbrain_config_rb19/hardware.py | 43 ++--- config/testbrain_config_rb19/params.py | 1 + config/testbrain_config_rb19/robotbrain.py | 2 +- field_friend/automations/puncher.py | 11 +- field_friend/hardware/__init__.py | 18 ++- field_friend/hardware/{z_axis.py => axis.py} | 15 +- field_friend/hardware/axis_D1.py | 151 ++++++++++++++++++ field_friend/hardware/field_friend.py | 9 +- .../hardware/field_friend_hardware.py | 35 +++- .../hardware/field_friend_simulation.py | 15 +- field_friend/hardware/safety.py | 31 ++-- field_friend/hardware/y_axis.py | 130 --------------- .../hardware/y_axis_canopen_hardware.py | 4 +- .../hardware/y_axis_stepper_hardware.py | 4 +- .../hardware/z_axis_canopen_hardware.py | 4 +- .../hardware/z_axis_stepper_hardware.py | 4 +- .../interface/components/camera_card.py | 6 +- .../components/field_friend_object.py | 8 +- .../interface/components/hardware_control.py | 12 +- .../interface/components/status_dev.py | 12 +- .../interface/components/status_drawer.py | 17 +- 22 files changed, 321 insertions(+), 257 deletions(-) rename field_friend/hardware/{z_axis.py => axis.py} (90%) create mode 100644 field_friend/hardware/axis_D1.py delete mode 100644 field_friend/hardware/y_axis.py diff --git a/config/f15_config_f15/hardware.py b/config/f15_config_f15/hardware.py index b8c170fb..c8041ca1 100644 --- a/config/f15_config_f15/hardware.py +++ b/config/f15_config_f15/hardware.py @@ -17,32 +17,30 @@ # 'eyes_pin': 12, # }, 'y_axis': { - 'version': 'none', - # 'version': 'axis_d1', - # 'name': 'yaxis', - # 'can_address': 0x60, - # 'homing_acceleration': 2000, - # 'homing_velocity': 400, - # 'profile_acceleration': 400000, - # 'profile_velocity': 14000, - # 'profile_deceleration': 500000, - # 'min_position': -0.11, - # 'max_position': 0.11, - # 'axis_offset': 0.115, + 'version': 'axis_d1', + 'name': 'yaxis', + 'can_address': 0x60, + 'homing_acceleration': 2000, + 'homing_velocity': 400, + 'profile_acceleration': 500000, + 'profile_velocity': 40000, + 'profile_deceleration': 500000, + 'min_position': -0.11, + 'max_position': 0.11, + 'axis_offset': 0.115, }, 'z_axis': { - 'version': 'none', - # 'version': 'axis_d1', - # 'name': 'zaxis', - # 'can_address': 0x70, - # 'homing_acceleration': 1000, - # 'homing_velocity': 500, - # 'profile_acceleration': 500000, - # 'profile_velocity': 14000, - # 'profile_deceleration': 500000, - # 'min_position': 0.230, - # 'max_position': 0, - # 'axis_offset': 0.01, + 'version': 'axis_d1', + 'name': 'zaxis', + 'can_address': 0x70, + 'homing_acceleration': 1000, + 'homing_velocity': 500, + 'profile_acceleration': 500000, + 'profile_velocity': 40000, + 'profile_deceleration': 500000, + 'min_position': 0.230, + 'max_position': 0, + 'axis_offset': 0.01, }, 'estop': { 'name': 'estop', diff --git a/config/testbrain_config_rb19/hardware.py b/config/testbrain_config_rb19/hardware.py index 0a141e93..cb504935 100644 --- a/config/testbrain_config_rb19/hardware.py +++ b/config/testbrain_config_rb19/hardware.py @@ -10,20 +10,20 @@ 'is_right_reversed': False, }, 'y_axis': { - 'version': 'none', + 'version': 'axis_d1', 'name': 'yaxis', - 'can_address': 0x70, - 'max_speed': 2000, - 'reference_speed': 30, - 'min_position': -0.125, - 'max_position': 0.125, - 'axis_offset': 0.13, - 'steps_per_m': 1_666_666.667, # 4000steps/turn motor; 1/10 gear; 0.024m/u - 'end_r_pin': 12, - 'end_l_pin': 25, - 'motor_on_expander': False, - 'end_stops_on_expander': True, - 'reversed_direction': False, + 'can_address': 0x60, + 'homing_acceleration': 2000, + 'homing_velocity': 40, + 'profile_acceleration': 4000, + 'profile_velocity': 400, + 'profile_deceleration': 5000, + 'min_position': 0, + 'max_position': 23000, + 'axis_offset': 23000, + }, + 'z_axis': { + 'version': 'none', }, 'estop': { 'name': 'estop', @@ -40,22 +40,7 @@ 'flashlight': { 'version': 'none', }, - 'z_axis': { - 'version': 'none', - 'name': 'zaxis', - 'can_address': 0x60, - 'max_speed': 2000, - 'reference_speed': 30, - 'min_position': -0.197, - 'max_position': 0.0, - 'axis_offset': 0.0, - 'steps_per_m': 4_000_000, # 4000steps/turn motor; 1/20 gear; 0.02m/u - 'end_t_pin': 22, - 'end_b_pin': 23, - 'motor_on_expander': False, - 'end_stops_on_expander': True, - 'reversed_direction': False, - }, + 'bluetooth': { 'name': 'TestBrain', }, diff --git a/config/testbrain_config_rb19/params.py b/config/testbrain_config_rb19/params.py index a4cb1569..c81beaca 100644 --- a/config/testbrain_config_rb19/params.py +++ b/config/testbrain_config_rb19/params.py @@ -6,4 +6,5 @@ 'work_x': 0.0125, 'drill_radius': 0.025, 'tool': 'none', + 'antenna_offset': 0.205, } diff --git a/config/testbrain_config_rb19/robotbrain.py b/config/testbrain_config_rb19/robotbrain.py index 3dc97482..49c9ee58 100644 --- a/config/testbrain_config_rb19/robotbrain.py +++ b/config/testbrain_config_rb19/robotbrain.py @@ -1,4 +1,4 @@ configuration = {'robot_brain': { - 'flash_params': [''] + 'flash_params': ['nand'] }} # test diff --git a/field_friend/automations/puncher.py b/field_friend/automations/puncher.py index 5facf67d..0291f01d 100644 --- a/field_friend/automations/puncher.py +++ b/field_friend/automations/puncher.py @@ -5,7 +5,7 @@ from rosys.driving import Driver from rosys.geometry import Point -from ..hardware import ChainAxis, FieldFriend, Tornado, YAxis, ZAxis +from ..hardware import Axis, ChainAxis, FieldFriend, Tornado from .kpi_provider import KpiProvider @@ -83,7 +83,7 @@ async def punch(self, if not self.field_friend.y_axis.min_position <= y <= self.field_friend.y_axis.max_position: rosys.notify('y position out of range', type='negative') raise PuncherException('y position out of range') - elif isinstance(self.field_friend.y_axis, YAxis): + elif isinstance(self.field_friend.y_axis, Axis): if not self.field_friend.y_axis.min_position <= y <= self.field_friend.y_axis.max_position: rosys.notify('y position out of range', type='negative') raise PuncherException('y position out of range') @@ -92,7 +92,7 @@ async def punch(self, await self.field_friend.y_axis.move_to(y) await self.tornado_drill(angle=angle, turns=turns, with_open_drill=with_open_tornado) - elif isinstance(self.field_friend.z_axis, ZAxis): + elif isinstance(self.field_friend.z_axis, Axis): if self.is_demo: self.log.warning('punching with demo mode is not yet implemented for z axis') await self.field_friend.y_axis.move_to(y) @@ -119,7 +119,10 @@ async def clear_view(self) -> None: if isinstance(self.field_friend.y_axis, ChainAxis): await self.field_friend.y_axis.return_to_reference() return - elif isinstance(self.field_friend.y_axis, YAxis): + elif isinstance(self.field_friend.y_axis, Axis): + if isinstance(self.field_friend.z_axis,Axis): + if self.field_friend.z_axis.position != 0: + await self.field_friend.z_axis.return_to_reference() y = self.field_friend.y_axis.min_position if self.field_friend.y_axis.position <= 0 else self.field_friend.y_axis.max_position await self.field_friend.y_axis.move_to(y, speed=self.field_friend.y_axis.max_speed) await self.field_friend.y_axis.stop() diff --git a/field_friend/hardware/__init__.py b/field_friend/hardware/__init__.py index b6fda9c3..ee7322c1 100644 --- a/field_friend/hardware/__init__.py +++ b/field_friend/hardware/__init__.py @@ -1,3 +1,5 @@ +from .axis import Axis, AxisSimulation +from .axis_D1 import AxisD1 from .can_open_master import CanOpenMasterHardware from .chain_axis import ChainAxis, ChainAxisHardware, ChainAxisSimulation from .double_wheels import DoubleWheelsHardware @@ -6,19 +8,25 @@ from .field_friend_hardware import FieldFriendHardware from .field_friend_simulation import FieldFriendSimulation from .flashlight import Flashlight, FlashlightHardware, FlashlightSimulation -from .flashlight_pwm import FlashlightPWM, FlashlightPWMHardware, FlashlightPWMSimulation -from .flashlight_pwm_v2 import FlashlightPWMHardwareV2, FlashlightPWMSimulationV2, FlashlightPWMV2 +from .flashlight_pwm import ( + FlashlightPWM, + FlashlightPWMHardware, + FlashlightPWMSimulation, +) +from .flashlight_pwm_v2 import ( + FlashlightPWMHardwareV2, + FlashlightPWMSimulationV2, + FlashlightPWMV2, +) from .flashlight_v2 import FlashlightHardwareV2, FlashlightSimulationV2, FlashlightV2 from .imu import IMUHardware from .led_eyes import LedEyesHardware from .safety import Safety, SafetyHardware, SafetySimulation from .safety_small import SmallSafetyHardware from .status_control import StatusControlHardware +from .teltonika_router import TeltonikaRouter from .tornado import Tornado, TornadoHardware, TornadoSimulation -from .y_axis import YAxis, YAxisSimulation from .y_axis_canopen_hardware import YAxisCanOpenHardware from .y_axis_stepper_hardware import YAxisStepperHardware -from .z_axis import ZAxis, ZAxisSimulation from .z_axis_canopen_hardware import ZAxisCanOpenHardware from .z_axis_stepper_hardware import ZAxisStepperHardware -from .teltonika_router import TeltonikaRouter diff --git a/field_friend/hardware/z_axis.py b/field_friend/hardware/axis.py similarity index 90% rename from field_friend/hardware/z_axis.py rename to field_friend/hardware/axis.py index ca05464f..b0aedfdd 100644 --- a/field_friend/hardware/z_axis.py +++ b/field_friend/hardware/axis.py @@ -4,7 +4,7 @@ import rosys -class ZAxis(rosys.hardware.Module, abc.ABC): +class Axis(rosys.hardware.Module, abc.ABC): def __init__(self, max_speed, @@ -25,12 +25,14 @@ def __init__(self, self.steps_per_m: float = steps_per_m self.reversed_direction: bool = reversed_direction - self.is_referenced: bool = False self.steps: int = 0 + self.is_referenced: bool = False self.alarm: bool = False self.idle: bool = False self.end_t: bool = False self.end_b: bool = False + self.end_l: bool = False + self.end_r: bool = False rosys.on_shutdown(self.stop) @@ -45,9 +47,10 @@ async def move_to(self, position: float, speed: int | None = None) -> None: if not self.is_referenced: raise RuntimeError('zaxis is not referenced, reference first') if speed > self.max_speed: - raise RuntimeError('zaxis speed is too high') + raise RuntimeError(f'axis speed is too high, max speed is {self.max_speed}') if not self.min_position <= position <= self.max_position: - raise RuntimeError('zaxis depth is out of range') + raise RuntimeError( + f'target position {position} ist out of axis range {self.min_position} to {self.max_position}') @abc.abstractmethod async def try_reference(self) -> bool: @@ -74,7 +77,7 @@ async def return_to_reference(self) -> None: self.log.error(f'could not return zaxis to reference because of {e}') -class ZAxisSimulation(ZAxis, rosys.hardware.ModuleSimulation): +class AxisSimulation(Axis, rosys.hardware.ModuleSimulation): '''The z axis simulation module is a simple example for a representation of simulated robot hardware. ''' @@ -110,7 +113,7 @@ async def move_to(self, position: float, speed: int | None = None) -> None: try: await super().move_to(position, speed) except RuntimeError as e: - self.log.error(f'could not move zaxis to {position} because of {e}') + self.log.error(f'could not move axis to {position} because of {e}') return self.target_steps = self.compute_steps(position) self.speed = speed if self.target_steps > self.steps else speed * -1 diff --git a/field_friend/hardware/axis_D1.py b/field_friend/hardware/axis_D1.py new file mode 100644 index 00000000..9332e768 --- /dev/null +++ b/field_friend/hardware/axis_D1.py @@ -0,0 +1,151 @@ +import rosys +from rosys.helpers import remove_indentation + +from .axis import Axis + +D1_STEPS_P_M = 100000 + + +class AxisD1(Axis, rosys.hardware.ModuleHardware): + def __init__(self, robot_brain: rosys.hardware.RobotBrain, *, + name: str = 'axis_D1', + can: rosys.hardware.CanHardware, + max_position: int = -0.1, + min_position: int = 0.1, + axis_offset: int = 0, + can_address: int = 0x60, + homing_acceleration: int = 100, + homing_velocity: int = 20, + profile_velocity: int = 20, + profile_acceleration: int = 200, + profile_deceleration: int = 400, + + + ** kwargs + ) -> None: + """Rosys module to control the Igus D1 motor controller. + + :param: robot_brain: The RobotBrain object. + :param name: The name of the axis (default: 'axis_D1'). + :param can: The CAN hardware object. + :param can_address: The CAN address of the axis (default: 0x60). + """ + self.name = name + self.statusword: int = 0 + self._position: int = 0 + self.velocity: int = 0 + + # flags of the Statusword for more information refer to the CANopen standard and D1 manual + self.ready_to_switch_on: bool = False + self.switched_on: bool = False + self.operation_enabled: bool = False + self.fault: bool = False + self.voltage_enabled: bool = False + self.quick_stop: bool = False + self.switch_on_disabled: bool = False + self.warning: bool = False + self.manufacturer_specific: bool = False # this has no funktion in the D1 + self.remote_enable: bool = False + self.target_reached: bool = False + self.internal_limit_active: bool = False + self.operation_mode_specific: bool = False + self.manufacturer_specific2: bool = False + + lizard_code = remove_indentation(f''' + {self.name}_motor = D1Motor({can.name}, {can_address}) + {self.name}_motor.homing_acceleration = {homing_acceleration} + {self.name}_motor.switch_search_speed = {homing_velocity} + {self.name}_motor.zero_search_speed = {homing_velocity} + {self.name}_motor.profile_acceleration = {profile_acceleration} + {self.name}_motor.profile_deceleration = {profile_deceleration} + {self.name}_motor.profile_velocity = {profile_velocity} + ''') + core_message_fields = [ + f'{name}_motor.position', + f'{name}_motor.velocity', + f'{name}_motor.status_word', + f'{name}_motor.status_flags', + ] + super().__init__( + robot_brain=robot_brain, + lizard_code=lizard_code, + core_message_fields=core_message_fields, + max_speed=profile_velocity, + max_position=max_position, + min_position=min_position, + axis_offset=axis_offset, + reference_speed=homing_velocity, + steps_per_m=0, + reversed_direction=False, + **kwargs) + + @property + def position(self) -> float: + return (self._position / D1_STEPS_P_M) - self.axis_offset + + async def stop(self): + pass + + async def move_to(self, position: float, speed: int | None = None) -> None: + if self.is_referenced: + await self.robot_brain.send(f'{self.name}_motor.profile_position({self.compute_steps(position)});') + if not self.is_referenced: + self.log.error(f'AxisD1 {self.name} is not refernced') + return + while abs(self.position - position) > 0.02: + await rosys.sleep(0.1) + + async def enable_motor(self): + if self.fault: + await self.reset_error() + await rosys.sleep(0.5) + await self.robot_brain.send(f'{self.name}_motor.setup()') + + async def reset_error(self): + if self.fault: + await self.robot_brain.send(f'{self.name}_motor.reset()') + else: self.log.error(f'AxisD1 {self.name} is not in fault state') + + async def try_reference(self): + if not self._valid_status(): + await self.enable_motor() + if self.is_referenced: + self.log.error(f'AxisD1 {self.name} is already referenced') + else: + #due to some timing issues, the homing command is sent twice + await self.robot_brain.send(f'{self.name}_motor.home()') + await self.robot_brain.send(f'{self.name}_motor.home()') + + async def speed_Mode(self, speed: int): + #due to some timing issues, the speed command is sent twice + await self.robot_brain.send(f'{self.name}_motor.profile_velocity({speed});') + await self.robot_brain.send(f'{self.name}_motor.profile_velocity({speed});') + + def handle_core_output(self, time: float, words: list[str]) -> None: + self._position = int(words.pop(0)) + self.velocity = int(words.pop(0)) + self.statusword = int(words.pop(0)) + self.is_referenced = int(words.pop(0)) == 1 + self._split_statusword() + + def _valid_status(self) -> bool: + return self.ready_to_switch_on and self.switched_on and self.operation_enabled and self.quick_stop + + def _compute_steps(self, position: float) -> int: + return int(abs(position + self.axis_offset) * D1_STEPS_P_M) + + def _split_statusword(self) -> None: + self.ready_to_switch_on = ((self.statusword >> 0) & 1) == 1 + self.switched_on = ((self.statusword >> 1) & 1) == 1 + self.operation_enabled = ((self.statusword >> 2) & 1) == 1 + self.fault = ((self.statusword >> 3) & 1) == 1 + self.voltage_enabled = ((self.statusword >> 4) & 1) == 1 + self.quick_stop = ((self.statusword >> 5) & 1) == 1 + self.switch_on_disabled = ((self.statusword >> 6) & 1) == 1 + self.warning = ((self.statusword >> 7) & 1) == 1 + self.manufacturer_specific = ((self.statusword >> 8) & 1) == 1 # No function in D1 + self.remote_enable = ((self.statusword >> 9) & 1) == 1 + self.target_reached = ((self.statusword >> 10) & 1) == 1 + self.internal_limit_active = ((self.statusword >> 11) & 1) == 1 + self.operation_mode_specific = ((self.statusword >> 12) & 1) == 1 + self.manufacturer_specific2 = ((self.statusword >> 13) & 1) == 1 diff --git a/field_friend/hardware/field_friend.py b/field_friend/hardware/field_friend.py index a42a799b..2ec9f3b6 100644 --- a/field_friend/hardware/field_friend.py +++ b/field_friend/hardware/field_friend.py @@ -11,8 +11,7 @@ from .flashlight_v2 import FlashlightV2 from .safety import Safety from .tornado import Tornado -from .y_axis import YAxis -from .z_axis import ZAxis +from .axis import Axis class FieldFriend(rosys.hardware.Robot): @@ -33,8 +32,8 @@ def __init__( implement_name: str, wheels: rosys.hardware.Wheels, flashlight: Union[Flashlight, FlashlightV2, FlashlightPWM, None], - y_axis: Union[YAxis, ChainAxis, None], - z_axis: Union[ZAxis, Tornado, None], + y_axis: Union[Axis, ChainAxis, None], + z_axis: Union[Axis, Tornado, None], mower: Union[Mower, None], estop: rosys.hardware.EStop, bumper: Union[rosys.hardware.Bumper, None], @@ -69,7 +68,7 @@ def can_reach(self, local_point: rosys.geometry.Point, second_tool: bool = False The point is given in local coordinates, i.e. the origin is the center of the tool. """ - if self.implement_name in ['weed_screw', 'tornado'] and isinstance(self.y_axis, YAxis): + if self.implement_name in ['weed_screw', 'tornado'] and isinstance(self.y_axis, Axis): return self.y_axis.min_position <= local_point.y <= self.y_axis.max_position elif self.implement_name in ['dual_mechanism'] and isinstance(self.y_axis, ChainAxis): if second_tool: diff --git a/field_friend/hardware/field_friend_hardware.py b/field_friend/hardware/field_friend_hardware.py index ff906ab1..b4fca00b 100644 --- a/field_friend/hardware/field_friend_hardware.py +++ b/field_friend/hardware/field_friend_hardware.py @@ -5,6 +5,7 @@ import config.config_selection as config_selector +from .axis_D1 import AxisD1 from .can_open_master import CanOpenMasterHardware from .chain_axis import ChainAxisHardware from .double_wheels import DoubleWheelsHardware @@ -100,12 +101,25 @@ def __init__(self) -> None: else: raise NotImplementedError(f'Unknown wheels version: {config_hardware["wheels"]["version"]}') - if config_hardware['y_axis']['version'] == 'y_axis_canopen' or config_hardware['z_axis']['version'] == 'z_axis_canopen': + if config_hardware['y_axis']['version'] in ('y_axis_canopen', 'axis_d1') or config_hardware['z_axis']['version'] in ('y_axis_canopen', 'axis_d1'): can_open_master = CanOpenMasterHardware(robot_brain, can=can, name='master') else: can_open_master = None - y_axis: ChainAxisHardware | YAxisStepperHardware | YAxisCanOpenHardware | None - if config_hardware['y_axis']['version'] == 'chain_axis': + y_axis: ChainAxisHardware | YAxisStepperHardware | YAxisCanOpenHardware | AxisD1 | None + if config_hardware['y_axis']['version'] == 'axis_d1': + y_axis = AxisD1(robot_brain, + can=can, + can_address=config_hardware['y_axis']['can_address'], + name=config_hardware['y_axis']['name'], + homing_acceleration=config_hardware['y_axis']['homing_acceleration'], + homing_velocity=config_hardware['y_axis']['homing_velocity'], + profile_acceleration=config_hardware['y_axis']['profile_acceleration'], + profile_velocity=config_hardware['y_axis']['profile_velocity'], + profile_deceleration=config_hardware['y_axis']['profile_deceleration'], + max_position=config_hardware['y_axis']['max_position'], + min_position=config_hardware['y_axis']['min_position'], + axis_offset=config_hardware['y_axis']['axis_offset'],) + elif config_hardware['y_axis']['version'] == 'chain_axis': y_axis = ChainAxisHardware(robot_brain, expander=expander, name=config_hardware['y_axis']['name'], @@ -170,7 +184,7 @@ def __init__(self) -> None: else: raise NotImplementedError(f'Unknown y_axis version: {config_hardware["y_axis"]["version"]}') - z_axis: TornadoHardware | ZAxisCanOpenHardware | ZAxisStepperHardware | None + z_axis: TornadoHardware | ZAxisCanOpenHardware | ZAxisStepperHardware | AxisD1 | None if config_hardware['z_axis']['version'] == 'z_axis_stepper': z_axis = ZAxisStepperHardware(robot_brain, expander=expander, @@ -191,6 +205,19 @@ def __init__(self) -> None: reversed_direction=config_hardware['z_axis']['reversed_direction'], end_stops_inverted=config_hardware['z_axis']['end_stops_inverted'], ) + elif config_hardware['z_axis']['version'] == 'axis_d1': + z_axis = AxisD1(robot_brain, + can=can, + can_address=config_hardware['z_axis']['can_address'], + name=config_hardware['z_axis']['name'], + homing_acceleration=config_hardware['z_axis']['homing_acceleration'], + homing_velocity=config_hardware['z_axis']['homing_velocity'], + profile_acceleration=config_hardware['z_axis']['profile_acceleration'], + profile_velocity=config_hardware['z_axis']['profile_velocity'], + profile_deceleration=config_hardware['z_axis']['profile_deceleration'], + max_position=config_hardware['z_axis']['max_position'], + min_position=config_hardware['z_axis']['min_position'], + axis_offset=config_hardware['z_axis']['axis_offset'],) elif config_hardware['z_axis']['version'] == 'tornado': z_axis = TornadoHardware(robot_brain, expander=expander, diff --git a/field_friend/hardware/field_friend_simulation.py b/field_friend/hardware/field_friend_simulation.py index f4e637e2..f94860de 100644 --- a/field_friend/hardware/field_friend_simulation.py +++ b/field_friend/hardware/field_friend_simulation.py @@ -4,6 +4,7 @@ # change the config to the config of simulated Robot import config.config_selection as config_selector +from .axis import AxisSimulation from .chain_axis import ChainAxisSimulation from .external_mower import MowerSimulation from .field_friend import FieldFriend @@ -12,8 +13,6 @@ from .flashlight_v2 import FlashlightSimulationV2 from .safety import SafetySimulation from .tornado import TornadoSimulation -from .y_axis import YAxisSimulation -from .z_axis import ZAxisSimulation class FieldFriendSimulation(FieldFriend, rosys.hardware.RobotSimulation): @@ -43,11 +42,11 @@ def __init__(self, robot_id) -> None: raise NotImplementedError(f'Unknown FieldFriend tool: {tool}') wheels = rosys.hardware.WheelsSimulation(self.WHEEL_DISTANCE) - y_axis: YAxisSimulation | ChainAxisSimulation | None + y_axis: AxisSimulation | ChainAxisSimulation | None if config_hardware['y_axis']['version'] == 'chain_axis': y_axis = ChainAxisSimulation() - elif config_hardware['y_axis']['version'] in ['y_axis_stepper', 'y_axis_canopen']: - y_axis = YAxisSimulation( + elif config_hardware['y_axis']['version'] in ['y_axis_stepper', 'y_axis_canopen', 'axis_d1']: + y_axis = AxisSimulation( min_position=config_hardware['y_axis']['min_position'], max_position=config_hardware['y_axis']['max_position'], axis_offset=config_hardware['y_axis']['axis_offset'], @@ -57,9 +56,9 @@ def __init__(self, robot_id) -> None: else: raise NotImplementedError(f'Unknown Y-Axis version: {config_hardware["y_axis"]["version"]}') - z_axis: ZAxisSimulation | TornadoSimulation | None - if config_hardware['z_axis']['version'] in ['z_axis_stepper', 'z_axis_canopen']: - z_axis = ZAxisSimulation( + z_axis: AxisSimulation | TornadoSimulation | None + if config_hardware['z_axis']['version'] in ['z_axis_stepper', 'z_axis_canopen', 'axis_d1']: + z_axis = AxisSimulation( min_position=config_hardware['z_axis']['min_position'], max_position=config_hardware['z_axis']['max_position'], axis_offset=config_hardware['z_axis']['axis_offset'], diff --git a/field_friend/hardware/safety.py b/field_friend/hardware/safety.py index 2be8dab9..2ddf4e56 100644 --- a/field_friend/hardware/safety.py +++ b/field_friend/hardware/safety.py @@ -3,18 +3,26 @@ import rosys +from .axis import Axis, AxisSimulation +from .axis_D1 import AxisD1 from .chain_axis import ChainAxis, ChainAxisHardware, ChainAxisSimulation from .double_wheels import DoubleWheelsHardware from .external_mower import Mower, MowerHardware, MowerSimulation from .flashlight import Flashlight, FlashlightHardware, FlashlightSimulation -from .flashlight_pwm import FlashlightPWM, FlashlightPWMHardware, FlashlightPWMSimulation -from .flashlight_pwm_v2 import FlashlightPWMHardwareV2, FlashlightPWMSimulationV2, FlashlightPWMV2 +from .flashlight_pwm import ( + FlashlightPWM, + FlashlightPWMHardware, + FlashlightPWMSimulation, +) +from .flashlight_pwm_v2 import ( + FlashlightPWMHardwareV2, + FlashlightPWMSimulationV2, + FlashlightPWMV2, +) from .flashlight_v2 import FlashlightHardwareV2, FlashlightSimulationV2, FlashlightV2 from .tornado import Tornado, TornadoHardware, TornadoSimulation -from .y_axis import YAxis, YAxisSimulation from .y_axis_canopen_hardware import YAxisCanOpenHardware from .y_axis_stepper_hardware import YAxisStepperHardware -from .z_axis import ZAxis, ZAxisSimulation from .z_axis_canopen_hardware import ZAxisCanOpenHardware from .z_axis_stepper_hardware import ZAxisStepperHardware @@ -25,8 +33,8 @@ class Safety(rosys.hardware.Module, abc.ABC): def __init__(self, *, wheels: rosys.hardware.Wheels, estop: rosys.hardware.EStop, - y_axis: Union[YAxis, ChainAxis, None] = None, - z_axis: Union[ZAxis, Tornado, None] = None, + y_axis: Union[Axis, ChainAxis, None] = None, + z_axis: Union[Axis, Tornado, None] = None, flashlight: Union[Flashlight, FlashlightV2, FlashlightPWM, FlashlightPWMV2, None] = None, mower: Union[Mower, None] = None, **kwargs) -> None: @@ -57,11 +65,16 @@ def __init__(self, robot_brain: rosys.hardware.RobotBrain, *, # implement lizard stop method for available hardware lizard_code = f'let stop do {wheels.name}.speed(0, 0);' if y_axis is not None: - lizard_code += f' {y_axis.name}.stop();' + if isinstance(y_axis, AxisD1): + lizard_code += f'{y_axis.name}_motor.stop();' + else: + lizard_code += f' {y_axis.name}.stop();' if z_axis is not None: if isinstance(z_axis, TornadoHardware): lizard_code += f'{z_axis.name}_z.stop();' lizard_code += f'{z_axis.name}_motor_turn.speed(0);' + elif isinstance(z_axis, AxisD1): + lizard_code += f'{z_axis.name}_motor.stop();' else: lizard_code += f' {z_axis.name}.stop();' if isinstance(flashlight, FlashlightHardware): @@ -105,8 +118,8 @@ class SafetySimulation(Safety, rosys.hardware.ModuleSimulation): def __init__(self, *, wheels: rosys.hardware.Wheels, estop: rosys.hardware.EStop, - y_axis: Union[YAxisSimulation, ChainAxisSimulation, None] = None, - z_axis: Union[ZAxisSimulation, TornadoSimulation, None] = None, + y_axis: Union[AxisSimulation, ChainAxisSimulation, None] = None, + z_axis: Union[AxisSimulation, TornadoSimulation, None] = None, flashlight: Union[FlashlightSimulation, FlashlightSimulationV2, FlashlightPWMSimulation, FlashlightPWMSimulationV2, None], mower: Union[MowerSimulation, None] = None) -> None: super().__init__(wheels=wheels, estop=estop, y_axis=y_axis, z_axis=z_axis, flashlight=flashlight, mower=mower) diff --git a/field_friend/hardware/y_axis.py b/field_friend/hardware/y_axis.py deleted file mode 100644 index c0394a70..00000000 --- a/field_friend/hardware/y_axis.py +++ /dev/null @@ -1,130 +0,0 @@ -import abc -from typing import Optional - -import rosys - - -class YAxis(rosys.hardware.Module, abc.ABC): - """The y axis module is a simple example for a representation of real or simulated robot hardware.""" - - def __init__(self, max_speed, reference_speed, min_position, max_position, axis_offset, steps_per_m, reversed_direction, **kwargs) -> None: - super().__init__(**kwargs) - - self.max_speed: int = max_speed - self.reference_speed: int = reference_speed - self.min_position: float = min_position - self.max_position: float = max_position - self.axis_offset: float = axis_offset - self.steps_per_m: float = steps_per_m - self.reversed_direction: bool = reversed_direction - - self.steps: int = 0 - self.alarm: bool = False - self.idle: bool = False - - self.is_referenced: bool = False - - self.end_l: bool = False - self.end_r: bool = False - - rosys.on_shutdown(self.stop) - - @abc.abstractmethod - async def stop(self) -> None: - pass - - @abc.abstractmethod - async def move_to(self, position: float, speed: int | None = None) -> None: - if not speed: - speed = self.max_speed - if not self.is_referenced: - raise RuntimeError('yaxis is not referenced, reference first') - if speed > self.max_speed: - raise RuntimeError(f'yaxis speed is too high, max speed is {self.max_speed}') - if not self.min_position <= position <= self.max_position: - raise RuntimeError( - f'target position {position} ist out of yaxis range {self.min_position} to {self.max_position}') - self.log.info(f'moving yaxis to {position} with speed {speed}') - - @abc.abstractmethod - async def try_reference(self) -> bool: - return True - - def compute_steps(self, position: float) -> int: - """Compute the number of steps to move the y axis to the given position. - - The position is given in meters. - """ - return int((position + self.axis_offset) * self.steps_per_m) * (-1 if self.reversed_direction else 1) - - def compute_position(self, steps: int) -> float: - return steps / self.steps_per_m - self.axis_offset * (-1 if self.reversed_direction else 1) - - @property - def position(self) -> float: - return self.compute_position(self.steps) - - -class YAxisSimulation(YAxis, rosys.hardware.ModuleSimulation): - '''The y axis simulation module is a simple example for a representation of simulated robot hardware. - ''' - - def __init__(self, - max_speed: int = 80_000, - reference_speed: int = 40, - min_position: float = -0.12, - max_position: float = 0.12, - axis_offset: float = 0.123, - steps_per_m: float = 666.67 * 1000, - reversed_direction: bool = False, - ) -> None: - self.speed: int = 0 - self.target_steps: Optional[int] = None - super().__init__( - max_speed=max_speed, - reference_speed=reference_speed, - min_position=min_position, - max_position=max_position, - axis_offset=axis_offset, - steps_per_m=steps_per_m, - reversed_direction=reversed_direction, - ) - - async def stop(self) -> None: - await super().stop() - self.speed = 0 - self.target_steps = None - - async def move_to(self, position: float, speed: int | None = None) -> None: - if speed is None: - speed = self.max_speed - try: - await super().move_to(position, speed) - except RuntimeError as e: - rosys.notify(e, type='negative') - self.log.error(f'could not move yaxis to {position} because of {e}') - return - self.target_steps = self.compute_steps(position) - self.speed = speed if self.target_steps > self.steps else speed * -1 - while self.target_steps is not None: - await rosys.sleep(0.2) - - async def try_reference(self) -> bool: - if not await super().try_reference(): - return False - self.steps = 0 - self.is_referenced = True - return True - - async def reset_fault(self) -> None: - self.alarm = False - - async def step(self, dt: float) -> None: - await super().step(dt) - self.steps += int(dt * self.speed) - self.idle = self.speed == 0 - if self.target_steps is not None: - if (self.speed > 0) == (self.steps > self.target_steps): - self.steps = self.target_steps - self.target_steps = None - self.speed = 0 diff --git a/field_friend/hardware/y_axis_canopen_hardware.py b/field_friend/hardware/y_axis_canopen_hardware.py index 09d9ef45..4a835628 100644 --- a/field_friend/hardware/y_axis_canopen_hardware.py +++ b/field_friend/hardware/y_axis_canopen_hardware.py @@ -3,10 +3,10 @@ import rosys from rosys.helpers import remove_indentation -from .y_axis import YAxis +from .axis import Axis -class YAxisCanOpenHardware(YAxis, rosys.hardware.ModuleHardware): +class YAxisCanOpenHardware(Axis, rosys.hardware.ModuleHardware): """The y axis hardware module is a simple example for a representation of real robot hardware.""" def __init__(self, robot_brain: rosys.hardware.RobotBrain, *, diff --git a/field_friend/hardware/y_axis_stepper_hardware.py b/field_friend/hardware/y_axis_stepper_hardware.py index 33c212e8..788a75fb 100644 --- a/field_friend/hardware/y_axis_stepper_hardware.py +++ b/field_friend/hardware/y_axis_stepper_hardware.py @@ -3,10 +3,10 @@ import rosys from rosys.helpers import remove_indentation -from .y_axis import YAxis +from .axis import Axis -class YAxisStepperHardware(YAxis, rosys.hardware.ModuleHardware): +class YAxisStepperHardware(Axis, rosys.hardware.ModuleHardware): """The y axis hardware module is a simple example for a representation of real robot hardware.""" def __init__(self, robot_brain: rosys.hardware.RobotBrain, *, diff --git a/field_friend/hardware/z_axis_canopen_hardware.py b/field_friend/hardware/z_axis_canopen_hardware.py index 8e1870fa..c740579d 100644 --- a/field_friend/hardware/z_axis_canopen_hardware.py +++ b/field_friend/hardware/z_axis_canopen_hardware.py @@ -3,10 +3,10 @@ import rosys from rosys.helpers import remove_indentation -from .z_axis import ZAxis +from .axis import Axis -class ZAxisCanOpenHardware(ZAxis, rosys.hardware.ModuleHardware): +class ZAxisCanOpenHardware(Axis, rosys.hardware.ModuleHardware): """The z axis hardware module is a simple example for a representation of real robot hardware.""" def __init__(self, robot_brain: rosys.hardware.RobotBrain, *, diff --git a/field_friend/hardware/z_axis_stepper_hardware.py b/field_friend/hardware/z_axis_stepper_hardware.py index 492416a9..81c819ee 100644 --- a/field_friend/hardware/z_axis_stepper_hardware.py +++ b/field_friend/hardware/z_axis_stepper_hardware.py @@ -3,10 +3,10 @@ import rosys from rosys.helpers import remove_indentation -from .z_axis import ZAxis +from .axis import Axis -class ZAxisStepperHardware(ZAxis, rosys.hardware.ModuleHardware): +class ZAxisStepperHardware(Axis, rosys.hardware.ModuleHardware): """The z axis module is a simple example for a representation of real or simulated robot hardware.""" def __init__(self, robot_brain: rosys.hardware.RobotBrain, *, diff --git a/field_friend/interface/components/camera_card.py b/field_friend/interface/components/camera_card.py index 91f68d97..f77d5ad5 100644 --- a/field_friend/interface/components/camera_card.py +++ b/field_friend/interface/components/camera_card.py @@ -11,7 +11,7 @@ from field_friend.automations.implements.weeding_implement import WeedingImplement from ...automations import PlantLocator, Puncher -from ...hardware import FieldFriend, FlashlightPWM, FlashlightPWMV2, Tornado, ZAxis +from ...hardware import Axis, FieldFriend, FlashlightPWM, FlashlightPWMV2, Tornado from .calibration_dialog import calibration_dialog if TYPE_CHECKING: @@ -69,7 +69,7 @@ async def toggle_flashlight(): with ui.row(): ui.checkbox('Punching').bind_value(self, 'punching_enabled').tooltip( 'Enable punching mode').bind_enabled_from(self.automator, 'is_running', backward=lambda x: not x) - if isinstance(self.field_friend.z_axis, ZAxis): + if isinstance(self.field_friend.z_axis, Axis): self.depth = ui.number('depth', value=0.02, format='%.2f', step=0.01, min=self.field_friend.z_axis.max_position, max=-self.field_friend.z_axis.min_position).classes('w-16').bind_visibility_from(self, 'punching_enabled') elif isinstance(self.field_friend.z_axis, Tornado): @@ -150,7 +150,7 @@ def on_mouse_move(self, e: MouseEventArguments): self.debug_position.set_text(f'last punch: {point2d} -> {point3d}') if self.puncher is not None and self.punching_enabled: self.log.info(f'punching {point3d}') - if isinstance(self.field_friend.z_axis, ZAxis): + if isinstance(self.field_friend.z_axis, Axis): self.automator.start(self.puncher.drive_and_punch(point3d.x, point3d.y, self.depth.value)) elif isinstance(self.field_friend.z_axis, Tornado): self.automator.start(self.puncher.drive_and_punch( diff --git a/field_friend/interface/components/field_friend_object.py b/field_friend/interface/components/field_friend_object.py index 33bee3c9..340f90a0 100644 --- a/field_friend/interface/components/field_friend_object.py +++ b/field_friend/interface/components/field_friend_object.py @@ -4,7 +4,7 @@ from rosys.geometry import Prism from rosys.vision import CameraProjector, CameraProvider, camera_objects -from ...hardware import ChainAxis, FieldFriend, Tornado, YAxis, ZAxis +from ...hardware import ChainAxis, FieldFriend, Tornado, Axis class field_friend_object(robot_object): @@ -19,7 +19,7 @@ def __init__(self, odometer: Odometer, camera_provider: CameraProvider, self.with_stl('assets/field_friend.stl', x=-0.365, y=-0.3, z=0.06, scale=0.001, color='#6E93D6', opacity=0.7) camera_objects(camera_provider, CameraProjector(camera_provider, interval=0.1), interval=0.1) with self: - if isinstance(self.robot.y_axis, YAxis): + if isinstance(self.robot.y_axis, Axis): with Group() as self.tool: Box(0.015, 0.015, 0.35).material('#4488ff').move(z=0.4) Cylinder(0.03, 0, 0.05).material('#4488ff').move(z=0.2).rotate(1.571, 0, 0) @@ -34,13 +34,13 @@ def __init__(self, odometer: Odometer, camera_provider: CameraProvider, def update(self) -> None: super().update() - if isinstance(self.robot.y_axis, YAxis) and isinstance(self.robot.z_axis, ZAxis): + if isinstance(self.robot.y_axis, Axis) and isinstance(self.robot.z_axis, Axis): self.tool.move(x=self.robot.WORK_X, y=self.robot.y_axis.position, z=self.robot.z_axis.position) if self.robot.y_axis.position > self.robot.y_axis.max_position or self.robot.y_axis.position < self.robot.y_axis.min_position: self.tool.material('red') else: self.tool.material('#4488ff') - elif isinstance(self.robot.y_axis, YAxis) and isinstance(self.robot.z_axis, Tornado): + elif isinstance(self.robot.y_axis, Axis) and isinstance(self.robot.z_axis, Tornado): self.tool.move(x=self.robot.WORK_X, y=self.robot.y_axis.position, z=self.robot.z_axis.position_z) if self.robot.y_axis.position > self.robot.y_axis.max_position or self.robot.y_axis.position < self.robot.y_axis.min_position: self.tool.material('red') diff --git a/field_friend/interface/components/hardware_control.py b/field_friend/interface/components/hardware_control.py index 2a1e04b2..33f796bd 100644 --- a/field_friend/interface/components/hardware_control.py +++ b/field_friend/interface/components/hardware_control.py @@ -4,8 +4,8 @@ from ...automations import Puncher from ...hardware import (ChainAxis, FieldFriend, FieldFriendHardware, Flashlight, FlashlightPWM, FlashlightPWMV2, - FlashlightV2, Mower, MowerHardware, MowerSimulation, Tornado, YAxis, YAxisCanOpenHardware, - ZAxis, ZAxisCanOpenHardware) + FlashlightV2, Mower, MowerHardware, MowerSimulation, Tornado, Axis, YAxisCanOpenHardware, + ZAxisCanOpenHardware) from .confirm_dialog import ConfirmDialog as confirm_dialog from .status_bulb import StatusBulb as status_bulb @@ -64,7 +64,7 @@ async def toggle_flashlight(e: ValueChangeEventArguments) -> None: field_friend.flashlight, 'duty_cycle') if field_friend.y_axis is not None: - if isinstance(field_friend.y_axis, YAxis): + if isinstance(field_friend.y_axis, Axis): with ui.column(): ui.markdown('**Y-Axis**') ui.button('Reference', on_click=lambda: automator.start(field_friend.y_axis.try_reference())) @@ -89,7 +89,7 @@ async def toggle_flashlight(e: ValueChangeEventArguments) -> None: ui.button('chop to right', on_click=lambda: automator.start(field_friend.y_axis.move_dw_to_r_ref())) if field_friend.z_axis is not None: - if isinstance(field_friend.z_axis, ZAxis): + if isinstance(field_friend.z_axis, Axis): with ui.column(): ui.markdown('**Z-Axis**') ui.button('Reference', on_click=lambda: automator.start(field_friend.z_axis.try_reference())) @@ -131,13 +131,13 @@ async def toggle_flashlight(e: ValueChangeEventArguments) -> None: ui.button(on_click=lambda: automator.start(puncher.punch(0, depth=depth.value))) ui.button(on_click=lambda: automator.start( puncher.punch(field_friend.y_axis.max_position, depth=depth.value))) - elif isinstance(field_friend.y_axis, YAxis) and isinstance(field_friend.z_axis, Tornado): + elif isinstance(field_friend.y_axis, Axis) and isinstance(field_friend.z_axis, Tornado): ui.button(on_click=lambda: automator.start( puncher.punch(field_friend.y_axis.min_position, angle=angle.value))) ui.button(on_click=lambda: automator.start(puncher.punch(0, angle=angle.value))) ui.button(on_click=lambda: automator.start( puncher.punch(field_friend.y_axis.max_position, angle=angle.value))) - elif isinstance(field_friend.y_axis, YAxis) and isinstance(field_friend.z_axis, ZAxis): + elif isinstance(field_friend.y_axis, Axis) and isinstance(field_friend.z_axis, Axis): ui.button(on_click=lambda: automator.start( puncher.punch(field_friend.y_axis.max_position, depth=depth.value))) ui.button(on_click=lambda: automator.start(puncher.punch(0, depth=depth.value))) diff --git a/field_friend/interface/components/status_dev.py b/field_friend/interface/components/status_dev.py index 9551d577..b8c27b79 100644 --- a/field_friend/interface/components/status_dev.py +++ b/field_friend/interface/components/status_dev.py @@ -6,8 +6,8 @@ from nicegui import ui from ... import localization -from ...hardware import (ChainAxis, FieldFriend, FieldFriendHardware, FlashlightPWMHardware, FlashlightPWMHardwareV2, - Tornado, YAxis, ZAxis) +from ...hardware import (Axis, AxisD1, ChainAxis, FieldFriend, FieldFriendHardware, FlashlightPWMHardware, + FlashlightPWMHardwareV2, Tornado) if TYPE_CHECKING: from field_friend.system import System @@ -27,7 +27,7 @@ def status_dev_page(robot: FieldFriend, system: 'System'): ui.label('Software ESTOP is active!').classes('text-red mt-1') with ui.row().bind_visibility_from(robot.estop, 'active', value=False): - if isinstance(robot.z_axis, ZAxis): + if isinstance(robot.z_axis, Axis): with ui.row().bind_visibility_from(robot.z_axis, 'end_t'): ui.icon('report').props('size=md').classes('text-red') ui.label('Z-axis in end top position, error!').classes('text-red mt-1') @@ -39,7 +39,7 @@ def status_dev_page(robot: FieldFriend, system: 'System'): with ui.row().bind_visibility_from(robot.z_axis, 'alarm'): ui.icon('report').props('size=md').classes('text-yellow') ui.label('Z-axis in alarm, warning!').classes('text-orange mt-1') - if isinstance(robot.y_axis, ChainAxis) or isinstance(robot.y_axis, YAxis): + if isinstance(robot.y_axis, ChainAxis) or isinstance(robot.y_axis, Axis): with ui.row().bind_visibility_from(robot.y_axis, 'alarm'): ui.icon('report').props('size=md').classes('text-yellow') ui.label('Y-axis in alarm, warning!').classes('text-orange mt-1') @@ -213,7 +213,7 @@ def update_status() -> None: f'{robot.y_axis.steps:.0f}', f'{robot.y_axis.position:.2f}m' if robot.y_axis.is_referenced else '' ] - elif isinstance(robot.y_axis, YAxis): + elif isinstance(robot.y_axis, Axis): y_axis_flags = [ 'not referenced' if not robot.y_axis.is_referenced else '', 'alarm' if robot.y_axis.alarm else '', @@ -225,7 +225,7 @@ def update_status() -> None: ] else: y_axis_flags = ['no y-axis'] - if isinstance(robot.z_axis, ZAxis): + if isinstance(robot.z_axis, Axis): z_axis_flags = [ '' if robot.z_axis.is_referenced else 'not referenced', 'alarm' if robot.z_axis.alarm else '', diff --git a/field_friend/interface/components/status_drawer.py b/field_friend/interface/components/status_drawer.py index 7df0039d..b55d71cc 100644 --- a/field_friend/interface/components/status_drawer.py +++ b/field_friend/interface/components/status_drawer.py @@ -4,7 +4,14 @@ from nicegui import ui from ... import localization -from ...hardware import ChainAxis, FieldFriend, FlashlightPWMHardware, FlashlightPWMHardwareV2, Tornado, YAxis, ZAxis +from ...hardware import ( + Axis, + ChainAxis, + FieldFriend, + FlashlightPWMHardware, + FlashlightPWMHardwareV2, + Tornado, +) from ...localization import Gnss if TYPE_CHECKING: @@ -42,7 +49,7 @@ def status_drawer(system: 'System', robot: FieldFriend, gnss: Gnss, odometer: ro ui.label('Software ESTOP is active!').classes('text-red mt-1') with ui.row().bind_visibility_from(robot.estop, 'active', value=False): - if isinstance(robot.z_axis, ZAxis): + if isinstance(robot.z_axis, Axis): with ui.row().bind_visibility_from(robot.z_axis, 'end_t'): ui.icon('report').props('size=md').classes('text-red') ui.label('Z-axis in top end position!').classes('text-red mt-1') @@ -54,7 +61,7 @@ def status_drawer(system: 'System', robot: FieldFriend, gnss: Gnss, odometer: ro with ui.row().bind_visibility_from(robot.z_axis, 'alarm'): ui.icon('report').props('size=md').classes('text-yellow') ui.label('Z-axis in alarm, warning!').classes('text-orange mt-1') - if isinstance(robot.y_axis, ChainAxis) or isinstance(robot.y_axis, YAxis): + if isinstance(robot.y_axis, ChainAxis) or isinstance(robot.y_axis, Axis): with ui.row().bind_visibility_from(robot.y_axis, 'alarm'): ui.icon('report').props('size=md').classes('text-yellow') ui.label('Y-axis in alarm, warning!').classes('text-orange mt-1') @@ -151,7 +158,7 @@ def update_status() -> None: f'{robot.y_axis.steps:.0f}', f'{robot.y_axis.position:.2f}m' if robot.y_axis.is_referenced else '' ] - elif isinstance(robot.y_axis, YAxis): + elif isinstance(robot.y_axis, Axis): y_axis_flags = [ 'not referenced' if not robot.y_axis.is_referenced else '', 'alarm' if robot.y_axis.alarm else '', @@ -163,7 +170,7 @@ def update_status() -> None: ] else: y_axis_flags = ['no y-axis'] - if isinstance(robot.z_axis, ZAxis): + if isinstance(robot.z_axis, Axis): z_axis_flags = [ '' if robot.z_axis.is_referenced else 'not referenced', 'alarm' if robot.z_axis.alarm else '',