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; }