Feature/sequential homing

being/awakening.py

@@ -22,6 +22,7 @@
 from being.clock import Clock
 from being.configuration import CONFIG
 from being.connectables import MessageInput
+from being.constants import FORWARD, BACKWARD
 from being.logging import get_logger
 from being.pacemaker import Pacemaker
 from being.resources import register_resource
@@ -155,6 +156,9 @@ def awake(
         usePacemaker: bool = True,
         clock: Optional[Clock] = None,
         network: Optional[CanBackend] = None,
+        sequential_homing: bool = False,
+        pre_homing: bool = False,
+        pre_homing_direction: float = BACKWARD,
     ):
     """Run being block network.
 
@@ -175,7 +179,7 @@ def awake(
         network = CanBackend.single_instance_get()
 
     pacemaker = Pacemaker(network)
-    being = Being(blocks, clock, pacemaker, network)
+    being = Being(blocks, clock, pacemaker, network, sequential_homing, pre_homing, pre_homing_direction)
 
     if network is not None:
         network.reset_communication()

being/being.py

@@ -9,13 +9,15 @@
 from being.can.nmt import OPERATIONAL, PRE_OPERATIONAL
 from being.clock import Clock
 from being.configuration import CONFIG
+from being.constants import FORWARD, BACKWARD
 from being.connectables import ValueOutput, MessageOutput
 from being.execution import execute, block_network_graph
 from being.graph import Graph, topological_sort
 from being.logging import get_logger
 from being.motion_player import MotionPlayer
 from being.motors.blocks import MotorBlock
 from being.motors.homing import HomingState
+from being.motors.definitions import MotorEvent
 from being.pacemaker import Pacemaker
 from being.params import Parameter
 from being.utils import filter_by_type
@@ -62,6 +64,9 @@ def __init__(self,
             clock: Clock,
             pacemaker: Pacemaker,
             network: Optional[CanBackend] = None,
+            sequential_homing: bool = False,
+            pre_homing: bool = False,
+            pre_homing_direction: float = BACKWARD,
         ):
         """
         Args:
@@ -105,6 +110,22 @@ def __init__(self,
         self.params: List[Parameter] = list(filter_by_type(self.execOrder, Parameter))
         """All parameter blocks."""
 
+        self.sequential_homing: bool = sequential_homing
+        """One by one homing."""
+
+        self.pre_homing: bool = pre_homing
+        """Moves motors to safe position before homing."""
+
+        self.pre_homing_direction: float = pre_homing_direction
+        """Direction for safe homing."""
+
+        self.motors_unhomed: Iterator = iter(self.motors)
+        """Iterator for sequential homing."""
+
+        if sequential_homing:
+            for motor in self.motors:
+                motor.controller.subscribe(MotorEvent.HOMING_CHANGED, lambda: self.next_homing())
+
     def enable_motors(self):
         """Enable all motor blocks."""
         self.logger.info('enable_motors()')
@@ -120,8 +141,14 @@ def disable_motors(self):
     def home_motors(self):
         """Home all motors."""
         self.logger.info('home_motors()')
-        for motor in self.motors:
-            motor.home()
+        if self.pre_homing:
+            for motor in self.motors:
+                motor.pre_home(self.pre_homing_direction)
+        elif self.sequential_homing:
+            next(self.motors_unhomed).home()
+        else:
+            for motor in self.motors:
+                motor.home()
 
     def start_behaviors(self):
         """Start all behaviors."""
@@ -133,6 +160,16 @@ def pause_behaviors(self):
         for behavior in self.behaviors:
             behavior.pause()
 
+    def next_homing(self):
+        """Controls one by one homing."""
+        if any(motor.homing_state() is HomingState.ONGOING for motor in self.motors):
+            return
+        else:
+            try:
+                next(self.motors_unhomed).home()
+            except StopIteration:
+                self.logger.debug('All motors are homed.')
+
     def single_cycle(self):
         """Execute single being cycle. Network sync, executing block network,
         advancing clock.

being/motors/blocks.py

@@ -362,6 +362,9 @@ def home(self):
         self.controller.home()
         super().home()
 
+    def pre_home(self, direction):
+        self.controller.pre_home(direction)    
+
     def homing_state(self):
         return self.controller.homing_state()
 

being/motors/controllers.py

@@ -204,10 +204,7 @@ def __init__(self,
         """Last receive state of motor controller."""
 
         # Configure node
-        self.apply_motor_direction(direction)
-        self.node.apply_settings(self.settings)
-        for errMsg in self.error_history_messages():
-            self.logger.error(errMsg)
+        self.configure_node()
 
         self.node.set_operation_mode(operationMode)
 
@@ -267,7 +264,18 @@ def home(self):
             self.capture()
             self.homing.home()
 
-        self.publish(MotorEvent.HOMING_CHANGED)
+    def pre_home(self, direction):
+        """Start pre-homing for this controller. Will start by the next call of
+        :meth:`Controller.update`.
+        """
+        self.logger.debug('pre_home()')
+        if self.homing.ongoing:
+            self.homing.stop()
+            self.wasEnabled = False  # Do not re-enable motor since not homed anymore
+            self.restore()
+        else:
+            self.capture()
+            self.homing.pre_home(direction)
 
     def homing_state(self) -> HomingState:
         return self.homing.state
@@ -293,6 +301,11 @@ def apply_motor_direction(self, direction: float):
         """Configure direction or orientation of controller / motor."""
         raise NotImplementedError
 
+    @abc.abstractmethod
+    def configure_node(self):
+        """Apply settings to the controller""" 
+        raise NotImplementedError()
+
     def format_emcy(self, emcy: EmcyError) -> str:
         """Get vendor specific description of EMCY error."""
         return format_error_code(emcy.code, self.EMERGENCY_DESCRIPTIONS)
@@ -431,7 +444,20 @@ def init_homing(self, **homingKwargs):
         else:
             super().init_homing(homingMethod=method)
 
+    def configure_node(self):
+        self.apply_motor_direction(self.direction)
+        if self.direction < 0:
+            # Swap interpretation of min / max positions
+            minimum = self.settings['Software Position Limit/Minimum Position Limit']
+            maximum = self.settings['Software Position Limit/Maximum Position Limit']
+            self.settings['Software Position Limit/Maximum Position Limit'] = minimum
+            self.settings['Software Position Limit/Minimum Position Limit'] = maximum
+        self.node.apply_settings(self.settings)
+        for errMsg in self.error_history_messages():
+            self.logger.error(errMsg)
+
     def apply_motor_direction(self, direction: float):
+        self.logger.debug(f"Apply motor direction {direction}")
         if direction >= 0:
             positivePolarity = 0
             self.node.sdo['Polarity'].raw = positivePolarity
@@ -506,6 +532,12 @@ def firmware_version(self) -> int:
         firmwareVersion = revisionNumber >> lowWord
         return firmwareVersion
 
+    def configure_node(self):
+        self.apply_motor_direction(self.direction)
+        self.node.apply_settings(self.settings)
+        for errMsg in self.error_history_messages():
+            self.logger.error(errMsg)
+
     def apply_motor_direction(self, direction: float):
         variable = self.node.sdo['Axis configuration']['Axis configuration miscellaneous']
         misc = variable.raw

being/motors/homing.py

@@ -171,6 +171,11 @@ def homing_job(self) -> Generator:
         """Primary homing job."""
         raise NotImplementedError
 
+    @abc.abstractmethod
+    def pre_homing_job(self) -> Generator:
+        """Primary homing job."""
+        raise NotImplementedError
+
     def home(self):
         """Start homing."""
         self.logger.debug('home()')
@@ -182,6 +187,17 @@ def home(self):
         self.job = self.homing_job()
         self.state = HomingState.ONGOING
 
+    def pre_home(self, direction):
+        """Start homing."""
+        self.logger.debug('pre_home()')
+        self.logger.debug('Starting safe homing')
+        self.state = HomingState.UNHOMED
+        if self.job:
+            self.cancel_job()
+
+        self.job = self.pre_homing_job(direction)
+        self.state = HomingState.ONGOING
+
     def update(self):
         """Tick homing one step further."""
         if self.job:
@@ -407,3 +423,23 @@ def homing_job(self, speed: int = 100):
             sdo['Home Offset'].raw = self.lower
 
         self.state = final
+
+    def pre_homing_job(self, direction: float = -1.):
+        """Moves motor to one end for safe homing."""
+        self.logger.debug('pre_homing_job()')
+
+        yield from self.change_state(CiA402State.READY_TO_SWITCH_ON)
+        self.set_operation_mode(OperationMode.PROFILE_VELOCITY)
+
+        yield from self.halt_drive()
+        yield from self.move_drive(direction * 100)
+
+        self.logger.debug('Driving towards the wall')
+        while not self.on_the_wall(): # and not self.timeout_expired():
+            yield
+
+        self.logger.debug('Hit the wall')
+
+        yield from self.halt_drive()
+
+        self.state = HomingState.UNHOMED