Ignore subsequent posd commands on buffer error

roboticslab-uc3m · Mar 26, 2021 · 99b7f41 · 99b7f41
1 parent 0e38541
commit 99b7f41
Show file tree

Hide file tree

Showing 5 changed files with 16 additions and 8 deletions.
diff --git a/libraries/YarpPlugins/TechnosoftIpos/IControlModeRawImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/IControlModeRawImpl.cpp
@@ -114,6 +114,7 @@ bool TechnosoftIpos::setControlModeRaw(int j, int mode)
         if (ipBuffer)
         {
             vars.ipBufferFilled = vars.ipMotionStarted = false;
+            vars.ipBufferEnabled = true;
             ipBuffer->clearQueue();
 
             PdoConfiguration rpdo3Conf;

diff --git a/libraries/YarpPlugins/TechnosoftIpos/IPositionDirectRawImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/IPositionDirectRawImpl.cpp
@@ -18,8 +18,8 @@ bool TechnosoftIpos::setPositionRaw(int j, double ref)
     {
         ipBuffer->addSetpoint(ref); // register point in the internal queue
 
-        // drive's buffer is empty, motion has not started yet, we have enough points in the queue
-        if (!vars.ipBufferFilled && !vars.ipMotionStarted && ipBuffer->isQueueReady())
+        // ip mode is enabled, drive's buffer is empty, motion has not started yet, we have enough points in the queue
+        if (vars.ipBufferEnabled && !vars.ipBufferFilled && !vars.ipMotionStarted && ipBuffer->isQueueReady())
         {
             std::int32_t refInternal = vars.lastEncoderRead->queryPosition();
             ipBuffer->setInitial(vars.internalUnitsToDegrees(refInternal));

diff --git a/libraries/YarpPlugins/TechnosoftIpos/StateVariables.cpp b/libraries/YarpPlugins/TechnosoftIpos/StateVariables.cpp
@@ -251,7 +251,7 @@ void StateVariables::reset()
     synchronousCommandTarget = prevSyncTarget = 0.0;
     enableSync = false;
 
-    ipBufferFilled = ipMotionStarted = false;
+    ipBufferFilled = ipMotionStarted = ipBufferEnabled = false;
 }
 
 // -----------------------------------------------------------------------------

diff --git a/libraries/YarpPlugins/TechnosoftIpos/StateVariables.hpp b/libraries/YarpPlugins/TechnosoftIpos/StateVariables.hpp
@@ -140,6 +140,7 @@ struct StateVariables
 
     std::atomic<bool> ipMotionStarted {false};
     std::atomic<bool> ipBufferFilled {false};
+    std::atomic<bool> ipBufferEnabled {false};
 
     // read only, conceptually immutable
 

diff --git a/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.cpp b/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.cpp
@@ -393,21 +393,27 @@ void TechnosoftIpos::interpretIpStatus(std::uint16_t status)
     std::uint8_t ic = status & 0x007F; // integrity counter
 
     // 7-10: reserved
-    reportBitToggle(report, WARN, 11, "Drive has performed a quick stop after a buffer empty condition (last velocity was non-zero).",
+    reportBitToggle(report, INFO, 11, "Drive has performed a quick stop after a buffer empty condition (last velocity was non-zero).",
             "Drive has maintained interpolated position mode after a buffer empty condition.");
-    reportBitToggle(report, WARN, 12, "Integrity counter error.", "No integrity counter error.");
 
+    auto isBufferError = reportBitToggle(report, WARN, 12, "Integrity counter error.", "No integrity counter error.");
     auto isBufferFull = reportBitToggle(report, NONE, 13, "Buffer is full.", "Buffer is not full.");
     auto isBufferLow = reportBitToggle(report, NONE, 14, "Buffer is low.", "Buffer is not low."); // also true if empty!
     auto isBufferEmpty = reportBitToggle(report, INFO, 15, "Buffer is empty.", "Buffer is not empty.");
 
-    if (isBufferFull && ipBuffer && !vars.ipMotionStarted)
+    if (isBufferError && vars.ipBufferEnabled)
+    {
+        can->driveStatus()->controlword(can->driveStatus()->controlword().set(8)); // stop drive with profile acceleration
+        vars.ipBufferEnabled = false;
+    }
+
+    if (isBufferFull && vars.ipBufferEnabled && !vars.ipMotionStarted)
     {
         // enable ip mode
         vars.ipMotionStarted = can->driveStatus()->controlword(can->driveStatus()->controlword().set(4));
     }
 
-    if (isBufferLow && ipBuffer && vars.ipMotionStarted && !ipBuffer->isQueueEmpty() && !isBufferEmpty)
+    if (isBufferLow && vars.ipBufferEnabled && vars.ipMotionStarted && !ipBuffer->isQueueEmpty() && !isBufferEmpty)
     {
         // load next batch of points into the drive's buffer (unless reported empty, in which case stop motion and replenish again)
         for (auto setpoint : ipBuffer->popBatch(false))
@@ -416,7 +422,7 @@ void TechnosoftIpos::interpretIpStatus(std::uint16_t status)
         }
     }
 
-    if (isBufferEmpty && ipBuffer && vars.ipMotionStarted)
+    if (isBufferEmpty && vars.ipBufferEnabled && vars.ipMotionStarted)
     {
         // no elements in the queue and buffer is empty, disable ip mode
         vars.ipMotionStarted = !can->driveStatus()->controlword(can->driveStatus()->controlword().reset(4));