bugfix in velocity-to-value conversion for XL320

ROBOTIS-GIT · Nov 12, 2021 · 8ec052f · 8ec052f
1 parent 5013826
commit 8ec052f
Showing 1 changed file with 4 additions and 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
@@ -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;
     }