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

Merge branch 'master' of https://github.com/lopsided98/libcreate into lopsided98-master

Browse source 
Conflicts:
	src/create.cpp
This commit is contained in:
Jacob Perron 2016-06-28 12:49:06 -07:00
commit ff0fce11f4
4 changed files with 64 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

45
src/create.cpp
View file

@ -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;
@ -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)
@ -149,6 +151,9 @@ namespace create {
}
} // if CREATE_2
totalLeftDist += leftWheelDist;
totalRightDist += rightWheelDist;
if (fabs(dt) > util::EPS) {
vel.x = deltaDist / dt;
vel.y = 0.0;
@ -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);

4
src/data.cpp
View file

@ -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");
@ -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");