Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

U4 odrive update #156

Closed
wants to merge 3 commits into from
Closed

U4 odrive update #156

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
7e84d65
Make the tornado compatible with new odrive
Johannes-Thiel Aug 12, 2024
abaa265
change u4 config to use odrive version 0.5.6
Johannes-Thiel Aug 12, 2024
88fd97e
hotfix for lizard issue#59
Johannes-Thiel Aug 20, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions config/u4_config_rb28/hardware.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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',
Expand Down Expand Up @@ -53,6 +54,7 @@
'current_limit': 30,
'z_reference_speed': 0.0075,
'turn_reference_speed': 0.25,
'odrive_version': 6,
},
'flashlight': {
'version': 'flashlight_pwm',
Expand Down
4 changes: 3 additions & 1 deletion field_friend/hardware/field_friend_hardware.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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,
Expand Down
59 changes: 42 additions & 17 deletions field_friend/hardware/tornado.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -131,47 +132,51 @@ 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})
{name}_motor_turn.limits({self.turn_speed_limit}, {self.current_limit})
{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 = [
Expand All @@ -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,
Expand Down Expand Up @@ -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):
Expand Down
25 changes: 15 additions & 10 deletions field_friend/interface/components/status_dev.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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')
Expand Down Expand Up @@ -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'
Expand Down
Loading