Speedup tornado workflow by moving axis during driving.

field_friend/automations/implements/tornado.py

@@ -1,4 +1,6 @@
 
+import asyncio
+
 from collections import deque
 from typing import TYPE_CHECKING, Any
 
@@ -21,6 +23,49 @@ def __init__(self, system: 'System') -> None:
         self.drill_between_crops: bool = False
         self.field_friend = system.field_friend
 
+        self.move_y_while_driving = False
+        self.move_knifes_while_driving = False
+
+        self.y_axis_move_task = None
+        self.knife_reset_task = None
+
+    async def _reset_knifes(self) -> None:
+        await self.field_friend.z_axis.turn_knifes_to(0)
+        await rosys.sleep(0.5)
+        await self.field_friend.z_axis.turn_knifes_to(self.tornado_angle)
+
+    async def finish(self) -> None:
+        await super().finish()
+        if self.y_axis_move_task:
+            try:
+                self.y_axis_move_task.cancel()
+                self.y_axis_move_task = None
+            except:
+                pass
+        if self.knife_reset_task:
+            try:
+                self.knife_reset_task.cancel()
+                self.knife_reset_task = None
+            except:
+                pass
+        await self.field_friend.z_axis.turn_knifes_to(0)
+
+    async def prepare(self) -> None:
+        res = await super().prepare()
+        if self.y_axis_move_task:
+            try:
+                self.y_axis_move_task.cancel()
+                self.y_axis_move_task = None
+            except:
+                res = False
+        if self.knife_reset_task:
+            try:
+                self.knife_reset_task.cancel()
+                self.knife_reset_task = None
+            except:
+                res = False
+        return res
+
     async def start_workflow(self) -> None:
         await super().start_workflow()
         self.log.info('Performing Tornado Workflow..')
@@ -33,7 +78,24 @@ async def start_workflow(self) -> None:
             open_drill = False
             if self.drill_with_open_tornado:
                 open_drill = True
+
+            if self.y_axis_move_task:
+                await self.y_axis_move_task
+                if self.y_axis_move_task.exception():
+                    raise self.y_axis_move_task.exception()
+                self.y_axis_move_task = None
+
+            if self.knife_reset_task:
+                await self.knife_reset_task
+                if self.knife_reset_task.exception():
+                    raise self.knife_reset_task.exception()
+                self.knife_reset_task = None
+
             await self.system.puncher.punch(y=self.next_punch_y_position, angle=self.tornado_angle, with_open_tornado=open_drill)
+            if self.move_knifes_while_driving:
+                self.knife_reset_task = asyncio.create_task(self._reset_knifes())
+            else:
+                await self._reset_knifes()
             # TODO remove weeds from plant_provider and increment kpis (like in Weeding Screw)
             if isinstance(self.system.detector, rosys.vision.DetectorSimulation):
                 # remove the simulated weeds
@@ -61,7 +123,7 @@ async def get_stretch(self, max_distance: float) -> float:
         # for p in self.last_punches:
         #     self.log.info(f'Last punch: {p} - {p.distance(closest_crop_world_position)} - {self.crop_safety_distance} - {closest_crop_world_position}')
         if any(p.distance(closest_crop_world_position) < self.field_friend.DRILL_RADIUS for p in self.last_punches):
-            self.log.info('Skipping weed because it was already punched')
+            self.log.info('Skipping crop because it was already punched')
             return self.WORKING_DISTANCE
         if not self.system.field_friend.can_reach(closest_crop_position):
             self.log.info('Target crop is not reachable')
@@ -81,6 +143,8 @@ async def get_stretch(self, max_distance: float) -> float:
             stretch = 0
         if stretch < max_distance:
             self.next_punch_y_position = closest_crop_position.y
+            if self.move_y_while_driving:
+                self.y_axis_move_task = asyncio.create_task(self.field_friend.y_axis.move_to(self.next_punch_y_position))
             return stretch
         return self.WORKING_DISTANCE
 
@@ -121,6 +185,9 @@ def settings_ui(self):
         ui.checkbox('Demo Mode') \
             .bind_value(self.puncher, 'is_demo') \
             .tooltip('If active, stop right before the ground')
+
+        ui.checkbox("Move y while driving").bind_value(self, 'move_y_while_driving').tooltip("Moves the y-axis into position whilst driving.")
+        ui.checkbox("Move knifes while driving").bind_value(self, 'move_knifes_while_driving').tooltip("Moves the knifes into the correct angle whilst driving.")
         # TODO test and reactivate these options
         # ui.checkbox('Drill 2x with open tornado') \
         #     .bind_value(self, 'drill_with_open_tornado') \

field_friend/automations/puncher.py

@@ -153,21 +153,15 @@ async def tornado_drill(self, angle: float = 180, turns: float = 2, with_open_dr
             await self.field_friend.z_axis.move_down_until_reference(min_position=-0.058 if self.is_demo else None)
 
             await self.field_friend.z_axis.turn_knifes_to(angle)
-            await rosys.sleep(2)
             await self.field_friend.z_axis.turn_by(turns)
-            await rosys.sleep(2)
 
             if with_open_drill:
                 self.log.info('Drilling again with open drill...')
                 await self.field_friend.z_axis.turn_knifes_to(0)
-                await rosys.sleep(2)
                 await self.field_friend.z_axis.turn_by(turns)
-                await rosys.sleep(2)
 
             await self.field_friend.z_axis.return_to_reference()
             await rosys.sleep(0.5)
-            await self.field_friend.z_axis.turn_knifes_to(0)
-            await rosys.sleep(0.5)
         except Exception as e:
             raise PuncherException(f'tornado drill failed because of: {e}') from e
         finally:

field_friend/hardware/tornado.py

@@ -234,11 +234,11 @@ async def move_down_until_reference(self, *, min_position: Optional[float] = Non
                 f'{self.name}_knife_stop_enabled = true;'
                 f'{self.name}_knife_ground_enabled = true;'
             )
-            await rosys.sleep(0.5)
+            await rosys.sleep(0.2)
             await self.robot_brain.send(
                 f'{self.name}_z.position({min_position}, {self.speed_limit}, 0);'
             )
-            await rosys.sleep(0.5)
+            await rosys.sleep(0.2)
             while self.ref_knife_ground and not self.ref_knife_stop:
                 if min_position - 0.005 <= self.position_z <= min_position + 0.005:
                     self.log.info('minimum position reached')

field_friend/hardware/y_axis_canopen_hardware.py

@@ -73,14 +73,16 @@ async def move_to(self, position: float, speed: int | None = None) -> None:
             self.log.error(f'could not move yaxis to {position} because of {error}')
             raise Exception(f'could not move yaxis to {position} because of {error}')
         steps = self.compute_steps(position)
+        if steps == self.steps:
+            return # Already at steps
         self.log.info(f'moving to steps: {steps}')
         await self.enable_motor()
-        await rosys.sleep(1)  # necessary ?!
+        await rosys.sleep(0.2)  # necessary ?!
         await self.robot_brain.send(
             f'{self.name}.position({steps},{speed}, 0);'
         )
         # Give flags time to turn false first
-        await rosys.sleep(0.5)
+        await rosys.sleep(0.2)
         while not self.idle and not self.alarm:
             await rosys.sleep(0.2)
         if self.alarm: