Add and use method for state-to-state traversal

roboticslab-uc3m · Sep 27, 2019 · 4d492af · 4d492af
1 parent 2a40e00
commit 4d492af
Show file tree

Hide file tree

Showing 3 changed files with 71 additions and 11 deletions.
diff --git a/libraries/CanBusSharerLib/DriveStatusMachine.cpp b/libraries/CanBusSharerLib/DriveStatusMachine.cpp
@@ -4,6 +4,7 @@
 
 #include <bitset>
 #include <unordered_map>
+#include <vector>
 
 using namespace roboticslab;
 
@@ -89,7 +90,7 @@ namespace
         }
     }
 
-    std::unordered_map<std::pair<DriveState, DriveTransition>, DriveState> transitionToState = {
+    const std::unordered_map<std::pair<DriveState, DriveTransition>, DriveState> transitionToState = {
         {std::make_pair(DriveState::SWITCH_ON_DISABLED, DriveTransition::SHUTDOWN), DriveState::READY_TO_SWITCH_ON}, // 2
         {std::make_pair(DriveState::READY_TO_SWITCH_ON, DriveTransition::SWITCH_ON), DriveState::SWITCHED_ON}, // 3
         {std::make_pair(DriveState::SWITCHED_ON, DriveTransition::ENABLE_OPERATION), DriveState::OPERATION_ENABLED}, // 4
@@ -104,6 +105,23 @@ namespace
         {std::make_pair(DriveState::QUICK_STOP_ACTIVE, DriveTransition::DISABLE_VOLTAGE), DriveState::SWITCH_ON_DISABLED}, // 12
         {std::make_pair(DriveState::QUICK_STOP_ACTIVE, DriveTransition::ENABLE_OPERATION), DriveState::OPERATION_ENABLED}, // 16
     };
+
+    const std::unordered_map<std::pair<DriveState, DriveState>, std::vector<DriveTransition>> shortestPaths = {
+        {std::make_pair(DriveState::SWITCH_ON_DISABLED, DriveState::READY_TO_SWITCH_ON), {DriveTransition::SHUTDOWN}},
+        {std::make_pair(DriveState::SWITCH_ON_DISABLED, DriveState::SWITCHED_ON), {DriveTransition::SHUTDOWN, DriveTransition::SWITCH_ON}},
+        {std::make_pair(DriveState::SWITCH_ON_DISABLED, DriveState::OPERATION_ENABLED), {DriveTransition::SHUTDOWN, DriveTransition::SWITCH_ON, DriveTransition::ENABLE_OPERATION}},
+        {std::make_pair(DriveState::READY_TO_SWITCH_ON, DriveState::SWITCHED_ON), {DriveTransition::SWITCH_ON}},
+        {std::make_pair(DriveState::READY_TO_SWITCH_ON, DriveState::OPERATION_ENABLED), {DriveTransition::SWITCH_ON, DriveTransition::ENABLE_OPERATION}},
+        {std::make_pair(DriveState::SWITCHED_ON, DriveState::OPERATION_ENABLED), {DriveTransition::ENABLE_OPERATION}},
+        {std::make_pair(DriveState::OPERATION_ENABLED, DriveState::SWITCHED_ON), {DriveTransition::DISABLE_OPERATION}},
+        {std::make_pair(DriveState::OPERATION_ENABLED, DriveState::READY_TO_SWITCH_ON), {DriveTransition::SHUTDOWN}},
+        {std::make_pair(DriveState::OPERATION_ENABLED, DriveState::SWITCH_ON_DISABLED), {DriveTransition::DISABLE_VOLTAGE}},
+        {std::make_pair(DriveState::SWITCHED_ON, DriveState::READY_TO_SWITCH_ON), {DriveTransition::SHUTDOWN}},
+        {std::make_pair(DriveState::SWITCHED_ON, DriveState::SWITCH_ON_DISABLED), {DriveTransition::DISABLE_VOLTAGE}},
+        {std::make_pair(DriveState::READY_TO_SWITCH_ON, DriveState::SWITCH_ON_DISABLED), {DriveTransition::DISABLE_VOLTAGE}},
+        {std::make_pair(DriveState::READY_TO_SWITCH_ON, DriveState::SWITCHED_ON), {DriveTransition::SWITCH_ON}},
+        {std::make_pair(DriveState::QUICK_STOP_ACTIVE, DriveState::SWITCH_ON_DISABLED), {DriveTransition::DISABLE_VOLTAGE}}
+    };
 }
 
 bool DriveStatusMachine::update(std::uint16_t statusword)
@@ -134,20 +152,54 @@ DriveState DriveStatusMachine::getCurrentState() const
 
 bool DriveStatusMachine::requestTransition(DriveTransition transition, bool wait)
 {
+    DriveState initialState = getCurrentState();
+    auto op = std::make_pair(initialState, transition);
+    auto it = transitionToState.find(op);
+
+    if (it == transitionToState.cend())
+    {
+        return false;
+    }
+
     prepareDriveTransition(transition, _controlword);
     std::uint16_t data = _controlword.to_ulong();
 
-    DriveState initialState = getCurrentState();
-
     if (!rpdo->write(data))
     {
         return false;
     }
 
     if (wait)
     {
-        auto op = std::make_pair(initialState, transition);
-        return stateObserver.await() && transitionToState[op] == getCurrentState();
+        return stateObserver.await() && it->second == getCurrentState();
+    }
+
+    return true;
+}
+
+bool DriveStatusMachine::requestState(DriveState goalState, bool wait)
+{
+    DriveState initialState = getCurrentState();
+
+    if (goalState == initialState)
+    {
+        return true;
+    }
+
+    auto op = std::make_pair(initialState, goalState);
+    auto it = shortestPaths.find(op);
+
+    if (it == shortestPaths.cend())
+    {
+        return false;
+    }
+
+    for (const auto & transition : it->second)
+    {
+        if (!requestTransition(transition, wait))
+        {
+            return false;
+        }
     }
 
     return true;

diff --git a/libraries/CanBusSharerLib/DriveStatusMachine.hpp b/libraries/CanBusSharerLib/DriveStatusMachine.hpp
@@ -56,6 +56,7 @@ class DriveStatusMachine
     const word_t & statusword() const;
     DriveState getCurrentState() const;
     bool requestTransition(DriveTransition transition, bool wait = true);
+    bool requestState(DriveState goalState, bool wait = true);
 
 private:
     word_t _controlword;

diff --git a/libraries/YarpPlugins/TechnosoftIpos/ICanBusSharerImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/ICanBusSharerImpl.cpp
@@ -251,18 +251,25 @@ bool TechnosoftIpos::initialize()
         return setEncoderRaw(0, extEnc);
     }
 
-    return can->nmt()->issueServiceCommand(NmtService::START_REMOTE_NODE)
-            && can->driveStatus()->requestTransition(DriveTransition::SHUTDOWN)
-            && can->driveStatus()->requestTransition(DriveTransition::SWITCH_ON)
-            && can->driveStatus()->requestTransition(DriveTransition::ENABLE_OPERATION);
+    if (!can->nmt()->issueServiceCommand(NmtService::START_REMOTE_NODE))
+    {
+        return false;
+    }
+
+    if (!can->driveStatus()->requestState(DriveState::SWITCHED_ON))
+    {
+        return false;
+    }
+
+    modeCurrentTorque = VOCAB_CM_IDLE; // TODO: rename
+    return true;
 }
 
 // -----------------------------------------------------------------------------
 
 bool TechnosoftIpos::finalize()
 {
-    return can->driveStatus()->requestTransition(DriveTransition::SWITCH_ON)
-            && can->driveStatus()->requestTransition(DriveTransition::SHUTDOWN);
+    return can->driveStatus()->requestState(DriveState::SWITCH_ON_DISABLED);
 }
 
 // -----------------------------------------------------------------------------