From 94928382d8021753932b849b0eeb9f4cd53001bd Mon Sep 17 00:00:00 2001 From: ted-miller Date: Thu, 5 Oct 2023 16:40:15 -0400 Subject: [PATCH] Fix unit scaling --- src/MathConstants.h | 3 +++ src/PositionMonitor.c | 14 ++++++-------- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/MathConstants.h b/src/MathConstants.h index 58ed6bf9..62f5e76f 100644 --- a/src/MathConstants.h +++ b/src/MathConstants.h @@ -17,6 +17,9 @@ // macro #define MICROMETERS_TO_METERS(x) (x * 0.000001) +#define METERS_TO_MICROMETERS(x) (x * 1000000) +#define RAD_TO_DEG_0001(x) (x * DEGREES_PER_RAD * 10000) +#define DEG_0001_TO_RAD(x) ((x / 10000) * RAD_PER_DEGREE) // used for comparing floating point values #define EPSILON_TOLERANCE_DOUBLE (0.001) diff --git a/src/PositionMonitor.c b/src/PositionMonitor.c index e13cad2a..c6b0c274 100644 --- a/src/PositionMonitor.c +++ b/src/PositionMonitor.c @@ -295,16 +295,14 @@ void Ros_PositionMonitor_CalculateTransforms(int groupIndex, long* pulsePos_moto { switch (group->baseTrackInfo.motionType[i]) { - case MOTION_TYPE_X: coordTrackTravel.x = track_pos_meters[i]; break; - case MOTION_TYPE_Y: coordTrackTravel.y = track_pos_meters[i]; break; - case MOTION_TYPE_Z: coordTrackTravel.z = track_pos_meters[i]; break; - case MOTION_TYPE_RX: coordTrackTravel.rx = track_pos_meters[i]; break; - case MOTION_TYPE_RY: coordTrackTravel.ry = track_pos_meters[i]; break; - case MOTION_TYPE_RZ: coordTrackTravel.rz = track_pos_meters[i]; break; + case MOTION_TYPE_X: coordTrackTravel.x = METERS_TO_MICROMETERS(track_pos_meters[i]); break; + case MOTION_TYPE_Y: coordTrackTravel.y = METERS_TO_MICROMETERS(track_pos_meters[i]); break; + case MOTION_TYPE_Z: coordTrackTravel.z = METERS_TO_MICROMETERS(track_pos_meters[i]); break; + case MOTION_TYPE_RX: coordTrackTravel.rx = RAD_TO_DEG_0001(track_pos_meters[i]); break; + case MOTION_TYPE_RY: coordTrackTravel.ry = RAD_TO_DEG_0001(track_pos_meters[i]); break; + case MOTION_TYPE_RZ: coordTrackTravel.rz = RAD_TO_DEG_0001(track_pos_meters[i]); break; } } - Ros_Debug_BroadcastMsg("coordTrackTravel.x = %.3f", coordTrackTravel.x); - Ros_Debug_BroadcastMsg("coordTrackTravel.y = %.3f", coordTrackTravel.y); mpZYXeulerToFrame(&coordWorldToBase, &frameWorldToTrack); mpZYXeulerToFrame(&coordTrackTravel, &frameTrackTravel); mpZYXeulerToFrame(&group->baseTrackInfo.offsetFromBaseToRobotOrigin, &frameTrackToRobot);