Merge branch 'new-drive-state' into apocanlypse

roboticslab-uc3m · Dec 28, 2019 · ad3855e · ad3855e
2 parents c0c2522 + 6f39c4b
commit ad3855e
Show file tree

Hide file tree

Showing 10 changed files with 127 additions and 38 deletions.
diff --git a/libraries/CanBusSharerLib/DriveStatusMachine.cpp b/libraries/CanBusSharerLib/DriveStatusMachine.cpp
@@ -76,6 +76,33 @@ namespace
         }
     }
 
+    using word_t = DriveStatusMachine::word_t;
+
+    word_t stateToControlword(DriveState state)
+    {
+        switch (state)
+        {
+        case DriveState::SWITCH_ON_DISABLED:
+            return static_cast<std::uint16_t>(DriveTransition::DISABLE_VOLTAGE);
+        case DriveState::READY_TO_SWITCH_ON:
+            return static_cast<std::uint16_t>(DriveTransition::SHUTDOWN);
+        case DriveState::SWITCHED_ON:
+            return static_cast<std::uint16_t>(DriveTransition::SWITCH_ON); // same as DISABLE_OPERATION
+        case DriveState::OPERATION_ENABLED:
+            return static_cast<std::uint16_t>(DriveTransition::ENABLE_OPERATION);
+        case DriveState::QUICK_STOP_ACTIVE:
+            return static_cast<std::uint16_t>(DriveTransition::QUICK_STOP);
+        default: // NOT_READY_TO_SWITCH_ON and fault states
+            return 0x0000;
+        }
+    }
+
+    inline word_t updateStateBits(const word_t & stored, const word_t & requested)
+    {
+        static const word_t controlwordMaskNot = ~word_t("0000000010001111"); // state machine-related bits
+        return (stored & controlwordMaskNot) | requested;
+    }
+
     using ds = DriveState;
     using dt = DriveTransition;
 
@@ -116,38 +143,38 @@ namespace
 
 bool DriveStatusMachine::update(std::uint16_t statusword)
 {
-    static const word_t mask("0000000001101111"); // state machine-related bits
-    word_t _old;
-    word_t _new = statusword;
+    static const word_t statuswordMask("0000000001101111"); // state machine-related bits
+
+    std::lock_guard<std::mutex> lock(stateMutex);
+    const word_t old = _statusword;
+    _statusword = statusword;
 
+    if ((old & statuswordMask) != (_statusword & statuswordMask))
     {
-        std::lock_guard<std::mutex> lock(stateMutex);
-        _old = _statusword;
-        _statusword = _new;
+        word_t requested = stateToControlword(parseDriveState(_statusword));
+        _controlword = updateStateBits(_controlword, requested);
+        return stateObserver.notify();
     }
 
-    _old &= mask;
-    _new &= mask;
-    return _old != _new ? stateObserver.notify() : true;
+    return true;
 }
 
 DriveStatusMachine::word_t DriveStatusMachine::controlword() const
 {
-    switch (getCurrentState())
+    std::lock_guard<std::mutex> lock(stateMutex);
+    return _controlword;
+}
+
+bool DriveStatusMachine::controlword(const word_t & controlbits)
+{
+    if (!rpdo->write<std::uint16_t>(controlbits.to_ulong()))
     {
-    case DriveState::SWITCH_ON_DISABLED:
-        return static_cast<std::uint16_t>(DriveTransition::DISABLE_VOLTAGE);
-    case DriveState::READY_TO_SWITCH_ON:
-        return static_cast<std::uint16_t>(DriveTransition::SHUTDOWN);
-    case DriveState::SWITCHED_ON:
-        return static_cast<std::uint16_t>(DriveTransition::SWITCH_ON); // same as DISABLE_OPERATION
-    case DriveState::OPERATION_ENABLED:
-        return static_cast<std::uint16_t>(DriveTransition::ENABLE_OPERATION);
-    case DriveState::QUICK_STOP_ACTIVE:
-        return static_cast<std::uint16_t>(DriveTransition::QUICK_STOP);
-    default: // NOT_READY_TO_SWITCH_ON and fault states
-        return 0;
+        return false;
     }
+
+    std::lock_guard<std::mutex> lock(stateMutex);
+    _controlword = controlbits;
+    return true;
 }
 
 DriveStatusMachine::word_t DriveStatusMachine::statusword() const
@@ -165,9 +192,10 @@ bool DriveStatusMachine::requestTransition(DriveTransition transition, bool wait
 {
     DriveState initialState = getCurrentState();
     auto it = nextStateOnTransition.find({initialState, transition});
+    word_t requested = updateStateBits(controlword(), static_cast<std::uint16_t>(transition));
 
     return it != nextStateOnTransition.cend()
-            && rpdo->write(static_cast<std::uint16_t>(transition))
+            && controlword(requested)
             && (wait ? awaitState(it->second) : true);
 }
 

diff --git a/libraries/CanBusSharerLib/DriveStatusMachine.hpp b/libraries/CanBusSharerLib/DriveStatusMachine.hpp
@@ -53,6 +53,7 @@ class DriveStatusMachine
 
     bool update(std::uint16_t statusword);
     word_t controlword() const;
+    bool controlword(const word_t & controlbits);
     word_t statusword() const;
     DriveState getCurrentState() const;
     bool requestTransition(DriveTransition transition, bool wait = true);

diff --git a/libraries/YarpPlugins/TechnosoftIpos/ICanBusSharerImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/ICanBusSharerImpl.cpp
@@ -104,11 +104,6 @@ bool TechnosoftIpos::initialize()
 
     CD_INFO("Serial number: %c%c%02x%02x.\n", getByte(data, 3), getByte(data, 2), getByte(data, 1), getByte(data, 0));
 
-    if (!can->sdo()->download<std::int16_t>("Quick stop option code", 6, 0x605A))
-    {
-        return false;
-    }
-
     if (!setLimitsRaw(0, vars.min, vars.max))
     {
         CD_ERROR("Unable to set software limits.\n");

diff --git a/libraries/YarpPlugins/TechnosoftIpos/ICurrentControlRawImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/ICurrentControlRawImpl.cpp
@@ -53,8 +53,9 @@ bool TechnosoftIpos::setRefCurrentRaw(int m, double curr)
     CD_DEBUG("(%d)\n", m);
     CHECK_JOINT(m);
     CHECK_MODE(VOCAB_CM_CURRENT);
+
     std::int32_t data = vars.currentToInternalUnits(curr) << 16;
-    return can->sdo()->download("External online reference", data, 0x201C);
+    return quitHaltState(VOCAB_CM_CURRENT) && can->sdo()->download("External online reference", data, 0x201C);
 }
 
 // -----------------------------------------------------------------------------

diff --git a/libraries/YarpPlugins/TechnosoftIpos/IPositionControlRawImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/IPositionControlRawImpl.cpp
@@ -18,7 +18,8 @@ bool TechnosoftIpos::positionMoveRaw(int j, double ref)
     CHECK_JOINT(j);
     CHECK_MODE(VOCAB_CM_POSITION);
 
-    return can->rpdo1()->write<std::uint16_t>(0x002F) // change set immediately
+    return quitHaltState(VOCAB_CM_POSITION)
+            && can->rpdo1()->write<std::uint16_t>(0x002F) // change set immediately
             && can->sdo()->download<std::int32_t>("Target position", vars.degreesToInternalUnits(ref), 0x607A)
             && can->rpdo1()->write<std::uint16_t>(0x003F); // new setpoint (assume absolute target position)
 }
@@ -47,7 +48,8 @@ bool TechnosoftIpos::relativeMoveRaw(int j, double delta)
     CHECK_JOINT(j);
     CHECK_MODE(VOCAB_CM_POSITION);
 
-    return can->rpdo1()->write<std::uint16_t>(0x002F) // change set immediately
+    return quitHaltState(VOCAB_CM_POSITION)
+            && can->rpdo1()->write<std::uint16_t>(0x002F) // change set immediately
             && can->sdo()->download<std::int32_t>("Target position", vars.degreesToInternalUnits(delta), 0x607A)
             && can->rpdo1()->write<std::uint16_t>(0x007F); // new setpoint (assume relative target position)
 }
@@ -249,9 +251,7 @@ bool TechnosoftIpos::stopRaw(int j)
 {
     CD_DEBUG("(%d)\n", j);
     CHECK_JOINT(j);
-
-    return can->driveStatus()->requestTransition(DriveTransition::QUICK_STOP)
-            && can->driveStatus()->requestTransition(DriveTransition::ENABLE_OPERATION);
+    return can->driveStatus()->controlword(can->driveStatus()->controlword().set(8));
 }
 
 // --------------------------------------------------------------------------------

diff --git a/libraries/YarpPlugins/TechnosoftIpos/ITorqueControlRawImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/ITorqueControlRawImpl.cpp
@@ -35,9 +35,10 @@ bool TechnosoftIpos::setRefTorqueRaw(int j, double t)
     CD_DEBUG("(%d, %f)\n", j, t);
     CHECK_JOINT(j);
     CHECK_MODE(VOCAB_CM_TORQUE);
+
     double curr = vars.torqueToCurrent(t);
     std::int32_t data = vars.currentToInternalUnits(curr) << 16;
-    return can->sdo()->download("External online reference", data, 0x201C);
+    return quitHaltState(VOCAB_CM_TORQUE) && can->sdo()->download("External online reference", data, 0x201C);
 }
 
 // -------------------------------------------------------------------------------------

diff --git a/libraries/YarpPlugins/TechnosoftIpos/IVelocityControlRawImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/IVelocityControlRawImpl.cpp
@@ -31,7 +31,7 @@ bool TechnosoftIpos::velocityMoveRaw(int j, double sp)
     CanUtils::encodeFixedPoint(value, &dataInt, &dataFrac);
 
     std::int32_t data = (dataInt << 16) + dataFrac;
-    return can->sdo()->download("Target velocity", data, 0x60FF);
+    return quitHaltState(VOCAB_CM_VELOCITY) && can->sdo()->download("Target velocity", data, 0x60FF);
 }
 
 // ----------------------------------------------------------------------------------

diff --git a/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.cpp b/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.cpp
@@ -16,12 +16,12 @@ using namespace roboticslab;
 namespace
 {
     template<std::size_t N>
-    void reportBitToggle(unsigned int canId, const std::bitset<N> & actual, const std::bitset<N> & stored,
+    bool reportBitToggle(unsigned int canId, const std::bitset<N> & actual, const std::bitset<N> & stored,
             std::size_t pos, const std::string & msgSet, const std::string & msgReset = "")
     {
         if (actual.test(pos) == stored.test(pos))
         {
-            return;
+            return false;
         }
 
         if (actual.test(pos))
@@ -39,6 +39,8 @@ namespace
                 CD_INFO("Bit reset: %s (canId: %d)\n", msgSet.c_str(), canId);
             }
         }
+
+        return true;
     }
 }
 
@@ -470,3 +472,23 @@ void TechnosoftIpos::handleEmcy(EmcyConsumer::code_t code, std::uint8_t reg, con
 }
 
 // -----------------------------------------------------------------------------
+
+bool TechnosoftIpos::quitHaltState(int mode)
+{
+    if (!can->driveStatus()->controlword()[8])
+    {
+        return true;
+    }
+
+    if (mode == VOCAB_CM_POSITION || mode == VOCAB_CM_VELOCITY)
+    {
+        if (!can->driveStatus()->statusword()[10])
+        {
+            return false;
+        }
+    }
+
+    return can->driveStatus()->controlword(can->driveStatus()->controlword().reset(8));
+}
+
+// -----------------------------------------------------------------------------
diff --git a/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.hpp b/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.hpp
@@ -266,6 +266,8 @@ class TechnosoftIpos : public yarp::dev::DeviceDriver,
     void handleTpdo3(std::int32_t position, std::int16_t current);
     void handleEmcy(EmcyConsumer::code_t code, std::uint8_t reg, const std::uint8_t * msef);
 
+    bool quitHaltState(int mode);
+
     CanOpen * can;
 
     yarp::dev::PolyDriver externalEncoderDevice;

diff --git a/tests/testCanBusSharerLib.cpp b/tests/testCanBusSharerLib.cpp
@@ -837,36 +837,62 @@ TEST_F(CanBusSharerTest, DriveStatusMachine)
     // test known states
 
     ASSERT_TRUE(status.update(notReadyToSwitchOn));
+    ASSERT_EQ(status.statusword(), notReadyToSwitchOn);
     ASSERT_EQ(status.getCurrentState(), DriveState::NOT_READY_TO_SWITCH_ON);
     ASSERT_EQ(DriveStatusMachine::parseStatusword(notReadyToSwitchOn), DriveState::NOT_READY_TO_SWITCH_ON);
+    ASSERT_EQ(status.controlword(), 0x0000);
 
     ASSERT_TRUE(status.update(switchOnDisabled));
+    ASSERT_EQ(status.statusword(), switchOnDisabled);
     ASSERT_EQ(status.getCurrentState(), DriveState::SWITCH_ON_DISABLED);
     ASSERT_EQ(DriveStatusMachine::parseStatusword(switchOnDisabled), DriveState::SWITCH_ON_DISABLED);
+    ASSERT_EQ(status.controlword(), static_cast<std::uint16_t>(DriveTransition::DISABLE_VOLTAGE));
 
     ASSERT_TRUE(status.update(readyToSwitchOn));
+    ASSERT_EQ(status.statusword(), readyToSwitchOn);
     ASSERT_EQ(status.getCurrentState(), DriveState::READY_TO_SWITCH_ON);
     ASSERT_EQ(DriveStatusMachine::parseStatusword(readyToSwitchOn), DriveState::READY_TO_SWITCH_ON);
+    ASSERT_EQ(status.controlword(), static_cast<std::uint16_t>(DriveTransition::SHUTDOWN));
 
     ASSERT_TRUE(status.update(switchedOn));
+    ASSERT_EQ(status.statusword(), switchedOn);
     ASSERT_EQ(status.getCurrentState(), DriveState::SWITCHED_ON);
     ASSERT_EQ(DriveStatusMachine::parseStatusword(switchedOn), DriveState::SWITCHED_ON);
+    ASSERT_EQ(status.controlword(), static_cast<std::uint16_t>(DriveTransition::SWITCH_ON));
 
     ASSERT_TRUE(status.update(operationEnabled));
+    ASSERT_EQ(status.statusword(), operationEnabled);
     ASSERT_EQ(status.getCurrentState(), DriveState::OPERATION_ENABLED);
     ASSERT_EQ(DriveStatusMachine::parseStatusword(operationEnabled), DriveState::OPERATION_ENABLED);
+    ASSERT_EQ(status.controlword(), static_cast<std::uint16_t>(DriveTransition::ENABLE_OPERATION));
 
     ASSERT_TRUE(status.update(quickStopActive));
+    ASSERT_EQ(status.statusword(), quickStopActive);
     ASSERT_EQ(status.getCurrentState(), DriveState::QUICK_STOP_ACTIVE);
     ASSERT_EQ(DriveStatusMachine::parseStatusword(quickStopActive), DriveState::QUICK_STOP_ACTIVE);
+    ASSERT_EQ(status.controlword(), static_cast<std::uint16_t>(DriveTransition::QUICK_STOP));
 
     ASSERT_TRUE(status.update(faultReactionActive));
+    ASSERT_EQ(status.statusword(), faultReactionActive);
     ASSERT_EQ(status.getCurrentState(), DriveState::FAULT_REACTION_ACTIVE);
     ASSERT_EQ(DriveStatusMachine::parseStatusword(faultReactionActive), DriveState::FAULT_REACTION_ACTIVE);
+    ASSERT_EQ(status.controlword(), 0x0000);
 
     ASSERT_TRUE(status.update(fault));
+    ASSERT_EQ(status.statusword(), fault);
     ASSERT_EQ(status.getCurrentState(), DriveState::FAULT);
     ASSERT_EQ(DriveStatusMachine::parseStatusword(fault), DriveState::FAULT);
+    ASSERT_EQ(status.controlword(), 0x0000);
+
+    // test random controlword commands
+
+    ASSERT_TRUE(status.controlword(0x1234));
+    ASSERT_EQ(getSender()->getLastMessage().id, rpdo.getCobId());
+    ASSERT_EQ(getSender()->getLastMessage().len, 2);
+    ASSERT_EQ(getSender()->getLastMessage().data, status.controlword().to_ulong());
+    ASSERT_EQ(status.controlword(), 0x1234);
+
+    ASSERT_TRUE(status.controlword(0x0000)); // reset
 
     // test SWITCH_ON_DISABLED -> READY_TO_SWITCH_ON (transition 2: SHUTDOWN)
 
@@ -1016,6 +1042,19 @@ TEST_F(CanBusSharerTest, DriveStatusMachine)
     ASSERT_TRUE(status.update(switchOnDisabled));
     ASSERT_FALSE(status.requestTransition(DriveTransition::ENABLE_OPERATION));
 
+    // test non-null controlword on transition SWITCH_ON_DISABLED -> READY_TO_SWITCH_ON (transition 2: SHUTDOWN)
+
+    ASSERT_FALSE(status.controlword().test(14));
+    ASSERT_TRUE(status.controlword(status.controlword().set(14)));
+    ASSERT_TRUE(status.update(switchOnDisabled));
+    f() = std::async(std::launch::async, observer_timer{MILLIS, [&]{ return status.update(readyToSwitchOn); }});
+    ASSERT_TRUE(status.requestTransition(DriveTransition::SHUTDOWN));
+    ASSERT_EQ(getSender()->getLastMessage().id, rpdo.getCobId());
+    ASSERT_EQ(getSender()->getLastMessage().len, 2);
+    ASSERT_EQ(getSender()->getLastMessage().data, status.controlword().to_ulong());
+    ASSERT_EQ(status.controlword(), 0x4006);
+    ASSERT_EQ(status.getCurrentState(), DriveState::READY_TO_SWITCH_ON);
+
     // test SWITCH_ON_DISABLED -> READY_TO_SWITCH_ON (state request)
 
     ASSERT_TRUE(status.update(switchOnDisabled));