Negotiate handshake on buffer-full signal, fix integrity counter

examples/cpp/examplePositionDirect/examplePositionDirect.cpp

@@ -187,15 +187,16 @@ int main(int argc, char *argv[])
 
     if (batch)
     {
-        bool done;
+        bool done = false;
 
-        while (pos->checkMotionDone(&done) && !done)
+        do
         {
-            CD_INFO_NO_HEADER(". ");
+            CD_INFO_NO_HEADER(".");
             yarp::os::Time::delay(0.5);
         }
+        while (pos->checkMotionDone(&done) && !done);
 
-        CD_INFO("end\n");
+        CD_INFO_NO_HEADER("end\n");
     }
 
     return 0;

libraries/YarpPlugins/CanBusControlboard/IPositionDirectImpl.cpp

@@ -10,7 +10,7 @@ using namespace roboticslab;
 
 bool CanBusControlboard::setPosition(int j, double ref)
 {
-    CD_DEBUG("(%d, %f)\n", j, ref);
+    //CD_DEBUG("(%d, %f)\n", j, ref);
     CHECK_JOINT(j);
     return deviceMapper.mapSingleJoint(&yarp::dev::IPositionDirectRaw::setPositionRaw, j, ref);
 }
@@ -19,15 +19,15 @@ bool CanBusControlboard::setPosition(int j, double ref)
 
 bool CanBusControlboard::setPositions(const double * refs)
 {
-    CD_DEBUG("\n");
+    //CD_DEBUG("\n");
     return deviceMapper.mapAllJoints(&yarp::dev::IPositionDirectRaw::setPositionsRaw, refs);
 }
 
 // -----------------------------------------------------------------------------
 
 bool CanBusControlboard::setPositions(int n_joint, const int * joints, const double * refs)
 {
-    CD_DEBUG("(%d)\n", n_joint);
+    //CD_DEBUG("(%d)\n", n_joint);
     return deviceMapper.mapJointGroup(&yarp::dev::IPositionDirectRaw::setPositionsRaw, n_joint, joints, refs);
 }
 

libraries/YarpPlugins/TechnosoftIpos/IPositionDirectRawImpl.cpp

@@ -10,7 +10,7 @@ using namespace roboticslab;
 
 bool TechnosoftIpos::setPositionRaw(int j, double ref)
 {
-    CD_DEBUG("(%d, %f)\n", j, ref);
+    //CD_DEBUG("(%d, %f)\n", j, ref);
     CHECK_JOINT(j);
     CHECK_MODE(VOCAB_CM_POSITION_DIRECT);
 
@@ -19,7 +19,7 @@ bool TechnosoftIpos::setPositionRaw(int j, double ref)
         linInterpBuffer->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.ipMotionStarted && linInterpBuffer->isMotionReady())
+        if (!vars.ipMotionStarted && !linInterpBuffer->isQueueRead() && linInterpBuffer->isMotionReady())
         {
             bool ok = true;
 
@@ -28,13 +28,7 @@ bool TechnosoftIpos::setPositionRaw(int j, double ref)
                 ok &= can->rpdo3()->write(setpoint); // load point into the buffer
             }
 
-            // enable ip mode
-            if (!ok || !can->driveStatus()->controlword(can->driveStatus()->controlword().set(4)))
-            {
-                return false;
-            }
-
-            vars.ipMotionStarted = true;
+            return ok;
         }
     }
     else
@@ -48,15 +42,15 @@ bool TechnosoftIpos::setPositionRaw(int j, double ref)
 
 bool TechnosoftIpos::setPositionsRaw(const double * refs)
 {
-    CD_DEBUG("\n");
+    //CD_DEBUG("\n");
     return setPositionRaw(0, refs[0]);
 }
 
 // -----------------------------------------------------------------------------
 
 bool TechnosoftIpos::setPositionsRaw(int n_joint, const int * joints, const double * refs)
 {
-    CD_DEBUG("\n");
+    //CD_DEBUG("\n");
     return setPositionRaw(joints[0], refs[0]);
 }
 

libraries/YarpPlugins/TechnosoftIpos/LinearInterpolationBuffer.cpp

@@ -25,7 +25,8 @@ LinearInterpolationBuffer::LinearInterpolationBuffer(const StateVariables & _var
     : vars(_vars),
       periodMs(_periodMs),
       integrityCounter(0),
-      initialTarget(0.0)
+      initialTarget(0.0),
+      queueRead(false)
 { }
 
 void LinearInterpolationBuffer::init(double _initialTarget)
@@ -34,6 +35,7 @@ void LinearInterpolationBuffer::init(double _initialTarget)
     integrityCounter = 0;
     initialTarget = _initialTarget;
     pendingTargets.clear();
+    queueRead = false;
 }
 
 std::uint16_t LinearInterpolationBuffer::getPeriodMs() const
@@ -43,7 +45,7 @@ std::uint16_t LinearInterpolationBuffer::getPeriodMs() const
 
 std::uint16_t LinearInterpolationBuffer::getBufferConfig() const
 {
-    std::bitset<16> bits("1010000010001000"); // 0xA088
+    std::bitset<16> bits("1010000010000000"); // 0xA080
     bits |= (BUFFER_LOW << 8);
     bits |= ((integrityCounter << 1) >> 1);
     return bits.to_ulong();
@@ -89,6 +91,7 @@ std::vector<std::uint64_t> LinearInterpolationBuffer::popBatch(bool fullBuffer)
                 return setpoint;
             });
 
+    queueRead |= !batch.empty();
     return batch;
 }
 
@@ -106,6 +109,12 @@ bool LinearInterpolationBuffer::isMotionDone() const
     return pendingTargets.empty() || pendingTargets.size() <= offset;
 }
 
+bool LinearInterpolationBuffer::isQueueRead() const
+{
+    std::lock_guard<std::mutex> lock(queueMutex);
+    return queueRead;
+}
+
 std::uint8_t LinearInterpolationBuffer::getIntegrityCounter() const
 {
     return integrityCounter;
@@ -160,7 +169,7 @@ std::uint64_t PvtBuffer::makeDataRecord(double previous, double current, double
     data += positionLSB;
     data += static_cast<std::uint64_t>(positionMSB) << 24;
 
-    if (std::abs(next - current) < 1e-6)
+    if (std::abs(next - current) > 1e-6)
     {
         double prevVelocity = (current - previous) / (getPeriodMs() * 0.001);
         double nextVelocity = (next - current) / (getPeriodMs() * 0.001);
@@ -181,6 +190,9 @@ std::uint64_t PvtBuffer::makeDataRecord(double previous, double current, double
     return data;
 }
 
+namespace roboticslab
+{
+
 LinearInterpolationBuffer * createInterpolationBuffer(const yarp::os::Searchable & config, const StateVariables & vars)
 {
     unsigned int periodMs = config.check("periodMs", yarp::os::Value(0), "pt/pvt time (ms)").asInt32();
@@ -206,3 +218,5 @@ LinearInterpolationBuffer * createInterpolationBuffer(const yarp::os::Searchable
         return nullptr;
     }
 }
+
+} // namespace roboticslab

libraries/YarpPlugins/TechnosoftIpos/LinearInterpolationBuffer.hpp

@@ -63,6 +63,9 @@ class LinearInterpolationBuffer
     //! Report whether motion can be finished (no more points in the queue).
     bool isMotionDone() const;
 
+    //! Report whether the internal queue has been processed (read from) at least once.
+    bool isQueueRead() const;
+
 protected:
     //! Retrieve current integrity counter value.
     std::uint8_t getIntegrityCounter() const;
@@ -80,6 +83,7 @@ class LinearInterpolationBuffer
     std::uint16_t periodMs;
     std::uint8_t integrityCounter;
     double initialTarget;
+    bool queueRead;
 
     std::deque<double> pendingTargets;
     mutable std::mutex queueMutex;

libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.cpp

@@ -396,7 +396,14 @@ void TechnosoftIpos::interpretPtStatus(std::uint16_t status)
     reportBitToggle(report, WARN, 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.");
-    reportBitToggle(report, NONE, 13, "Buffer is full.", "Buffer is not full.");
+
+    if (reportBitToggle(report, NONE, 13, "Buffer is full.", "Buffer is not full.") && linInterpBuffer
+            && !vars.ipMotionStarted
+            && linInterpBuffer->isMotionReady())
+    {
+        // buffer full, ready to enable ip mode
+        vars.ipMotionStarted = can->driveStatus()->controlword(can->driveStatus()->controlword().set(4));
+    }
 
     if (reportBitToggle(report, NONE, 14, "Buffer is low.", "Buffer is not low.") && linInterpBuffer
             && vars.ipMotionStarted