Skip to content

Expose individual wheel distances and velocities. #19

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
jacobperron merged 1 commit into from
Jun 28, 2016
Merged

Expose individual wheel distances and velocities. #19

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 18 additions & 2 deletions include/create/create.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,8 +73,8 @@ namespace create {

uint32_t prevTicksLeft;
uint32_t prevTicksRight;
float prevLeftVel;
float prevRightVel;
float totalLeftDist;
float totalRightDist;
bool firstOnData;
util::timestamp_t prevOnDataTime;

Expand Down Expand Up @@ -444,6 +444,22 @@ namespace create {
*/
bool isMovingForward() const;

/* Get the total distance (in meters) the left wheel has moved.
*/
float getLeftWheelDistance() const;

/* Get the total distance (in meters) the right wheel has moved.
*/
float getRightWheelDistance() const;

/* Get the requested velocity (in meters/sec) of the left wheel.
*/
float getRequestedLeftWheelVel() const;

/* Get the requested velocity (in meters/sec) of the right wheel.
*/
float getRequestedRightWheelVel() const;

/* Get the current charging state.
*/
create::ChargingState getChargingState() const;
Expand Down
2 changes: 2 additions & 0 deletions include/create/util.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,8 @@ POSSIBILITY OF SUCH DAMAGE.
namespace create {
namespace util {

static const float CREATE_1_AXLE_LENGTH = 0.26;

static const uint8_t CREATE_2_HEADER = 19;
static const float CREATE_2_AXLE_LENGTH = 0.235;
static const float CREATE_2_TICKS_PER_REV = 508.8;
Expand Down
45 changes: 42 additions & 3 deletions src/create.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,8 @@ namespace create {
powerLEDIntensity = 0;
prevTicksLeft = 0;
prevTicksRight = 0;
totalLeftDist = 0.0;
totalRightDist = 0.0;
firstOnData = true;
pose.x = 0;
pose.y = 0;
Expand Down Expand Up @@ -104,9 +106,9 @@ namespace create {
deltaYaw = ((int16_t) GET_DATA(ID_ANGLE)) * (util::PI / 180.0); // D2R
deltaX = deltaDist * cos( util::normalizeAngle(pose.yaw + deltaYaw) );
deltaY = -deltaDist * sin( util::normalizeAngle(pose.yaw + deltaYaw) );
leftWheelDist = deltaDist / 2.0;
rightWheelDist = leftWheelDist;

const float deltaYawWheelDist = (util::CREATE_1_AXLE_LENGTH / 2.0) * deltaYaw;
leftWheelDist = deltaDist - deltaYawWheelDist;
rightWheelDist = deltaDist + deltaYawWheelDist;
}
else if (model == CREATE_2) {
// Get cumulative ticks (wraps around at 65535)
Expand Down Expand Up @@ -149,6 +151,9 @@ namespace create {
}
} // if CREATE_2

totalLeftDist += leftWheelDist;
totalRightDist += rightWheelDist;
Copy link
Member

@jacobperron jacobperron Jun 25, 2016 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It may not be a serious issue, but there is potential for a wraparound here if someone is running the robot for the long term resulting in a negative distance.

You can disregard this issue for now, since the same problem exists in the global pose of the robot.

Copy link
Contributor Author

@lopsided98 lopsided98 Jun 25, 2016 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Since these are floats, the value wouldn't wrap around, it would just become +/- infinity. The bigger issue here is loss of precision. I did some quick tests and it looks like the precision (theoretically, I don't know how precise the measurements actually are) would get to about 1 mm after the robot travelled around 10 km. It wouldn't actually overflow unless it drove beyond the edge of the galaxy.


Copy link
Member

@jacobperron jacobperron Jun 24, 2016

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You can compute the individual wheel velocities here.

if (fabs(dt) > util::EPS) {
vel.x = deltaDist / dt;
vel.y = 0.0;
Expand Down Expand Up @@ -863,6 +868,40 @@ namespace create {
}
}

float Create::getLeftWheelDistance() const {
return totalLeftDist;
}

float Create::getRightWheelDistance() const {
return totalRightDist;
}

float Create::getRequestedLeftWheelVel() const {
if (data->isValidPacketID(ID_LEFT_VEL)) {
uint16_t uvel = GET_DATA(ID_LEFT_VEL);
int16_t vel;
std::memcpy(&vel, &uvel, sizeof(vel));
return (vel / 1000.0);
}
else {
CERR("[create::Create] ", "Left wheel velocity not supported!");
return 0;
}
}

float Create::getRequestedRightWheelVel() const {
if (data->isValidPacketID(ID_RIGHT_VEL)) {
uint16_t uvel = GET_DATA(ID_RIGHT_VEL);
int16_t vel;
std::memcpy(&vel, &uvel, sizeof(vel));
return (vel / 1000.0);
}
else {
CERR("[create::Create] ", "Right wheel velocity not supported!");
return 0;
}
}

create::CreateMode Create::getMode() const {
if (data->isValidPacketID(ID_OI_MODE)) {
return (create::CreateMode) GET_DATA(ID_OI_MODE);
Expand Down
4 changes: 2 additions & 2 deletions src/data.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,8 @@ namespace create {
ADD_PACKET(ID_CAPACITY, 2, "battery_capacity");
ADD_PACKET(ID_VIRTUAL_WALL, 1, "virtual_wall");
ADD_PACKET(ID_OI_MODE, 1, "oi_mode");
ADD_PACKET(ID_RIGHT_VEL, 2, "velocity_right");
ADD_PACKET(ID_LEFT_VEL, 2, "velocity_left");

if (model == CREATE_1) {
ADD_PACKET(ID_DISTANCE, 2, "distance");
Expand All @@ -48,8 +50,6 @@ namespace create {
//ADD_PACKET(ID_NUM_STREAM_PACKETS, 1, "oi_stream_num_packets");
//ADD_PACKET(ID_VEL, 2, "velocity");
//ADD_PACKET(ID_RADIUS, 2, "radius");
//ADD_PACKET(ID_RIGHT_VEL, 2, "velocity_right");
//ADD_PACKET(ID_LEFT_VEL, 2, "velocity_left");
ADD_PACKET(ID_LEFT_ENC, 2, "enc_counts_left");
ADD_PACKET(ID_RIGHT_ENC, 2, "enc_counts_right");
ADD_PACKET(ID_LIGHT, 1, "light_bumper");
Expand Down