From 58a7354a486c17551a7b17a97d0b46e6c4626970 Mon Sep 17 00:00:00 2001 From: Rodja Trappe Date: Tue, 30 Jul 2024 13:22:24 +0200 Subject: [PATCH] Punch demo (#143) * remove obsolete code * provide checkbox to run tornado in demo mode (eg. stop before the ground) --------- Co-authored-by: Pascal Schade --- .../automations/implements/tornado.py | 2 ++ field_friend/automations/puncher.py | 33 +++---------------- field_friend/hardware/tornado.py | 16 +++++---- 3 files changed, 15 insertions(+), 36 deletions(-) diff --git a/field_friend/automations/implements/tornado.py b/field_friend/automations/implements/tornado.py index 9b8d4757..8937e56b 100644 --- a/field_friend/automations/implements/tornado.py +++ b/field_friend/automations/implements/tornado.py @@ -101,6 +101,7 @@ def backup(self) -> dict: 'drill_with_open_tornado': self.drill_with_open_tornado, 'drill_between_crops': self.drill_between_crops, 'tornado_angle': self.tornado_angle, + 'is_demo': self.puncher.is_demo, } def restore(self, data: dict[str, Any]) -> None: @@ -108,6 +109,7 @@ def restore(self, data: dict[str, Any]) -> None: self.drill_with_open_tornado = data.get('drill_with_open_tornado', self.drill_with_open_tornado) self.drill_between_crops = data.get('drill_between_crops', self.drill_between_crops) self.tornado_angle = data.get('tornado_angle', self.tornado_angle) + self.puncher.is_demo = data.get('is_demo', self.puncher.is_demo) def settings_ui(self): super().settings_ui() diff --git a/field_friend/automations/puncher.py b/field_friend/automations/puncher.py index 32ff7765..7a1fdeb4 100644 --- a/field_friend/automations/puncher.py +++ b/field_friend/automations/puncher.py @@ -1,8 +1,6 @@ import logging import os -from typing import Optional -import numpy as np import rosys from rosys.driving import Driver from rosys.geometry import Point @@ -22,6 +20,7 @@ def __init__(self, field_friend: FieldFriend, driver: Driver, kpi_provider: KpiP self.driver = driver self.kpi_provider = kpi_provider self.log = logging.getLogger('field_friend.puncher') + self.is_demo = False async def try_home(self) -> bool: if self.field_friend.y_axis is None or self.field_friend.z_axis is None: @@ -94,6 +93,8 @@ async def punch(self, await self.tornado_drill(angle=angle, turns=turns, with_open_drill=with_open_tornado) elif isinstance(self.field_friend.z_axis, ZAxis): + 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) await self.field_friend.z_axis.move_to(-depth) if os.environ.get('Z_AXIS_REST_POSITION'): @@ -123,32 +124,6 @@ async def clear_view(self) -> None: await self.field_friend.y_axis.move_to(y, speed=self.field_friend.y_axis.max_speed) await self.field_friend.y_axis.stop() - async def drive_and_punch(self, - x: float, - y: float, - depth: float = 0.05, - angle: float = 180, - turns: float = 2.0, - backwards_allowed: bool = True, - plant_id: Optional[str] = None, - with_open_tornado: bool = False, - with_punch_check: bool = False, - ) -> None: - if self.field_friend.y_axis is None or self.field_friend.z_axis is None: - rosys.notify('no y or z axis', 'negative') - return - try: - work_x = self.field_friend.WORK_X - if x < work_x and not backwards_allowed: - self.log.warning(f'target x: {x} is behind') - return - await self.drive_to_punch(x) - await self.punch(y=y, depth=depth, angle=angle, turns=turns, - plant_id=plant_id, with_open_tornado=with_open_tornado, with_punch_check=with_punch_check) - # await self.clear_view() - except Exception as e: - raise PuncherException('drive and punch failed') from e - async def chop(self) -> None: if not isinstance(self.field_friend.y_axis, ChainAxis): raise PuncherException('chop is only available for chain axis') @@ -171,7 +146,7 @@ async def tornado_drill(self, angle: float = 180, turns: float = 2, with_open_dr rosys.notify('homing failed!', type='negative') raise PuncherException('homing failed') await rosys.sleep(0.5) - await self.field_friend.z_axis.move_down_until_reference() + await self.field_friend.z_axis.move_down_until_reference(min_position=-0.065 if self.is_demo else None) await self.field_friend.z_axis.turn_knifes_to(angle) await rosys.sleep(2) diff --git a/field_friend/hardware/tornado.py b/field_friend/hardware/tornado.py index 6dfeeeab..eb4959fa 100644 --- a/field_friend/hardware/tornado.py +++ b/field_friend/hardware/tornado.py @@ -50,7 +50,7 @@ async def move_to(self, position: float) -> None: raise RuntimeError(f'zaxis depth is out of range, min: {self.min_position}, given: {position}') @abc.abstractmethod - async def move_down_until_reference(self) -> None: + async def move_down_until_reference(self, *, min_position: Optional[float] = None) -> None: if not self.z_is_referenced: raise RuntimeError('zaxis is not referenced, reference first') @@ -211,28 +211,30 @@ async def move_to(self, position: float) -> None: await rosys.sleep(0.1) self.log.info(f'z axis moved to {position}') - async def move_down_until_reference(self) -> None: + async def move_down_until_reference(self, *, min_position: Optional[float] = None) -> None: try: await super().move_down_until_reference() except RuntimeError as e: raise Exception(e) from e try: - self.log.info('moving z axis down') + if min_position is None: + min_position = self.min_position + self.log.info(f'moving z axis down to {min_position}') await self.robot_brain.send( f'{self.name}_knife_stop_enabled = true;' f'{self.name}_knife_ground_enabled = true;' ) await rosys.sleep(0.5) await self.robot_brain.send( - f'{self.name}_z.position({self.min_position}, {self.speed_limit}, 0);' + f'{self.name}_z.position({min_position}, {self.speed_limit}, 0);' ) await rosys.sleep(0.5) while self.ref_knife_ground and not self.ref_knife_stop: - if self.min_position - 0.005 <= self.position_z <= self.min_position + 0.005: + if min_position - 0.005 <= self.position_z <= min_position + 0.005: self.log.info('minimum position reached') break await self.robot_brain.send( - f'{self.name}_z.position({self.min_position}, {self.speed_limit}, 0);' + f'{self.name}_z.position({min_position}, {self.speed_limit}, 0);' ) self.log.info('moving z axis down') await rosys.sleep(0.1) @@ -440,7 +442,7 @@ async def move_to(self, position: float) -> None: raise Exception(e) from e self.position_z = position - async def move_down_until_reference(self) -> None: + async def move_down_until_reference(self, *, min_position: Optional[float] = None) -> None: try: await super().move_down_until_reference() except RuntimeError as e: