diff --git a/config/u4_config_rb28/hardware.py b/config/u4_config_rb28/hardware.py index 03d19a5d..b4c343ee 100644 --- a/config/u4_config_rb28/hardware.py +++ b/config/u4_config_rb28/hardware.py @@ -8,6 +8,7 @@ 'right_front_can_address': 0x300, 'is_left_reversed': False, 'is_right_reversed': True, + 'odrive_version': 6, }, 'y_axis': { 'version': 'y_axis_stepper', @@ -53,6 +54,7 @@ 'current_limit': 30, 'z_reference_speed': 0.0075, 'turn_reference_speed': 0.25, + 'odrive_version': 6, }, 'flashlight': { 'version': 'flashlight_pwm', diff --git a/field_friend/hardware/field_friend_hardware.py b/field_friend/hardware/field_friend_hardware.py index e661f3b9..d612b487 100644 --- a/field_friend/hardware/field_friend_hardware.py +++ b/field_friend/hardware/field_friend_hardware.py @@ -1,9 +1,9 @@ import logging import numpy as np +import rosys import config.config_selection as config_selector -import rosys from .can_open_master import CanOpenMasterHardware from .chain_axis import ChainAxisHardware @@ -211,6 +211,7 @@ def __init__(self) -> None: current_limit=config_hardware['z_axis']['current_limit'], z_reference_speed=config_hardware['z_axis']['z_reference_speed'], turn_reference_speed=config_hardware['z_axis']['turn_reference_speed'], + odrive_version=config_hardware['z_axis']['odrive_version']if 'odrive_version' in config_hardware['z_axis'] else 4, ) elif config_hardware['z_axis']['version'] == 'tornado v1.1': z_axis = TornadoHardware(robot_brain, @@ -241,6 +242,7 @@ def __init__(self) -> None: current_limit=config_hardware['z_axis']['current_limit'], z_reference_speed=config_hardware['z_axis']['z_reference_speed'], turn_reference_speed=config_hardware['z_axis']['turn_reference_speed'], + odrive_version=config_hardware['z_axis']['odrive_version']if 'odrive_version' in config_hardware['z_axis'] else 4, ) elif config_hardware['z_axis']['version'] == 'z_axis_canopen': z_axis = ZAxisCanOpenHardware(robot_brain, diff --git a/field_friend/hardware/tornado.py b/field_friend/hardware/tornado.py index d144388b..fef678b5 100644 --- a/field_friend/hardware/tornado.py +++ b/field_friend/hardware/tornado.py @@ -123,6 +123,7 @@ def __init__(self, robot_brain: rosys.hardware.RobotBrain, *, current_limit: int = 20, z_reference_speed: float = 0.0075, turn_reference_speed: float = 0.25, + odrive_version: int = 4, ) -> None: self.name = name self.expander = expander @@ -131,10 +132,14 @@ def __init__(self, robot_brain: rosys.hardware.RobotBrain, *, self.current_limit = current_limit self.z_reference_speed = z_reference_speed self.turn_reference_speed = turn_reference_speed + self.odrive_version = odrive_version + self.turn_error = 0 + self.z_error = 0 + self.motor_error = False lizard_code = remove_indentation(f''' - {name}_motor_z = {expander.name + "." if motors_on_expander and expander else ""}ODriveMotor({can.name}, {z_can_address}) - {name}_motor_turn = {expander.name + "." if motors_on_expander and expander else ""}ODriveMotor({can.name}, {turn_can_address}) + {name}_motor_z = {expander.name + "." if motors_on_expander and expander else ""}ODriveMotor({can.name}, {z_can_address}{', 6'if self.odrive_version == 6 else ''}) + {name}_motor_turn = {expander.name + "." if motors_on_expander and expander else ""}ODriveMotor({can.name}, {turn_can_address}{', 6'if self.odrive_version == 6 else ''}) {name}_motor_z.m_per_tick = {m_per_tick} {name}_motor_turn.m_per_tick = {1/12.52} {name}_motor_z.limits({self.speed_limit}, {self.current_limit}) @@ -142,36 +147,36 @@ def __init__(self, robot_brain: rosys.hardware.RobotBrain, *, {name}_motor_z.reversed = {'true' if is_z_reversed else 'false'} {name}_motor_turn.reversed = {'true' if is_turn_reversed else 'false'} {name}_end_top = {expander.name + "." if end_stops_on_expander or end_top_pin_expander and expander else ""}Input({end_top_pin}) - {name}_end_top.inverted = true; + {name}_end_top.inverted = true {name}_end_bottom = {expander.name + "." if end_stops_on_expander or end_bottom_pin_expander and expander else ""}Input({end_bottom_pin}) - {name}_end_bottom.inverted = true; + {name}_end_bottom.inverted = true {name}_ref_motor = {expander.name + "." if end_stops_on_expander or ref_motor_pin_expander and expander else ""}Input({ref_motor_pin}) - {name}_ref_motor.inverted = true; + {name}_ref_motor.inverted = true {name}_ref_gear = {expander.name + "." if end_stops_on_expander or ref_gear_pin_expander and expander else ""}Input({ref_gear_pin}) - {name}_ref_gear.inverted = false; + {name}_ref_gear.inverted = false {name}_ref_knife_stop = {expander.name + "." if end_stops_on_expander or ref_knife_stop_pin_expander and expander else ""}Input({ref_knife_stop_pin}) - {name}_ref_knife_stop.inverted = false; + {name}_ref_knife_stop.inverted = false {name}_ref_knife_ground = {expander.name + "." if end_stops_on_expander or ref_knife_ground_pin_expander and expander else ""}Input({ref_knife_ground_pin}) - {name}_ref_knife_ground.inverted = true; + {name}_ref_knife_ground.inverted = true {name}_z = {expander.name + "." if motors_on_expander and expander else ""}MotorAxis({name}_motor_z, {name + "_end_bottom" if is_z_reversed else name + "_end_top"}, {name + "_end_top" if is_z_reversed else name + "_end_bottom"}) - bool {name}_is_referencing = false; - bool {name}_ref_motor_enabled = false; - bool {name}_ref_gear_enabled = false; + bool {name}_is_referencing = false + bool {name}_ref_motor_enabled = false + bool {name}_ref_gear_enabled = false when {name}_ref_motor_enabled and {name}_is_referencing and {name}_ref_motor.level == 0 then - {name}_motor_turn.speed(0); + {name}_motor_turn.speed(0) end when {name}_ref_gear_enabled and {name}_is_referencing and {name}_ref_gear.level == 1 then {name}_motor_turn.speed(0); end - bool {name}_knife_ground_enabled = false; - bool {name}_knife_stop_enabled = false; + bool {name}_knife_ground_enabled = false + bool {name}_knife_stop_enabled = false when {name}_knife_ground_enabled and {name}_ref_knife_ground.level == 1 then - {name}_motor_z.off(); + {name}_motor_z.off() end when {name}_knife_stop_enabled and {name}_ref_knife_stop.level == 1 then - en3.off(); - {name}_knife_stop_enabled = false; + en3.off() + {name}_knife_stop_enabled = false end ''') # tornado axis references in positive direction, in contrast to all other axis core_message_fields = [ @@ -184,6 +189,11 @@ def __init__(self, robot_brain: rosys.hardware.RobotBrain, *, f'{name}_motor_z.position:3', f'{name}_motor_turn.position:3', ] + if self.odrive_version == 6: + core_message_fields.extend([ + f'{name}_motor_z.motor_error_flag', + f'{name}_motor_turn.motor_error_flag', + ]) super().__init__( min_position=min_position, robot_brain=robot_brain, @@ -411,6 +421,21 @@ def handle_core_output(self, time: float, words: list[str]) -> None: self.ref_knife_ground = words.pop(0) == 'true' self.position_z = float(words.pop(0)) self.position_turn = float(words.pop(0)) * 360 + if self.odrive_version == 6: + self.turn_error = int(words.pop(0)) + self.z_error = int(words.pop(0)) + if self.turn_error == 1 or self.z_error == 1: + self.motor_error = True + rosys.notify('warning', 'Tornado Motor Error') + + def reset_motors(self) -> None: + if not self.motor_error: + return + if self.z_error == 1: + self.robot_brain.send(f'{self.name}_motor_z.reset_motor()') + if self.turn_error == 1: + self.robot_brain.send(f'{self.name}_motor_turn.reset_motor()') + self.motor_error = False class TornadoSimulation(Tornado, rosys.hardware.ModuleSimulation): diff --git a/field_friend/interface/components/status_dev.py b/field_friend/interface/components/status_dev.py index ce5e47f5..84c65075 100644 --- a/field_friend/interface/components/status_dev.py +++ b/field_friend/interface/components/status_dev.py @@ -6,16 +6,8 @@ from nicegui import ui from ... import localization -from ...hardware import ( - ChainAxis, - FieldFriend, - FieldFriendHardware, - FlashlightPWMHardware, - FlashlightPWMHardwareV2, - Tornado, - YAxis, - ZAxis, -) +from ...hardware import (ChainAxis, FieldFriend, FieldFriendHardware, FlashlightPWMHardware, FlashlightPWMHardwareV2, + Tornado, YAxis, ZAxis) if TYPE_CHECKING: from field_friend.system import System @@ -105,6 +97,13 @@ def status_dev_page(robot: FieldFriend, system: 'System'): r1_status = ui.label() reset_motor_button = ui.button( 'Reset motor errors', on_click=robot.wheels.reset_motors).bind_visibility_from(robot.wheels, 'motor_error') + if system.is_real and isinstance(robot.z_axis, Tornado): + with ui.row().classes('place-items-center'): + ui.markdown('**Tornado motor status:**').style('color: #EDF4FB') + tornado_motor_turn_status = ui.label() + tornado_motor_z_status = ui.label() + reset_tornado_motor_button = ui.button( + 'Reset tornado motor errors', on_click=robot.z_axis.reset_motors).bind_visibility_from(robot.z_axis, 'motor_error') with ui.card().style('background-color: #3E63A6; color: white;'): ui.markdown('**Robot Brain**').style('color: #6E93D6;').classes('w-full text-center') @@ -306,6 +305,12 @@ def update_status() -> None: r1_status.text = 'Error in r1' if robot.wheels.r1_error else 'No error' if robot.wheels.odrive_version == 4: l0_status.text = 'cant read status update odrive to version 0.5.6' + if system.is_real and isinstance(robot.z_axis, Tornado): + if robot.z_axis.odrive_version == 6: + tornado_motor_turn_status.text = 'Error in turn motor' if robot.z_axis.turn_error else 'No error' + tornado_motor_z_status.text = 'Error in z motor' if robot.z_axis.z_error else 'No error' + else: + tornado_motor_turn_status.text = 'cant read status update odrive to version 0.5.6' gnss_device_label.text = 'No connection' if system.gnss.device is None else 'Connected' reference_position_label.text = 'No reference' if localization.reference is None else 'Set' gnss_label.text = str(system.gnss.current.location) if system.gnss.current is not None else 'No position'