From 1b6d36152c9dee681c6a2b4c109b74909b48c1e1 Mon Sep 17 00:00:00 2001 From: jacobperron Date: Thu, 31 Mar 2016 22:40:42 -0700 Subject: [PATCH] Fix odometry for Create 1 --- src/create.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/create.cpp b/src/create.cpp index e77a560..b0cc473 100644 --- a/src/create.cpp +++ b/src/create.cpp @@ -66,10 +66,10 @@ namespace create { float dt = (curTime - prevOnDataTime) / 1000000.0; float deltaDist, deltaX, deltaY, deltaYaw; if (model == CREATE_1) { - deltaDist = GET_DATA(ID_DISTANCE) / 1000.0; //mm -> m + deltaDist = ((int16_t) GET_DATA(ID_DISTANCE)) / 1000.0; //mm -> m deltaYaw = ((int16_t) GET_DATA(ID_ANGLE)) * (util::PI / 180.0); // D2R - deltaX = deltaDist * cos(pose.yaw); - deltaY = deltaDist * sin(pose.yaw); + deltaX = deltaDist * cos( util::normalizeAngle(pose.yaw + deltaYaw) ); + deltaY = deltaDist * sin( util::normalizeAngle(pose.yaw + deltaYaw) ); } else if (model == CREATE_2) { // Get cumulative ticks (wraps around at 65535)