D1 axis (#159)

* first test for a d1 axis module * first prototype of D1 module * Change to one axis base class * cleanup axis merge * change to one D1 axis module * fix imports for merged axis * make D1 available as a z axis * fix import * fix import * fix imports for simulation * fix imports * fix simulation * cleanup dev output * cleanup Ui * include the endstops * reactivate safety * fix homing function * rename D1Axis to AxisD1 * cast to int * adapt move_to * cleanup * rename actual_position to position * rework parameters * config for f15 * make position a property * fix namechange * rmenove camera * fix d1 entry * add setters to the d1 * add to testbrain hardware * fix typing and add min and max position * add defaults to z axis * update f15 params * fix d1 bugs * fix config * improve config * fix imports and fix pp mode * improve clear view * fix private functions improve reset fault * better default values, add comments, fix stop * fix merge * Make lizard methods consistent with merged lizrad version --------- Co-authored-by: Pascal Schade <[email protected]>
zauberzeug · Sep 4, 2024 · 60f32ae · 60f32ae
1 parent 2c7f9fb
commit 60f32ae
Show file tree

Hide file tree

Showing 22 changed files with 321 additions and 257 deletions.
diff --git a/config/f15_config_f15/hardware.py b/config/f15_config_f15/hardware.py
@@ -17,32 +17,30 @@
     #     'eyes_pin': 12,
     # },
     'y_axis': {
-        'version': 'none',
-        # 'version': 'axis_d1',
-        # 'name': 'yaxis',
-        # 'can_address': 0x60,
-        # 'homing_acceleration': 2000,
-        # 'homing_velocity': 400,
-        # 'profile_acceleration': 400000,
-        # 'profile_velocity': 14000,
-        # 'profile_deceleration': 500000,
-        # 'min_position': -0.11,
-        # 'max_position': 0.11,
-        # 'axis_offset': 0.115,
+        'version': 'axis_d1',
+        'name': 'yaxis',
+        'can_address': 0x60,
+        'homing_acceleration': 2000,
+        'homing_velocity': 400,
+        'profile_acceleration': 500000,
+        'profile_velocity': 40000,
+        'profile_deceleration': 500000,
+        'min_position': -0.11,
+        'max_position': 0.11,
+        'axis_offset': 0.115,
     },
     'z_axis': {
-        'version': 'none',
-        # 'version': 'axis_d1',
-        # 'name': 'zaxis',
-        # 'can_address': 0x70,
-        # 'homing_acceleration': 1000,
-        # 'homing_velocity': 500,
-        # 'profile_acceleration': 500000,
-        # 'profile_velocity': 14000,
-        # 'profile_deceleration': 500000,
-        # 'min_position': 0.230,
-        # 'max_position': 0,
-        # 'axis_offset': 0.01,
+        'version': 'axis_d1',
+        'name': 'zaxis',
+        'can_address': 0x70,
+        'homing_acceleration': 1000,
+        'homing_velocity': 500,
+        'profile_acceleration': 500000,
+        'profile_velocity': 40000,
+        'profile_deceleration': 500000,
+        'min_position': 0.230,
+        'max_position': 0,
+        'axis_offset': 0.01,
     },
     'estop': {
         'name': 'estop',

diff --git a/config/testbrain_config_rb19/hardware.py b/config/testbrain_config_rb19/hardware.py
@@ -10,20 +10,20 @@
         'is_right_reversed': False,
     },
     'y_axis': {
-        'version': 'none',
+        'version': 'axis_d1',
         'name': 'yaxis',
-        'can_address': 0x70,
-        'max_speed': 2000,
-        'reference_speed': 30,
-        'min_position': -0.125,
-        'max_position': 0.125,
-        'axis_offset': 0.13,
-        'steps_per_m': 1_666_666.667,  # 4000steps/turn motor; 1/10 gear; 0.024m/u
-        'end_r_pin': 12,
-        'end_l_pin': 25,
-        'motor_on_expander': False,
-        'end_stops_on_expander': True,
-        'reversed_direction': False,
+        'can_address': 0x60,
+        'homing_acceleration': 2000,
+        'homing_velocity': 40,
+        'profile_acceleration': 4000,
+        'profile_velocity': 400,
+        'profile_deceleration': 5000,
+        'min_position': 0,
+        'max_position': 23000,
+        'axis_offset': 23000,
+    },
+    'z_axis': {
+        'version': 'none',
     },
     'estop': {
         'name': 'estop',
@@ -40,22 +40,7 @@
     'flashlight': {
         'version': 'none',
     },
-    'z_axis': {
-        'version': 'none',
-        'name': 'zaxis',
-        'can_address': 0x60,
-        'max_speed': 2000,
-        'reference_speed': 30,
-        'min_position': -0.197,
-        'max_position': 0.0,
-        'axis_offset': 0.0,
-        'steps_per_m': 4_000_000,  # 4000steps/turn motor; 1/20 gear; 0.02m/u
-        'end_t_pin': 22,
-        'end_b_pin': 23,
-        'motor_on_expander': False,
-        'end_stops_on_expander': True,
-        'reversed_direction': False,
-    },
+
     'bluetooth': {
         'name': 'TestBrain',
     },

diff --git a/config/testbrain_config_rb19/params.py b/config/testbrain_config_rb19/params.py
@@ -6,4 +6,5 @@
     'work_x': 0.0125,
     'drill_radius': 0.025,
     'tool': 'none',
+    'antenna_offset': 0.205,
 }
diff --git a/config/testbrain_config_rb19/robotbrain.py b/config/testbrain_config_rb19/robotbrain.py
@@ -1,4 +1,4 @@
 configuration = {'robot_brain': {
-    'flash_params': ['']
+    'flash_params': ['nand']
 }}
 # test
diff --git a/field_friend/automations/puncher.py b/field_friend/automations/puncher.py
@@ -5,7 +5,7 @@
 from rosys.driving import Driver
 from rosys.geometry import Point
 
-from ..hardware import ChainAxis, FieldFriend, Tornado, YAxis, ZAxis
+from ..hardware import Axis, ChainAxis, FieldFriend, Tornado
 from .kpi_provider import KpiProvider
 
 
@@ -83,7 +83,7 @@ async def punch(self,
                 if not self.field_friend.y_axis.min_position <= y <= self.field_friend.y_axis.max_position:
                     rosys.notify('y position out of range', type='negative')
                     raise PuncherException('y position out of range')
-            elif isinstance(self.field_friend.y_axis, YAxis):
+            elif isinstance(self.field_friend.y_axis, Axis):
                 if not self.field_friend.y_axis.min_position <= y <= self.field_friend.y_axis.max_position:
                     rosys.notify('y position out of range', type='negative')
                     raise PuncherException('y position out of range')
@@ -92,7 +92,7 @@ async def punch(self,
                 await self.field_friend.y_axis.move_to(y)
                 await self.tornado_drill(angle=angle, turns=turns, with_open_drill=with_open_tornado)
 
-            elif isinstance(self.field_friend.z_axis, ZAxis):
+            elif isinstance(self.field_friend.z_axis, Axis):
                 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)
@@ -119,7 +119,10 @@ async def clear_view(self) -> None:
         if isinstance(self.field_friend.y_axis, ChainAxis):
             await self.field_friend.y_axis.return_to_reference()
             return
-        elif isinstance(self.field_friend.y_axis, YAxis):
+        elif isinstance(self.field_friend.y_axis, Axis):
+            if isinstance(self.field_friend.z_axis,Axis):
+                if self.field_friend.z_axis.position != 0:
+                    await self.field_friend.z_axis.return_to_reference()
             y = self.field_friend.y_axis.min_position if self.field_friend.y_axis.position <= 0 else self.field_friend.y_axis.max_position
             await self.field_friend.y_axis.move_to(y, speed=self.field_friend.y_axis.max_speed)
         await self.field_friend.y_axis.stop()

diff --git a/field_friend/hardware/__init__.py b/field_friend/hardware/__init__.py
@@ -1,3 +1,5 @@
+from .axis import Axis, AxisSimulation
+from .axis_D1 import AxisD1
 from .can_open_master import CanOpenMasterHardware
 from .chain_axis import ChainAxis, ChainAxisHardware, ChainAxisSimulation
 from .double_wheels import DoubleWheelsHardware
@@ -6,19 +8,25 @@
 from .field_friend_hardware import FieldFriendHardware
 from .field_friend_simulation import FieldFriendSimulation
 from .flashlight import Flashlight, FlashlightHardware, FlashlightSimulation
-from .flashlight_pwm import FlashlightPWM, FlashlightPWMHardware, FlashlightPWMSimulation
-from .flashlight_pwm_v2 import FlashlightPWMHardwareV2, FlashlightPWMSimulationV2, FlashlightPWMV2
+from .flashlight_pwm import (
+    FlashlightPWM,
+    FlashlightPWMHardware,
+    FlashlightPWMSimulation,
+)
+from .flashlight_pwm_v2 import (
+    FlashlightPWMHardwareV2,
+    FlashlightPWMSimulationV2,
+    FlashlightPWMV2,
+)
 from .flashlight_v2 import FlashlightHardwareV2, FlashlightSimulationV2, FlashlightV2
 from .imu import IMUHardware
 from .led_eyes import LedEyesHardware
 from .safety import Safety, SafetyHardware, SafetySimulation
 from .safety_small import SmallSafetyHardware
 from .status_control import StatusControlHardware
+from .teltonika_router import TeltonikaRouter
 from .tornado import Tornado, TornadoHardware, TornadoSimulation
-from .y_axis import YAxis, YAxisSimulation
 from .y_axis_canopen_hardware import YAxisCanOpenHardware
 from .y_axis_stepper_hardware import YAxisStepperHardware
-from .z_axis import ZAxis, ZAxisSimulation
 from .z_axis_canopen_hardware import ZAxisCanOpenHardware
 from .z_axis_stepper_hardware import ZAxisStepperHardware
-from .teltonika_router import TeltonikaRouter
diff --git a/field_friend/hardware/z_axis.py → field_friend/hardware/axis.py b/field_friend/hardware/z_axis.py → field_friend/hardware/axis.py
@@ -4,7 +4,7 @@
 import rosys
 
 
-class ZAxis(rosys.hardware.Module, abc.ABC):
+class Axis(rosys.hardware.Module, abc.ABC):
 
     def __init__(self,
                  max_speed,
@@ -25,12 +25,14 @@ def __init__(self,
         self.steps_per_m: float = steps_per_m
         self.reversed_direction: bool = reversed_direction
 
-        self.is_referenced: bool = False
         self.steps: int = 0
+        self.is_referenced: bool = False
         self.alarm: bool = False
         self.idle: bool = False
         self.end_t: bool = False
         self.end_b: bool = False
+        self.end_l: bool = False
+        self.end_r: bool = False
 
         rosys.on_shutdown(self.stop)
 
@@ -45,9 +47,10 @@ async def move_to(self, position: float, speed: int | None = None) -> None:
         if not self.is_referenced:
             raise RuntimeError('zaxis is not referenced, reference first')
         if speed > self.max_speed:
-            raise RuntimeError('zaxis speed is too high')
+            raise RuntimeError(f'axis speed is too high, max speed is {self.max_speed}')
         if not self.min_position <= position <= self.max_position:
-            raise RuntimeError('zaxis depth is out of range')
+            raise RuntimeError(
+                f'target position {position} ist out of axis range {self.min_position} to {self.max_position}')
 
     @abc.abstractmethod
     async def try_reference(self) -> bool:
@@ -74,7 +77,7 @@ async def return_to_reference(self) -> None:
             self.log.error(f'could not return zaxis to reference because of {e}')
 
 
-class ZAxisSimulation(ZAxis, rosys.hardware.ModuleSimulation):
+class AxisSimulation(Axis, rosys.hardware.ModuleSimulation):
     '''The z axis simulation module is a simple example for a representation of simulated robot hardware.
     '''
 
@@ -110,7 +113,7 @@ async def move_to(self, position: float, speed: int | None = None) -> None:
         try:
             await super().move_to(position, speed)
         except RuntimeError as e:
-            self.log.error(f'could not move zaxis to {position} because of {e}')
+            self.log.error(f'could not move axis to {position} because of {e}')
             return
         self.target_steps = self.compute_steps(position)
         self.speed = speed if self.target_steps > self.steps else speed * -1