sequential homing v2

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
@@ -149,19 +150,19 @@ def _say_hello(nBlocks: int):
 def awake(
         *blocks: Iterable[Block],
         web: bool = True,
-        enableMotors: bool = True,
         homeMotors: bool = True,
         usePacemaker: bool = True,
         clock: Optional[Clock] = None,
         network: Optional[CanBackend] = None,
+        sequential_homing: bool = False,
+        pre_homing: bool = False,
     ):
     """Run being block network.
 
     Args:
         blocks: Some blocks of the network. Remaining blocks will be auto
             discovered.
         web: Run with web server.
-        enableMotors: Enable motors on startup.
         homeMotors: Home motors on startup.
         usePacemaker: If to use an extra pacemaker thread.
         clock: Clock instance.
@@ -174,7 +175,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)
 
     if network is not None:
         network.reset_communication()
@@ -186,10 +187,7 @@ def awake(
         network.send_sync()  # Update local TXPDOs values
         time.sleep(0.200)
 
-    if enableMotors:
-        being.enable_motors()
-    else:
-        being.disable_motors()
+    being.disable_motors()
 
     if homeMotors:
         being.home_motors()

being/being.py

@@ -6,16 +6,15 @@
 from being.backends import CanBackend
 from being.behavior import Behavior
 from being.block import Block
-from being.can.nmt import OPERATIONAL, PRE_OPERATIONAL
 from being.clock import Clock
-from being.configuration import CONFIG
 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
@@ -48,7 +47,6 @@ def message_outputs(blocks: Iterable[Block]) -> Iterator[MessageOutput]:
 
 
 class Being:
-
     """Being core.
 
     Main application-like object. Container for being components. Block network
@@ -62,6 +60,8 @@ def __init__(self,
             clock: Clock,
             pacemaker: Pacemaker,
             network: Optional[CanBackend] = None,
+            sequential_homing: bool = False,
+            pre_homing: bool = False,
         ):
         """
         Args:
@@ -105,6 +105,19 @@ 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.motors_unhomed = None
+        """Iterator for sequential homing."""
+
+        if sequential_homing:
+            for motor in self.motors:
+                motor.controller.subscribe(MotorEvent.HOMING_CHANGED, lambda m=motor: self.next_homing(m))
+
     def enable_motors(self):
         """Enable all motor blocks."""
         self.logger.info('enable_motors()')
@@ -120,8 +133,24 @@ 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.sequential_homing:
+            self.pause_behaviors()
+            self.motors_unhomed = iter(sorted(self.motors, key=lambda x: x.id))
+
+        if self.pre_homing:
+            for motor in self.motors:
+                motor.pre_home()  # will trigger sequential homing (if enabled) at the end
+
+        if self.sequential_homing:
+            if not self.pre_homing:
+                motor = next(self.motors_unhomed)
+                motor.home()
+                motor.enable()
+        else:
+            # normal parallel homing
+            for motor in self.motors:
+                motor.home()
+                motor.enable()
 
     def start_behaviors(self):
         """Start all behaviors."""
@@ -133,6 +162,27 @@ def pause_behaviors(self):
         for behavior in self.behaviors:
             behavior.pause()
 
+    def next_homing(self, motor_done):
+        """Controls one by one homing."""
+        if any(motor.homing_state() is HomingState.ONGOING for motor in self.motors):
+            return
+        else:
+            state = motor_done.homing_state()
+            if state == HomingState.FAILED:
+                self.logger.warning(f'retrying homing on {motor_done}')
+                motor_done.home()
+                return
+            elif state == HomingState.HOMED:
+                motor_done.enable()
+            if self.motors_unhomed is not None:
+                try:
+                    motor = next(self.motors_unhomed)
+                    motor.home()
+                    motor.enable()
+                except StopIteration:
+                    self.logger.info('All motors are homed.')
+                    self.start_behaviors()
+
     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):
+        self.controller.pre_home()
+
     def homing_state(self):
         return self.controller.homing_state()
 

being/motors/controllers.py

@@ -134,6 +134,7 @@ def __init__(self,
             direction: float = FORWARD,
             settings: Optional[dict] = None,
             operationMode: OperationMode = OperationMode.CYCLIC_SYNCHRONOUS_POSITION,
+            preHomingDirection = None,
             **homingKwargs,
         ):
         """
@@ -188,13 +189,13 @@ def __init__(self,
         self.switchJob = None
         """Ongoing state switching job."""
 
-        self.wasEnabled: Optional[bool] = None  # None means "has not been set"
-
         # Prepare settings
         self.settings = merge_dicts(self.motor.defaultSettings, settings)
         """Final motor settings (which got applied to the drive."""
 
+        self.homing = None
         self.init_homing(**homingKwargs)
+        self.preHomingDirection = preHomingDirection
 
         current_state = self.node.get_state()
         self.logger.debug(f'current state: {current_state}')
@@ -207,8 +208,8 @@ def __init__(self,
         """Last receive state of motor controller."""
 
         # Configure node
-        self.apply_motor_direction(direction)
         self.node.apply_settings(self.settings)
+        self.apply_motor_direction(direction)
         for errMsg in self.error_history_messages():
             self.logger.error(errMsg)
 
@@ -237,24 +238,12 @@ def motor_state(self) -> MotorState:
 
     def capture(self):
         """Capture node state before homing."""
-        # If switchJob ongoing ignore
-        if not self.switchJob and self.wasEnabled is None:
-            self.wasEnabled = self.lastState is State.OPERATION_ENABLED
-        else:
-            self.wasEnabled = None
+        pass
 
     def restore(self):
         """Restore captured node state after homing is done."""
         self.node.sdo[MODES_OF_OPERATION].raw = self.operationMode
-
-        if self.wasEnabled is None:
-            pass
-        elif self.wasEnabled:
-            self.enable()
-        else:
-            self.disable()
-
-        self.wasEnabled = None
+        self.set_target_position(0)
 
     def home(self):
         """Start homing for this controller. Will start by the next call of
@@ -263,13 +252,23 @@ def home(self):
         self.logger.debug('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.home()
 
-        self.publish(MotorEvent.HOMING_CHANGED)
+    def pre_home(self):
+        """Start pre-homing for this controller. Will start by the next call of
+        :meth:`Controller.update`.
+        """
+        if self.preHomingDirection is not None:
+            self.logger.debug('pre_home()')
+            if self.homing.ongoing:
+                self.homing.stop()
+                self.restore()
+            else:
+                self.capture()
+                self.homing.pre_home(self.preHomingDirection)
 
     def homing_state(self) -> HomingState:
         return self.homing.state
@@ -282,11 +281,11 @@ def init_homing(self, **homingKwargs):
         Args:
             **homingKwargs: Arbitrary keyword arguments for Homing.
         """
-        method = default_homing_method(**homingKwargs)
+
+        self.homing = CiA402Homing(self.node, **homingKwargs)
+        method = self.homing.homingMethod
         if method not in self.SUPPORTED_HOMING_METHODS:
             raise ValueError(f'Homing method {method} not supported for controller {self}')
-
-        self.homing = CiA402Homing(self.node)
         self.logger.debug('Setting homing method to %d', method)
         self.node.sdo[HOMING_METHOD].raw = method
 
@@ -411,14 +410,14 @@ class Mclm3002(Controller):
 
     def __init__(self,
             *args,
-            homingMethod: Optional[int] = None,
             homingDirection: float = FORWARD,
+            homeOffset: float = 0,
             operationMode: OperationMode = OperationMode.CYCLIC_SYNCHRONOUS_POSITION,
             **kwargs,
         ):
+        self.homeOffset = homeOffset
         super().__init__(
             *args,
-            homingMethod=homingMethod,
             homingDirection=homingDirection,
             operationMode=operationMode,
             **kwargs,
@@ -429,7 +428,9 @@ def init_homing(self, **homingKwargs):
         if method in self.HARD_STOP_HOMING:
             minWidth = self.position_si_2_device * self.length
             currentLimit = self.settings['Current Control Parameter Set/Continuous Current Limit']
-            self.homing = CrudeHoming(self.node, minWidth, homingMethod=method, currentLimit=currentLimit)
+            self.homing = CrudeHoming(self.node, minWidth, homingMethod=method,
+                                      homeOffset=self.homeOffset,
+                                      currentLimit=currentLimit, **homingKwargs)
         else:
             super().init_homing(homingMethod=method)