jakob.kampichler/libcreate
jakob.kampichler/libcreate
1
0
Fork 0
forked from compAIR/libcreate
Code Pull requests Activity

Fix odometry for Create 1

Browse source
This commit is contained in:
jacobperron 2016-03-31 22:40:42 -07:00
parent 055cf859bf
commit 1b6d36152c
1 changed files with 3 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

6
src/create.cpp
View file

@ -66,10 +66,10 @@ namespace create {
float dt = (curTime - prevOnDataTime) / 1000000.0; float dt = (curTime - prevOnDataTime) / 1000000.0;
float deltaDist, deltaX, deltaY, deltaYaw; float deltaDist, deltaX, deltaY, deltaYaw;
if (model == CREATE_1) { 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 deltaYaw = ((int16_t) GET_DATA(ID_ANGLE)) * (util::PI / 180.0); // D2R
deltaX = deltaDist * cos(pose.yaw); deltaX = deltaDist * cos( util::normalizeAngle(pose.yaw + deltaYaw) );
deltaY = deltaDist * sin(pose.yaw); deltaY = deltaDist * sin( util::normalizeAngle(pose.yaw + deltaYaw) );
} }
else if (model == CREATE_2) { else if (model == CREATE_2) {
// Get cumulative ticks (wraps around at 65535) // Get cumulative ticks (wraps around at 65535)