DS402: Fix logic errors in the power state machine and generalize it. (#264)

* p402: Forbid transitions to states which only the drive can enter.

The NOT READY TO SWITCH ON and FAULT REACTION ACTIVE states can only
be reached if the drive encounters an error or is still busy with its
self-testing.  Raise an exception in case these states are requested
via the property.

In extension, only the drive can trigger a transition to the FAULT
state, so that is never valid as an end goal requested by the user.

* p402: Add a test script to check DS402 State Machine transitions.

Go through all possible target states and display where the
next_state_for_enabling() function would lead to based on the original
state.  Mark transitions which can happen directly because they are in
the TRANSITIONTABLE.

* p402: Extend test script to check the actual implementation.

Mock up a BaseNode402 object and compare the _next_state() behavior to
the simple lookups from the underlying transition tables.

* p402: Simplify check for intermediate states.

Don't special case the OPERATION ENABLED state, as the mechanism
formulated for it is actually usable for almost all states.  Instead,
check whether there is a direct transition and return that immediately
before consulting next_state_for_enabling().

* p402: Rename NEXTSTATE2ENABLE to generalize it.

The goal is to provide automatic transition paths for any state, not
only OPERATION ENABLED.  Reflect that in the naming, without changing
the actual logic yet.

* p402: Adjust automatic state transition paths.

As there is a direct transition from QUICK STOP ACTIVE to OPERATION
ENABLED, remove that from the transition paths.  Instead go through
the SWITCH ON DISABLED state, closing the cycle to make it work for
anything between SWITCH ON DISABLED and OPERATION ENABLED.

Also remove the self-reference OPERATION ENABLED to itself, which is
useless.  The whole state changing code will only be called if the
target state and the current state do not match.

* p402: Remove two illegal transitions from the state table.

Transitions 7 and 10 are duplicated and certainly wrong in the
quickstop context.  The only transition toward QUICK STOP ACTIVE is
from OPERATION ENABLED.

* Move test_p402_states script to a subdirectory and add a docstring.

Author: acolomb

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
 
@@ -551,10 +554,16 @@ def state(self, target_state):
             self.check_statusword()
 
     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:

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!')