compAIR/libcreate
compAIR/libcreate
4
0
Fork 1
Code Issues Pull requests Projects Releases Packages 1 Wiki Activity Actions

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 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)