diff --git a/canopen/profiles/p402.py b/canopen/profiles/p402.py index f5233ea4..6d09a411 100644 --- a/canopen/profiles/p402.py +++ b/canopen/profiles/p402.py @@ -46,14 +46,14 @@ class State402(object): 'QUICK STOP ACTIVE': (0x6F, 0x07), } - # Transition path to get to the 'OPERATION ENABLED' state - NEXTSTATE2ENABLE = { + # Transition path to reach and state without a direct transition + NEXTSTATE2ANY = { ('START'): 'NOT READY TO SWITCH ON', - ('FAULT', 'NOT READY TO SWITCH ON'): 'SWITCH ON DISABLED', + ('FAULT', 'NOT READY TO SWITCH ON', 'QUICK STOP ACTIVE'): 'SWITCH ON DISABLED', ('SWITCH ON DISABLED'): 'READY TO SWITCH ON', ('READY TO SWITCH ON'): 'SWITCHED ON', - ('SWITCHED ON', 'QUICK STOP ACTIVE', 'OPERATION ENABLED'): 'OPERATION ENABLED', - ('FAULT REACTION ACTIVE'): 'FAULT' + ('SWITCHED ON'): 'OPERATION ENABLED', + ('FAULT REACTION ACTIVE'): 'FAULT', } # Tansition table from the DS402 State Machine @@ -78,22 +78,25 @@ class State402(object): ('SWITCHED ON', 'OPERATION ENABLED'): CW_OPERATION_ENABLED, # transition 4 ('QUICK STOP ACTIVE', 'OPERATION ENABLED'): CW_OPERATION_ENABLED, # transition 16 # quickstop --------------------------------------------------------------------------- - ('READY TO SWITCH ON', 'QUICK STOP ACTIVE'): CW_QUICK_STOP, # transition 7 - ('SWITCHED ON', 'QUICK STOP ACTIVE'): CW_QUICK_STOP, # transition 10 ('OPERATION ENABLED', 'QUICK STOP ACTIVE'): CW_QUICK_STOP, # transition 11 # fault ------------------------------------------------------------------------------- ('FAULT', 'SWITCH ON DISABLED'): CW_SWITCH_ON_DISABLED, # transition 15 } @staticmethod - def next_state_for_enabling(_from): - """Return the next state needed for reach the state Operation Enabled. + def next_state_indirect(_from): + """Return the next state needed to reach any state indirectly. + + The chosen path always points toward the OPERATION ENABLED state, except when + coming from QUICK STOP ACTIVE. In that case, it will cycle through SWITCH ON + DISABLED first, as there would have been a direct transition if the opposite was + desired. :param str target: Target state. :return: Next target to change. :rtype: str """ - for cond, next_state in State402.NEXTSTATE2ENABLE.items(): + for cond, next_state in State402.NEXTSTATE2ANY.items(): if _from in cond: return next_state @@ -497,10 +500,16 @@ def state(self, target_state): time.sleep(self.INTERVAL_CHECK_STATE) def _next_state(self, target_state): - if target_state == 'OPERATION ENABLED': - return State402.next_state_for_enabling(self.state) - else: + if target_state in ('NOT READY TO SWITCH ON', + 'FAULT REACTION ACTIVE', + 'FAULT'): + raise ValueError( + 'Target state {} cannot be entered programmatically'.format(target_state)) + from_state = self.state + if (from_state, target_state) in State402.TRANSITIONTABLE: return target_state + else: + return State402.next_state_indirect(from_state) def _change_state(self, target_state): try: diff --git a/canopen/profiles/tools/test_p402_states.py b/canopen/profiles/tools/test_p402_states.py new file mode 100644 index 00000000..39f085f5 --- /dev/null +++ b/canopen/profiles/tools/test_p402_states.py @@ -0,0 +1,37 @@ +"""Verification script to diagnose automatic state transitions. + +This is meant to be run for verifying changes to the DS402 power state +machine code. For each target state, it just lists the next +intermediate state which would be set automatically, depending on the +assumed current state. +""" + +from canopen.objectdictionary import ObjectDictionary +from canopen.profiles.p402 import State402, BaseNode402 + + +if __name__ == '__main__': + n = BaseNode402(1, ObjectDictionary()) + + for target_state in State402.SW_MASK: + print('\n--- Target =', target_state, '---') + for from_state in State402.SW_MASK: + if target_state == from_state: + continue + if (from_state, target_state) in State402.TRANSITIONTABLE: + print('direct:\t{} -> {}'.format(from_state, target_state)) + else: + next_state = State402.next_state_indirect(from_state) + if not next_state: + print('FAIL:\t{} -> {}'.format(from_state, next_state)) + else: + print('\t{} -> {} ...'.format(from_state, next_state)) + + try: + while from_state != target_state: + n.tpdo_values[0x6041] = State402.SW_MASK[from_state][1] + next_state = n._next_state(target_state) + print('\t\t-> {}'.format(next_state)) + from_state = next_state + except ValueError: + print('\t\t-> disallowed!')