From 1deb13d433bee3ad07b78eddcff89ff9e11d7651 Mon Sep 17 00:00:00 2001 From: vincenttumminello Date: Wed, 8 Jul 2026 10:31:09 +0900 Subject: [PATCH 1/2] Change joint velocity to convert to rad/s instead of rev/s --- NUSense/Core/Src/nusense/Convert.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/NUSense/Core/Src/nusense/Convert.cpp b/NUSense/Core/Src/nusense/Convert.cpp index 2a857d1..9b0d96a 100644 --- a/NUSense/Core/Src/nusense/Convert.cpp +++ b/NUSense/Core/Src/nusense/Convert.cpp @@ -144,7 +144,7 @@ namespace nusense { // Range: -210 - +210 = -48.09 rpm - +48.09 rpm // Default servo limits for velocity - minimum of all for X-Series, MX-106 and MX-64 // X-Series has the minimum at 167 - return utility::math::clamp(int32_t(-167), velocity, int32_t(167)) * 0.229f / 60.0f; + return utility::math::clamp(int32_t(-167), velocity, int32_t(167)) * 0.229f * 2 * M_PI / 60.0f; } int32_t velocity(float velocity) { @@ -152,7 +152,7 @@ namespace nusense { // Range: -210 - +210 = -48.09 rpm - +48.09 rpm // Default servo limits for velocity - minimum of all for X-Series, MX-106 and MX-64 // X-Series has the minimum at 167 - return int32_t(utility::math::clamp(-167.0f, (velocity * 60.0f / 0.229f), 167.0f)); + return int32_t(utility::math::clamp(-167.0f, (velocity * 60.0f / (0.229f * 2 * M_PI)), 167.0f)); } uint32_t profile_velocity(float profile_velocity) { From 41b706f3f08a7fd055b736450935b65389930393 Mon Sep 17 00:00:00 2001 From: vincenttumminello Date: Wed, 8 Jul 2026 15:48:00 +0900 Subject: [PATCH 2/2] Use float type PI instead of double type in clamp function --- NUSense/Core/Src/nusense/Convert.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/NUSense/Core/Src/nusense/Convert.cpp b/NUSense/Core/Src/nusense/Convert.cpp index 9b0d96a..2f42470 100644 --- a/NUSense/Core/Src/nusense/Convert.cpp +++ b/NUSense/Core/Src/nusense/Convert.cpp @@ -144,7 +144,7 @@ namespace nusense { // Range: -210 - +210 = -48.09 rpm - +48.09 rpm // Default servo limits for velocity - minimum of all for X-Series, MX-106 and MX-64 // X-Series has the minimum at 167 - return utility::math::clamp(int32_t(-167), velocity, int32_t(167)) * 0.229f * 2 * M_PI / 60.0f; + return utility::math::clamp(int32_t(-167), velocity, int32_t(167)) * 0.229f * 2 * static_cast(M_PI) / 60.0f; } int32_t velocity(float velocity) { @@ -152,7 +152,7 @@ namespace nusense { // Range: -210 - +210 = -48.09 rpm - +48.09 rpm // Default servo limits for velocity - minimum of all for X-Series, MX-106 and MX-64 // X-Series has the minimum at 167 - return int32_t(utility::math::clamp(-167.0f, (velocity * 60.0f / (0.229f * 2 * M_PI)), 167.0f)); + return int32_t(utility::math::clamp(-167.0f, (velocity * 60.0f / (0.229f * 2 * static_cast(M_PI))), 167.0f)); } uint32_t profile_velocity(float profile_velocity) {