From 7ce9227d1cff55b3468a383a08cc5c815e3a0f9e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Bartek=20=C5=81ukawski?= <bwmn.peter@gmail.com>
Date: Fri, 13 Jan 2023 13:16:11 +0100
Subject: [PATCH] Reset stored velocity reference on idle

---
 .../TechnosoftIpos/external-pid/IControlModeRawImpl.cpp         | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/libraries/YarpPlugins/TechnosoftIpos/external-pid/IControlModeRawImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/external-pid/IControlModeRawImpl.cpp
index 17fd0c456..8a66382dd 100644
--- a/libraries/YarpPlugins/TechnosoftIpos/external-pid/IControlModeRawImpl.cpp
+++ b/libraries/YarpPlugins/TechnosoftIpos/external-pid/IControlModeRawImpl.cpp
@@ -55,6 +55,8 @@ bool TechnosoftIposExternal::setControlModeRaw(int j, int mode)
         }
         // no break
     case VOCAB_CM_IDLE:
+        trapTrajectory.reset(internalUnitsToDegrees(lastEncoderRead->queryPosition())); // reset stored velocity reference
+
         return can->driveStatus()->requestState(DriveState::SWITCHED_ON)
             && can->sdo()->download<std::int8_t>("Modes of Operation", 0, 0x6060) // reset drive mode
             && can->driveStatus()->controlword(can->driveStatus()->controlword().reset(4)); // disable ext. ref. torque mode