Punch demo (#143)

* remove obsolete code * provide checkbox to run tornado in demo mode (eg. stop before the ground) --------- Co-authored-by: Pascal Schade <[email protected]>
zauberzeug · Jul 30, 2024 · 58a7354 · 58a7354
1 parent b6a5fa8
commit 58a7354
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 36 deletions.
diff --git a/field_friend/automations/implements/tornado.py b/field_friend/automations/implements/tornado.py
@@ -101,13 +101,15 @@ 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:
         super().restore(data)
         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
@@ -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
@@ -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: