Refactor halt bit check, test current statusword, drop event listener

Statusword bit 10 switches too fast in case a default inhibit time (30 milliseconds) is used. It is best to check against current statusword whenever this bit is actually used instead of listening to a status change event.
roboticslab-uc3m · Dec 28, 2019 · a460407 · a460407
1 parent 41a8050
commit a460407
Show file tree

Hide file tree

Showing 6 changed files with 28 additions and 38 deletions.
diff --git a/libraries/YarpPlugins/TechnosoftIpos/ICurrentControlRawImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/ICurrentControlRawImpl.cpp
@@ -54,17 +54,8 @@ bool TechnosoftIpos::setRefCurrentRaw(int m, double curr)
     CHECK_JOINT(m);
     CHECK_MODE(VOCAB_CM_CURRENT);
 
-    if (can->driveStatus()->controlword()[8])
-    {
-        if (!can->driveStatus()->controlword(can->driveStatus()->controlword().reset(8)))
-        {
-            CD_ERROR("Unable to reset halt bit.\n");
-            return false;
-        }
-    }
-
     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,7 @@ bool TechnosoftIpos::positionMoveRaw(int j, double ref)
     CHECK_JOINT(j);
     CHECK_MODE(VOCAB_CM_POSITION);
 
-    return !can->driveStatus()->controlword()[8] // not halting
+    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)
@@ -48,7 +48,7 @@ bool TechnosoftIpos::relativeMoveRaw(int j, double delta)
     CHECK_JOINT(j);
     CHECK_MODE(VOCAB_CM_POSITION);
 
-    return !can->driveStatus()->controlword()[8] // not halting
+    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)

diff --git a/libraries/YarpPlugins/TechnosoftIpos/ITorqueControlRawImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/ITorqueControlRawImpl.cpp
@@ -36,18 +36,9 @@ bool TechnosoftIpos::setRefTorqueRaw(int j, double t)
     CHECK_JOINT(j);
     CHECK_MODE(VOCAB_CM_TORQUE);
 
-    if (can->driveStatus()->controlword()[8])
-    {
-        if (!can->driveStatus()->controlword(can->driveStatus()->controlword().reset(8)))
-        {
-            CD_ERROR("Unable to reset halt bit.\n");
-            return false;
-        }
-    }
-
     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
@@ -18,12 +18,6 @@ bool TechnosoftIpos::velocityMoveRaw(int j, double sp)
     CHECK_JOINT(j);
     CHECK_MODE(VOCAB_CM_VELOCITY);
 
-    if (can->driveStatus()->controlword()[8])
-    {
-        CD_WARNING("Currently halting.\n");
-        return false;
-    }
-
     if (std::abs(sp) > vars.maxVel)
     {
         CD_WARNING("Requested speed exceeds maximum velocity (%f).\n", vars.maxVel);
@@ -37,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
@@ -264,15 +264,7 @@ void TechnosoftIpos::interpretStatusword(std::uint16_t statusword)
     reportBitToggle(id, bits, stored, 7, "Warning. A TML function / homing was called, while another TML function / homing is still in execution. The last call is ignored.", "No warning.");
     reportBitToggle(id, bits, stored, 8, "A TML function or homing is executed. Until the function or homing execution ends or is aborted, no other TML function / homing may be called.", "No TML function or homing is executed. The execution of the last called TML function or homing is completed.");
     reportBitToggle(id, bits, stored, 9, "Remote - drive parameters may be modified via CAN and the drive will execute the command message.", "Remote – drive is in local mode and will not execute the command message.");
-
-    if (reportBitToggle(id, bits, stored, 10, "Target reached.") && can->driveStatus()->controlword()[8])
-    {
-        if (!can->driveStatus()->controlword(can->driveStatus()->controlword().reset(8)))
-        {
-            CD_WARNING("Unable to reset halt bit in controlword.\n");
-        }
-    }
-
+    reportBitToggle(id, bits, stored, 10, "Target reached.");
     reportBitToggle(id, bits, stored, 11, "Internal Limit Active.");
 
     switch (vars.actualControlMode.load())
@@ -480,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;