diff --git a/being/awakening.py b/being/awakening.py index c35b72e..4b17ab9 100644 --- a/being/awakening.py +++ b/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,11 +150,12 @@ 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. @@ -161,7 +163,6 @@ def awake( 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() diff --git a/being/being.py b/being/being.py index b7d054c..d78a24c 100644 --- a/being/being.py +++ b/being/being.py @@ -6,9 +6,7 @@ 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 @@ -16,6 +14,7 @@ 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. diff --git a/being/motors/blocks.py b/being/motors/blocks.py index cb46c55..5343c04 100644 --- a/being/motors/blocks.py +++ b/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() diff --git a/being/motors/controllers.py b/being/motors/controllers.py index 54a1d79..a14c4c5 100644 --- a/being/motors/controllers.py +++ b/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) diff --git a/being/motors/homing.py b/being/motors/homing.py index 7e463e0..49e062a 100644 --- a/being/motors/homing.py +++ b/being/motors/homing.py @@ -27,7 +27,7 @@ UNDEFINED, determine_homing_method ) -from being.constants import INF +from being.constants import INF, BACKWARD from being.logging import get_logger from being.serialization import register_enum from being.utils import toss_coin @@ -57,6 +57,7 @@ def default_homing_method( homingDirection: int = UNDEFINED, endSwitches: bool = False, indexPulse: bool = False, + **kwargs ) -> int: """Determine homing method from default homing kwargs.""" if homingMethod is not None: @@ -172,6 +173,11 @@ def homing_job(self) -> Generator: """Primary homing job.""" raise NotImplementedError + @abc.abstractmethod + def pre_homing_job(self, direction) -> Generator: + """Primary homing job.""" + raise NotImplementedError + def home(self): """Start homing.""" self.logger.debug('home()') @@ -183,6 +189,17 @@ def home(self): self.job = self.homing_job() self.state = HomingState.ONGOING + def pre_home(self, direction): + """Start pre-homing.""" + self.logger.debug('pre_home()') + self.logger.debug('Starting pre-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: @@ -240,10 +257,10 @@ class CiA402Homing(HomingBase): """CiA 402 by the book.""" - def __init__(self, node, timeout=10.0, **kwargs): + def __init__(self, node, homingTimeout=10.0, **kwargs): super().__init__() self.node = node - self.timeout = timeout + self.timeout = homingTimeout self.homingMethod = default_homing_method(**kwargs) self.logger = get_logger(f'CiA402Homing(nodeId: {node.id})') @@ -303,24 +320,24 @@ def homing_job(self): def __str__(self): return f'{type(self).__name__}({self.node}, {self.state})' + def pre_homing_job(self, direction): + pass + class CrudeHoming(CiA402Homing): """Crude hard stop homing for Faulhaber linear motors. - - Args: - speed: Speed for homing in device units. """ - def __init__(self, node, minWidth, currentLimit, timeout=10.0, **kwargs): - super().__init__(node, timeout=timeout, **kwargs) + def __init__(self, node, minWidth, homeOffset, currentLimit, + homingTimeout=10.0, homingSpeed=100, **kwargs): + super().__init__(node, homingTimeout=homingTimeout, **kwargs) + self.speed = homingSpeed self.minWidth = minWidth + self.homeOffset = homeOffset self.currentLimit = currentLimit self.lower = INF self.upper = -INF - self.logger.info('Overwriting TxPDO4 of %s for Current Actual Value', node) - node.setup_txpdo(4, 'Current Actual Value') - @property def width(self) -> float: """Current homing width in device units.""" @@ -362,7 +379,8 @@ def on_the_wall(self) -> bool: current = self.node.pdo['Current Actual Value'].raw return current > self.currentLimit # Todo: Add percentage threshold? - def homing_job(self, speed: int = 100): + def homing_job(self): + speed = self.speed self.logger.debug('homing_job()') self.start_timeout_clock() self.lower = INF @@ -381,6 +399,9 @@ def homing_job(self, speed: int = 100): sdo['Home Offset'].raw = 0 self.set_operation_mode(OperationMode.PROFILE_VELOCITY) + self.logger.debug('Overwriting TxPDO4 of %s for "Current Actual Value"', node) + node.setup_txpdo(4, 'Current Actual Value') + for vel in velocities: yield from self.halt_drive() yield from self.move_drive(vel) @@ -409,7 +430,39 @@ def homing_job(self, speed: int = 100): margin = .5 * (width - self.minWidth) self.lower += margin self.upper -= margin + self.logger.debug(f'{self.lower} {self.upper}') + sdo['Home Offset'].raw = self.lower + self.homeOffset + + self.node.enable() - sdo['Home Offset'].raw = self.lower + self.logger.debug('Disabling TxPDO4 of %s', node) + node.setup_txpdo(4, enabled=False) self.state = final + + def pre_homing_job(self, direction: float = BACKWARD): + """Moves motor to one end for safe homing.""" + self.logger.debug('pre_homing_job()') + node = self.node + + self.logger.debug('Overwriting TxPDO4 of %s for "Current Actual Value"', node) + node.setup_txpdo(4, 'Current Actual Value') + + 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.logger.debug('Disabling TxPDO4 of %s', node) + node.setup_txpdo(4, enabled=False) + + self.state = HomingState.UNHOMED