Merge pull request #346 from yoshito-okada/fix-xl320-vel2value

bugfix in velocity-to-value conversion for XL320
ROBOTIS-GIT · Jan 25, 2022 · 50b3c57 · 50b3c57
2 parents f25532b + 5d5180f
commit 50b3c57
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
@@ -1303,8 +1303,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;
+      // CCW rotation to [0, 1023]
+      else if (velocity < 0.0f) value = (-velocity / (model_info->rpm * RPM2RADPERSEC));
+      // CW rotation to [1024, 2047]
+      else if (velocity > 0.0f) value = (velocity / (model_info->rpm * RPM2RADPERSEC)) + 1024;
 
       return value;
     }