From 8ec052ff7325a0a6f5648a1195db9104238841b3 Mon Sep 17 00:00:00 2001
From: Yoshito Okada <okada@rm.is.tohoku.ac.jp>
Date: Fri, 12 Nov 2021 07:45:42 +0000
Subject: [PATCH] bugfix in velocity-to-value conversion for XL320

---
 .../src/dynamixel_workbench_toolbox/dynamixel_workbench.cpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/dynamixel_workbench_toolbox/src/dynamixel_workbench_toolbox/dynamixel_workbench.cpp b/dynamixel_workbench_toolbox/src/dynamixel_workbench_toolbox/dynamixel_workbench.cpp
index 500035da..8e784ab7 100644
--- a/dynamixel_workbench_toolbox/src/dynamixel_workbench_toolbox/dynamixel_workbench.cpp
+++ b/dynamixel_workbench_toolbox/src/dynamixel_workbench_toolbox/dynamixel_workbench.cpp
@@ -1277,8 +1277,10 @@ int32_t DynamixelWorkbench::convertVelocity2Value(uint8_t id, float velocity)
     if (strcmp(getModelName(id), "XL-320") == 0)
     {
       if (velocity == 0.0f) value = 0;
-      else if (velocity < 0.0f) value = (velocity / (model_info->rpm * RPM2RADPERSEC));
-      else if (velocity > 0.0f) value = (velocity / (model_info->rpm * RPM2RADPERSEC)) + 1023;
+      // CW rotation to [0, 1023]
+      else if (velocity < 0.0f) value = (-velocity / (model_info->rpm * RPM2RADPERSEC));
+      // CCW rotation to [1024, 2047]
+      else if (velocity > 0.0f) value = (velocity / (model_info->rpm * RPM2RADPERSEC)) + 1024;
 
       return value;
     }