Add support for early model Roomba 400s and other robots using the original SCI protocol.
This commit is contained in:
Ben Wolsieffer
parent
c68a308c71
commit
618956e14c
18 changed files with 675 additions and 307 deletions
261
src/create.cpp
261
src/create.cpp
|
|
@ -8,7 +8,8 @@
|
|||
#include "create/create.h"
|
||||
|
||||
#define GET_DATA(id) (data->getPacket(id)->getData())
|
||||
#define BOUND(val,min,max) (val<min?val=min:(val>max?val=max:val=val))
|
||||
#define BOUND_CONST(val,min,max) (val<min?min:(val>max?max:val))
|
||||
#define BOUND(val,min,max) (val = BOUND_CONST(val,min,max))
|
||||
|
||||
namespace create {
|
||||
|
||||
|
|
@ -31,6 +32,7 @@ namespace create {
|
|||
totalLeftDist = 0.0;
|
||||
totalRightDist = 0.0;
|
||||
firstOnData = true;
|
||||
mode = MODE_OFF;
|
||||
pose.x = 0;
|
||||
pose.y = 0;
|
||||
pose.yaw = 0;
|
||||
|
|
@ -40,8 +42,14 @@ namespace create {
|
|||
vel.yaw = 0;
|
||||
vel.covariance = std::vector<float>(9, 0.0);
|
||||
poseCovar = Matrix(3, 3, 0.0);
|
||||
data = boost::shared_ptr<Data>(new Data(model));
|
||||
serial = boost::make_shared<Serial>(data);
|
||||
requestedLeftVel = 0;
|
||||
requestedRightVel = 0;
|
||||
data = boost::shared_ptr<Data>(new Data(model.getVersion()));
|
||||
if (model.getVersion() == V_1) {
|
||||
serial = boost::make_shared<SerialQuery>(data);
|
||||
} else {
|
||||
serial = boost::make_shared<SerialStream>(data);
|
||||
}
|
||||
}
|
||||
|
||||
Create::Create(RobotModel m) : model(m) {
|
||||
|
|
@ -83,7 +91,7 @@ namespace create {
|
|||
|
||||
void Create::onData() {
|
||||
if (firstOnData) {
|
||||
if (model == CREATE_2) {
|
||||
if (model.getVersion() >= V_3) {
|
||||
// Initialize tick counts
|
||||
prevTicksLeft = GET_DATA(ID_LEFT_ENC);
|
||||
prevTicksRight = GET_DATA(ID_RIGHT_ENC);
|
||||
|
|
@ -95,22 +103,45 @@ namespace create {
|
|||
// Get current time
|
||||
util::timestamp_t curTime = util::getTimestamp();
|
||||
float dt = (curTime - prevOnDataTime) / 1000000.0;
|
||||
float deltaDist, deltaX, deltaY, deltaYaw, leftWheelDist, rightWheelDist;
|
||||
if (model == CREATE_1) {
|
||||
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
|
||||
* Angle returned is NOT correct if your robot is using older firmware: *
|
||||
* http://answers.ros.org/question/31935/createroomba-odometry/ *
|
||||
* TODO: Consider using velocity command as substitute for pose estimation. *
|
||||
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
|
||||
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( util::normalizeAngle(pose.yaw + deltaYaw) );
|
||||
deltaY = deltaDist * sin( util::normalizeAngle(pose.yaw + deltaYaw) );
|
||||
const float deltaYawWheelDist = (util::CREATE_1_AXLE_LENGTH / 2.0) * deltaYaw;
|
||||
leftWheelDist = deltaDist - deltaYawWheelDist;
|
||||
rightWheelDist = deltaDist + deltaYawWheelDist;
|
||||
float deltaDist, deltaX, deltaY, deltaYaw, leftWheelDist, rightWheelDist, wheelDistDiff;
|
||||
|
||||
// Protocol versions 1 and 2 use distance and angle fields for odometry
|
||||
int16_t angleField;
|
||||
if (model.getVersion() <= V_2) {
|
||||
// This is a standards compliant way of doing unsigned to signed conversion
|
||||
uint16_t distanceRaw = GET_DATA(ID_DISTANCE);
|
||||
int16_t distance;
|
||||
std::memcpy(&distance, &distanceRaw, sizeof(distance));
|
||||
deltaDist = distance / 1000.0; // mm -> m
|
||||
|
||||
// Angle is processed differently in versions 1 and 2
|
||||
uint16_t angleRaw = GET_DATA(ID_ANGLE);
|
||||
std::memcpy(&angleField, &angleRaw, sizeof(angleField));
|
||||
}
|
||||
else if (model == CREATE_2) {
|
||||
|
||||
if (model.getVersion() == V_1) {
|
||||
wheelDistDiff = 2.0 * angleField / 1000.0;
|
||||
leftWheelDist = deltaDist - (wheelDistDiff / 2.0);
|
||||
rightWheelDist = deltaDist + (wheelDistDiff / 2.0);
|
||||
deltaYaw = wheelDistDiff / model.getAxleLength();
|
||||
} else if (model.getVersion() == V_2) {
|
||||
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
|
||||
* Certain older Creates have major problems with odometry *
|
||||
* http://answers.ros.org/question/31935/createroomba-odometry/ *
|
||||
* *
|
||||
* All Creates have an issue with rounding of the angle field, which causes *
|
||||
* major errors to accumulate in the odometry yaw. *
|
||||
* http://wiki.tekkotsu.org/index.php/Create_Odometry_Bug *
|
||||
* https://github.com/AutonomyLab/create_autonomy/issues/28 *
|
||||
* *
|
||||
* TODO: Consider using velocity command as substitute for pose estimation *
|
||||
* to mitigate both of these problems. *
|
||||
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
|
||||
deltaYaw = angleField * (util::PI / 180.0); // D2R
|
||||
wheelDistDiff = model.getAxleLength() * deltaYaw;
|
||||
leftWheelDist = deltaDist - (wheelDistDiff / 2.0);
|
||||
rightWheelDist = deltaDist + (wheelDistDiff / 2.0);
|
||||
} else if (model.getVersion() >= V_3) {
|
||||
// Get cumulative ticks (wraps around at 65535)
|
||||
uint16_t totalTicksLeft = GET_DATA(ID_LEFT_ENC);
|
||||
uint16_t totalTicksRight = GET_DATA(ID_RIGHT_ENC);
|
||||
|
|
@ -121,35 +152,33 @@ namespace create {
|
|||
prevTicksRight = totalTicksRight;
|
||||
|
||||
// Handle wrap around
|
||||
if (fabs(ticksLeft) > 0.9 * util::CREATE_2_MAX_ENCODER_TICKS) {
|
||||
ticksLeft = (ticksLeft % util::CREATE_2_MAX_ENCODER_TICKS) + 1;
|
||||
if (fabs(ticksLeft) > 0.9 * util::V_3_MAX_ENCODER_TICKS) {
|
||||
ticksLeft = (ticksLeft % util::V_3_MAX_ENCODER_TICKS) + 1;
|
||||
}
|
||||
if (fabs(ticksRight) > 0.9 * util::CREATE_2_MAX_ENCODER_TICKS) {
|
||||
ticksRight = (ticksRight % util::CREATE_2_MAX_ENCODER_TICKS) + 1;
|
||||
if (fabs(ticksRight) > 0.9 * util::V_3_MAX_ENCODER_TICKS) {
|
||||
ticksRight = (ticksRight % util::V_3_MAX_ENCODER_TICKS) + 1;
|
||||
}
|
||||
|
||||
// Compute distance travelled by each wheel
|
||||
leftWheelDist = (ticksLeft / util::CREATE_2_TICKS_PER_REV)
|
||||
* util::CREATE_2_WHEEL_DIAMETER * util::PI;
|
||||
rightWheelDist = (ticksRight / util::CREATE_2_TICKS_PER_REV)
|
||||
* util::CREATE_2_WHEEL_DIAMETER * util::PI;
|
||||
|
||||
float wheelDistDiff = rightWheelDist - leftWheelDist;
|
||||
leftWheelDist = (ticksLeft / util::V_3_TICKS_PER_REV)
|
||||
* model.getWheelDiameter() * util::PI;
|
||||
rightWheelDist = (ticksRight / util::V_3_TICKS_PER_REV)
|
||||
* model.getWheelDiameter() * util::PI;
|
||||
deltaDist = (rightWheelDist + leftWheelDist) / 2.0;
|
||||
|
||||
// Moving straight
|
||||
if (fabs(wheelDistDiff) < util::EPS) {
|
||||
deltaX = deltaDist * cos(pose.yaw);
|
||||
deltaY = deltaDist * sin(pose.yaw);
|
||||
deltaYaw = 0.0;
|
||||
}
|
||||
else {
|
||||
float turnRadius = (util::CREATE_2_AXLE_LENGTH / 2.0) * (leftWheelDist + rightWheelDist) / wheelDistDiff;
|
||||
deltaYaw = wheelDistDiff / util::CREATE_2_AXLE_LENGTH;
|
||||
deltaX = turnRadius * (sin(pose.yaw + deltaYaw) - sin(pose.yaw));
|
||||
deltaY = -turnRadius * (cos(pose.yaw + deltaYaw) - cos(pose.yaw));
|
||||
}
|
||||
} // if CREATE_2
|
||||
wheelDistDiff = rightWheelDist - leftWheelDist;
|
||||
deltaYaw = wheelDistDiff / model.getAxleLength();
|
||||
}
|
||||
|
||||
// Moving straight
|
||||
if (fabs(wheelDistDiff) < util::EPS) {
|
||||
deltaX = deltaDist * cos(pose.yaw);
|
||||
deltaY = deltaDist * sin(pose.yaw);
|
||||
} else {
|
||||
float turnRadius = (model.getAxleLength() / 2.0) * (leftWheelDist + rightWheelDist) / wheelDistDiff;
|
||||
deltaX = turnRadius * (sin(pose.yaw + deltaYaw) - sin(pose.yaw));
|
||||
deltaY = -turnRadius * (cos(pose.yaw + deltaYaw) - cos(pose.yaw));
|
||||
}
|
||||
|
||||
totalLeftDist += leftWheelDist;
|
||||
totalRightDist += rightWheelDist;
|
||||
|
|
@ -158,8 +187,7 @@ namespace create {
|
|||
vel.x = deltaDist / dt;
|
||||
vel.y = 0.0;
|
||||
vel.yaw = deltaYaw / dt;
|
||||
}
|
||||
else {
|
||||
} else {
|
||||
vel.x = 0.0;
|
||||
vel.y = 0.0;
|
||||
vel.yaw = 0.0;
|
||||
|
|
@ -171,7 +199,7 @@ namespace create {
|
|||
float kl = 1.0;
|
||||
float cosYawAndHalfDelta = cos(pose.yaw + (deltaYaw / 2.0)); // deltaX?
|
||||
float sinYawAndHalfDelta = sin(pose.yaw + (deltaYaw / 2.0)); // deltaY?
|
||||
float distOverTwoWB = deltaDist / (util::CREATE_2_AXLE_LENGTH * 2.0);
|
||||
float distOverTwoWB = deltaDist / (model.getAxleLength() * 2.0);
|
||||
|
||||
Matrix invCovar(2, 2);
|
||||
invCovar(0, 0) = kr * fabs(rightWheelDist);
|
||||
|
|
@ -184,8 +212,8 @@ namespace create {
|
|||
Finc(0, 1) = (cosYawAndHalfDelta / 2.0) + (distOverTwoWB * sinYawAndHalfDelta);
|
||||
Finc(1, 0) = (sinYawAndHalfDelta / 2.0) + (distOverTwoWB * cosYawAndHalfDelta);
|
||||
Finc(1, 1) = (sinYawAndHalfDelta / 2.0) - (distOverTwoWB * cosYawAndHalfDelta);
|
||||
Finc(2, 0) = (1.0 / util::CREATE_2_AXLE_LENGTH);
|
||||
Finc(2, 1) = (-1.0 / util::CREATE_2_AXLE_LENGTH);
|
||||
Finc(2, 0) = (1.0 / model.getAxleLength());
|
||||
Finc(2, 1) = (-1.0 / model.getAxleLength());
|
||||
Matrix FincT = boost::numeric::ublas::trans(Finc);
|
||||
|
||||
Matrix Fp(3, 3);
|
||||
|
|
@ -271,24 +299,39 @@ namespace create {
|
|||
//}
|
||||
|
||||
bool Create::setMode(const CreateMode& mode) {
|
||||
if (model.getVersion() == V_1){
|
||||
// Switch to safe mode (required for compatibility with V_1)
|
||||
if (!(serial->sendOpcode(OC_START) && serial->sendOpcode(OC_CONTROL))) return false;
|
||||
}
|
||||
bool ret;
|
||||
switch (mode) {
|
||||
case MODE_OFF:
|
||||
return serial->sendOpcode(OC_POWER);
|
||||
if (model.getVersion() == V_2) {
|
||||
CERR("[create::Create] ", "protocol version 2 does not support turning robot off");
|
||||
ret = false;
|
||||
} else {
|
||||
ret = serial->sendOpcode(OC_POWER);
|
||||
}
|
||||
break;
|
||||
case MODE_PASSIVE:
|
||||
return serial->sendOpcode(OC_START);
|
||||
ret = serial->sendOpcode(OC_START);
|
||||
break;
|
||||
case MODE_SAFE:
|
||||
return serial->sendOpcode(OC_SAFE);
|
||||
if (model.getVersion() > V_1) {
|
||||
ret = serial->sendOpcode(OC_SAFE);
|
||||
}
|
||||
break;
|
||||
case MODE_FULL:
|
||||
return serial->sendOpcode(OC_FULL);
|
||||
ret = serial->sendOpcode(OC_FULL);
|
||||
break;
|
||||
default:
|
||||
CERR("[create::Create] ", "cannot set robot to mode '" << mode << "'");
|
||||
break;
|
||||
ret = false;
|
||||
}
|
||||
return false;
|
||||
if (ret) {
|
||||
this->mode = mode;
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
bool Create::clean(const CleanMode& mode) {
|
||||
|
|
@ -309,16 +352,18 @@ namespace create {
|
|||
return serial->send(cmd, 4);
|
||||
}
|
||||
|
||||
bool Create::driveRadius(const float& vel, const float& radius) const {
|
||||
bool Create::driveRadius(const float& vel, const float& radius) {
|
||||
// Bound velocity
|
||||
float boundedVel = BOUND_CONST(vel, -model.getMaxVelocity(), model.getMaxVelocity());
|
||||
|
||||
// Expects each parameter as two bytes each and in millimeters
|
||||
int16_t vel_mm = roundf(vel * 1000);
|
||||
int16_t vel_mm = roundf(boundedVel * 1000);
|
||||
int16_t radius_mm = roundf(radius * 1000);
|
||||
BOUND(vel_mm, -util::CREATE_2_MAX_VEL * 1000, util::CREATE_2_MAX_VEL * 1000);
|
||||
|
||||
// Bound radius if not a special case
|
||||
if (radius_mm != 32768 && radius_mm != 32767 &&
|
||||
radius_mm != -1 && radius_mm != 1) {
|
||||
BOUND(radius_mm, -util::CREATE_2_MAX_RADIUS, util::CREATE_2_MAX_RADIUS);
|
||||
BOUND(radius_mm, -util::MAX_RADIUS * 1000, util::MAX_RADIUS * 1000);
|
||||
}
|
||||
|
||||
uint8_t cmd[5] = { OC_DRIVE,
|
||||
|
|
@ -331,25 +376,58 @@ namespace create {
|
|||
return serial->send(cmd, 5);
|
||||
}
|
||||
|
||||
bool Create::driveWheels(const float& leftVel, const float& rightVel) const {
|
||||
int16_t leftCmd = roundf(leftVel * 1000);
|
||||
int16_t rightCmd = roundf(rightVel * 1000);
|
||||
BOUND(leftCmd, -util::CREATE_2_MAX_VEL * 1000, util::CREATE_2_MAX_VEL * 1000);
|
||||
BOUND(rightCmd, -util::CREATE_2_MAX_VEL * 1000, util::CREATE_2_MAX_VEL * 1000);
|
||||
bool Create::driveWheels(const float& leftVel, const float& rightVel) {
|
||||
const float boundedLeftVel = BOUND_CONST(leftVel, -model.getMaxVelocity(), model.getMaxVelocity());
|
||||
const float boundedRightVel = BOUND_CONST(rightVel, -model.getMaxVelocity(), model.getMaxVelocity());
|
||||
requestedLeftVel = boundedLeftVel;
|
||||
requestedRightVel = boundedRightVel;
|
||||
if (model.getVersion() > V_1) {
|
||||
int16_t leftCmd = roundf(boundedLeftVel * 1000);
|
||||
int16_t rightCmd = roundf(boundedRightVel * 1000);
|
||||
|
||||
uint8_t cmd[5] = { OC_DRIVE_DIRECT,
|
||||
rightCmd >> 8,
|
||||
rightCmd & 0xff,
|
||||
leftCmd >> 8,
|
||||
leftCmd & 0xff
|
||||
};
|
||||
return serial->send(cmd, 5);
|
||||
uint8_t cmd[5] = { OC_DRIVE_DIRECT,
|
||||
rightCmd >> 8,
|
||||
rightCmd & 0xff,
|
||||
leftCmd >> 8,
|
||||
leftCmd & 0xff
|
||||
};
|
||||
return serial->send(cmd, 5);
|
||||
} else {
|
||||
float radius;
|
||||
// Prevent divide by zero when driving straight
|
||||
if (boundedLeftVel != boundedRightVel) {
|
||||
radius = -((model.getAxleLength() / 2.0) * (boundedLeftVel + boundedRightVel)) /
|
||||
(boundedLeftVel - boundedRightVel);
|
||||
} else {
|
||||
radius = util::STRAIGHT_RADIUS;
|
||||
}
|
||||
|
||||
float vel;
|
||||
// Fix signs for spin in place
|
||||
if (boundedLeftVel == -boundedRightVel || std::abs(roundf(radius * 1000)) <= 1) {
|
||||
radius = util::IN_PLACE_RADIUS;
|
||||
vel = boundedRightVel;
|
||||
} else {
|
||||
vel = (std::abs(boundedLeftVel) + std::abs(boundedRightVel)) / 2.0 * ((boundedLeftVel + boundedRightVel) > 0 ? 1.0 : -1.0);
|
||||
}
|
||||
|
||||
// Radius turns have a maximum radius of 2.0 meters
|
||||
// When the radius is > 2 but <= 10, use a 2 meter radius
|
||||
// When it is > 10, drive straight
|
||||
// TODO: alternate between these 2 meter radius and straight to
|
||||
// fake a larger radius
|
||||
if (radius > 10.0) {
|
||||
radius = util::STRAIGHT_RADIUS;
|
||||
}
|
||||
|
||||
return driveRadius(vel, radius);
|
||||
}
|
||||
}
|
||||
|
||||
bool Create::drive(const float& xVel, const float& angularVel) const {
|
||||
bool Create::drive(const float& xVel, const float& angularVel) {
|
||||
// Compute left and right wheel velocities
|
||||
float leftVel = xVel - ((util::CREATE_2_AXLE_LENGTH / 2.0) * angularVel);
|
||||
float rightVel = xVel + ((util::CREATE_2_AXLE_LENGTH / 2.0) * angularVel);
|
||||
float leftVel = xVel - ((model.getAxleLength() / 2.0) * angularVel);
|
||||
float rightVel = xVel + ((model.getAxleLength() / 2.0) * angularVel);
|
||||
return driveWheels(leftVel, rightVel);
|
||||
}
|
||||
|
||||
|
|
@ -552,8 +630,8 @@ namespace create {
|
|||
}
|
||||
|
||||
uint8_t Create::getDirtDetect() const {
|
||||
if (data->isValidPacketID(ID_DIRT_DETECT)) {
|
||||
return GET_DATA(ID_DIRT_DETECT);
|
||||
if (data->isValidPacketID(ID_DIRT_DETECT_LEFT)) {
|
||||
return GET_DATA(ID_DIRT_DETECT_LEFT);
|
||||
}
|
||||
else {
|
||||
CERR("[create::Create] ", "Dirt detector not supported!");
|
||||
|
|
@ -869,47 +947,26 @@ namespace create {
|
|||
}
|
||||
|
||||
float Create::getLeftWheelDistance() const {
|
||||
return totalLeftDist;
|
||||
return totalLeftDist;
|
||||
}
|
||||
|
||||
float Create::getRightWheelDistance() const {
|
||||
return totalRightDist;
|
||||
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;
|
||||
}
|
||||
return requestedLeftVel;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
return requestedRightVel;
|
||||
}
|
||||
|
||||
create::CreateMode Create::getMode() const {
|
||||
create::CreateMode Create::getMode() {
|
||||
if (data->isValidPacketID(ID_OI_MODE)) {
|
||||
return (create::CreateMode) GET_DATA(ID_OI_MODE);
|
||||
}
|
||||
else {
|
||||
CERR("[create::Create] ", "Querying Mode not supported!");
|
||||
return create::MODE_UNAVAILABLE;
|
||||
mode = (create::CreateMode) GET_DATA(ID_OI_MODE);
|
||||
}
|
||||
return mode;
|
||||
}
|
||||
|
||||
Pose Create::getPose() const {
|
||||
|
|
|
|||
96
src/data.cpp
96
src/data.cpp
|
|
@ -1,72 +1,50 @@
|
|||
#include "create/data.h"
|
||||
|
||||
#define ADD_PACKET(id,nbytes,info) (packets[id]=boost::make_shared<Packet>(nbytes,info))
|
||||
#define ADD_PACKET(id,nbytes,info,enabledVersion) if ((enabledVersion) & version) packets[id]=boost::make_shared<Packet>(nbytes,info)
|
||||
|
||||
namespace create {
|
||||
|
||||
Data::Data(RobotModel model) {
|
||||
Data::Data(ProtocolVersion version) {
|
||||
// Populate data map
|
||||
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
|
||||
* WARNING: Adding too many packets will flood the serial *
|
||||
* buffer and corrupt the stream. *
|
||||
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
|
||||
ADD_PACKET(ID_BUMP_WHEELDROP, 1, "bumps_wheeldrops");
|
||||
ADD_PACKET(ID_WALL, 1, "wall");
|
||||
ADD_PACKET(ID_CLIFF_LEFT, 1, "cliff_left");
|
||||
ADD_PACKET(ID_CLIFF_FRONT_LEFT, 1, "cliff_front_left");
|
||||
ADD_PACKET(ID_CLIFF_FRONT_RIGHT, 1, "cliff_front_right");
|
||||
ADD_PACKET(ID_CLIFF_RIGHT, 1, "cliff_right");
|
||||
ADD_PACKET(ID_IR_OMNI, 1, "ir_opcode");
|
||||
ADD_PACKET(ID_BUTTONS, 1, "buttons");
|
||||
ADD_PACKET(ID_CHARGE_STATE, 1, "charging_state");
|
||||
ADD_PACKET(ID_VOLTAGE, 2, "voltage");
|
||||
ADD_PACKET(ID_CURRENT, 2, "current");
|
||||
ADD_PACKET(ID_TEMP, 1, "temperature");
|
||||
ADD_PACKET(ID_CHARGE , 2, "battery_charge");
|
||||
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");
|
||||
ADD_PACKET(ID_ANGLE, 2, "angle");
|
||||
}
|
||||
else if (model == CREATE_2) {
|
||||
//ADD_PACKET(ID_OVERCURRENTS, 1, "overcurrents");
|
||||
ADD_PACKET(ID_DIRT_DETECT, 1, "dirt_detect");
|
||||
//ADD_PACKET(ID_UNUSED_1, 1, "unused 1");
|
||||
//ADD_PACKET(ID_WALL_SIGNAL, 2, "wall_signal");
|
||||
//ADD_PACKET(ID_CLIFF_LEFT_SIGNAL, 2, "cliff_left_signal");
|
||||
//ADD_PACKET(ID_CLIFF_FRONT_LEFT_SIGNAL, 2, "cliff_front_left_signal");
|
||||
//ADD_PACKET(ID_CLIFF_FRONT_RIGHT_SIGNAL, 2, "cliff_front_right_signal");
|
||||
//ADD_PACKET(ID_CLIFF_RIGHT_SIGNAL, 2, "cliff_right_signal");
|
||||
//ADD_PACKET(ID_UNUSED_2, 1, "unused 2");
|
||||
//ADD_PACKET(ID_UNUSED_3, 2, "unused 3");
|
||||
//ADD_PACKET(ID_CHARGE_SOURCE, 1, "charger_available");
|
||||
//ADD_PACKET(ID_SONG_NUM, 1, "song_number");
|
||||
//ADD_PACKET(ID_PLAYING, 1, "song_playing");
|
||||
//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_LEFT_ENC, 2, "enc_counts_left");
|
||||
ADD_PACKET(ID_RIGHT_ENC, 2, "enc_counts_right");
|
||||
ADD_PACKET(ID_LIGHT, 1, "light_bumper");
|
||||
ADD_PACKET(ID_LIGHT_LEFT, 2, "light_bumper_left");
|
||||
ADD_PACKET(ID_LIGHT_FRONT_LEFT, 2, "light_bumper_front_left");
|
||||
ADD_PACKET(ID_LIGHT_CENTER_LEFT, 2, "light_bumper_center_left");
|
||||
ADD_PACKET(ID_LIGHT_CENTER_RIGHT, 2, "light_bumper_center_right");
|
||||
ADD_PACKET(ID_LIGHT_FRONT_RIGHT, 2, "light_bumper_front_right");
|
||||
ADD_PACKET(ID_LIGHT_RIGHT, 2, "light_bumper_right");
|
||||
ADD_PACKET(ID_IR_LEFT, 1, "ir_opcode_left");
|
||||
ADD_PACKET(ID_IR_RIGHT, 1, "ir_opcode_right");
|
||||
//ADD_PACKET(ID_LEFT_MOTOR_CURRENT, 2, "left_motor_current");
|
||||
//ADD_PACKET(ID_RIGHT_MOTOR_CURRENT, 2, "right_motor_current");
|
||||
//ADD_PACKET(ID_MAIN_BRUSH_CURRENT, 2, "main_brush_current");
|
||||
//ADD_PACKET(ID_SIDE_BRUSH_CURRENT, 2, "side_brush_current");
|
||||
ADD_PACKET(ID_STASIS, 1, "stasis");
|
||||
} // if CREATE_2
|
||||
ADD_PACKET(ID_BUMP_WHEELDROP, 1, "bumps_wheeldrops", V_ALL);
|
||||
ADD_PACKET(ID_WALL, 1, "wall", V_ALL);
|
||||
ADD_PACKET(ID_CLIFF_LEFT, 1, "cliff_left", V_ALL);
|
||||
ADD_PACKET(ID_CLIFF_FRONT_LEFT, 1, "cliff_front_left", V_ALL);
|
||||
ADD_PACKET(ID_CLIFF_FRONT_RIGHT, 1, "cliff_front_right", V_ALL);
|
||||
ADD_PACKET(ID_CLIFF_RIGHT, 1, "cliff_right", V_ALL);
|
||||
ADD_PACKET(ID_VIRTUAL_WALL, 1, "virtual_wall", V_ALL);
|
||||
ADD_PACKET(ID_OVERCURRENTS, 1, "overcurrents", V_1);
|
||||
ADD_PACKET(ID_DIRT_DETECT_LEFT, 1, "dirt_detect_left", V_ALL);
|
||||
ADD_PACKET(ID_DIRT_DETECT_RIGHT, 1, "dirt_detect_right", V_1);
|
||||
ADD_PACKET(ID_IR_OMNI, 1, "ir_opcode", V_ALL);
|
||||
ADD_PACKET(ID_BUTTONS, 1, "buttons", V_ALL);
|
||||
ADD_PACKET(ID_DISTANCE, 2, "distance", V_1 | V_2);
|
||||
ADD_PACKET(ID_ANGLE, 2, "angle", V_1 | V_2);
|
||||
ADD_PACKET(ID_CHARGE_STATE, 1, "charging_state", V_ALL);
|
||||
ADD_PACKET(ID_VOLTAGE, 2, "voltage", V_ALL);
|
||||
ADD_PACKET(ID_CURRENT, 2, "current", V_ALL);
|
||||
ADD_PACKET(ID_TEMP, 1, "temperature", V_ALL);
|
||||
ADD_PACKET(ID_CHARGE , 2, "battery_charge", V_ALL);
|
||||
ADD_PACKET(ID_CAPACITY, 2, "battery_capacity", V_ALL);
|
||||
ADD_PACKET(ID_OI_MODE, 1, "oi_mode", V_2 | V_3);
|
||||
ADD_PACKET(ID_LEFT_ENC, 2, "enc_counts_left", V_3);
|
||||
ADD_PACKET(ID_RIGHT_ENC, 2, "enc_counts_right", V_3);
|
||||
ADD_PACKET(ID_LEFT_ENC, 2, "enc_counts_left", V_3);
|
||||
ADD_PACKET(ID_RIGHT_ENC, 2, "enc_counts_right", V_3);
|
||||
ADD_PACKET(ID_LIGHT, 1, "light_bumper", V_3);
|
||||
ADD_PACKET(ID_LIGHT_LEFT, 2, "light_bumper_left", V_3);
|
||||
ADD_PACKET(ID_LIGHT_FRONT_LEFT, 2, "light_bumper_front_left", V_3);
|
||||
ADD_PACKET(ID_LIGHT_CENTER_LEFT, 2, "light_bumper_center_left", V_3);
|
||||
ADD_PACKET(ID_LIGHT_CENTER_RIGHT, 2, "light_bumper_center_right", V_3);
|
||||
ADD_PACKET(ID_LIGHT_FRONT_RIGHT, 2, "light_bumper_front_right", V_3);
|
||||
ADD_PACKET(ID_LIGHT_RIGHT, 2, "light_bumper_right", V_3);
|
||||
ADD_PACKET(ID_IR_LEFT, 1, "ir_opcode_left", V_3);
|
||||
ADD_PACKET(ID_IR_RIGHT, 1, "ir_opcode_right", V_3);
|
||||
ADD_PACKET(ID_STASIS, 1, "stasis", V_3);
|
||||
|
||||
totalDataBytes = 0;
|
||||
for (std::map<uint8_t, boost::shared_ptr<Packet> >::iterator it = packets.begin();
|
||||
|
|
|
|||
|
|
@ -5,11 +5,9 @@
|
|||
|
||||
namespace create {
|
||||
|
||||
Serial::Serial(boost::shared_ptr<Data> d, const uint8_t& header) :
|
||||
Serial::Serial(boost::shared_ptr<Data> d) :
|
||||
data(d),
|
||||
headerByte(header),
|
||||
port(io),
|
||||
readState(READ_HEADER),
|
||||
isReading(false),
|
||||
dataReady(false),
|
||||
corruptPackets(0),
|
||||
|
|
@ -64,27 +62,10 @@ namespace create {
|
|||
// Only allow once
|
||||
if (isReading) return true;
|
||||
|
||||
// Request from Create that we want a stream containing all packets
|
||||
uint8_t numPackets = data->getNumPackets();
|
||||
std::vector<uint8_t> packetIDs = data->getPacketIDs();
|
||||
uint8_t streamReq[2 + numPackets];
|
||||
streamReq[0] = OC_STREAM;
|
||||
streamReq[1] = numPackets;
|
||||
int i = 2;
|
||||
for (std::vector<uint8_t>::iterator it = packetIDs.begin(); it != packetIDs.end(); ++it) {
|
||||
streamReq[i] = *it;
|
||||
i++;
|
||||
}
|
||||
|
||||
// Start OI
|
||||
sendOpcode(OC_START);
|
||||
|
||||
// Start streaming data
|
||||
send(streamReq, 2 + numPackets);
|
||||
|
||||
expectedNumBytes = data->getTotalDataBytes() + numPackets;
|
||||
|
||||
//TODO: handle boost exceptions
|
||||
if (!startSensorStream()) return false;
|
||||
|
||||
io.reset();
|
||||
|
||||
|
|
@ -112,7 +93,7 @@ namespace create {
|
|||
|
||||
// Request data again
|
||||
sendOpcode(OC_START);
|
||||
send(streamReq, 2 + numPackets);
|
||||
startSensorStream();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -158,74 +139,7 @@ namespace create {
|
|||
|
||||
// Should have read exactly one byte
|
||||
if (size == 1) {
|
||||
numBytesRead++;
|
||||
byteSum += byteRead;
|
||||
switch (readState) {
|
||||
case READ_HEADER:
|
||||
if (byteRead == headerByte) {
|
||||
readState = READ_NBYTES;
|
||||
byteSum = byteRead;
|
||||
}
|
||||
break;
|
||||
|
||||
case READ_NBYTES:
|
||||
if (byteRead == expectedNumBytes) {
|
||||
readState = READ_PACKET_ID;
|
||||
numBytesRead = 0;
|
||||
}
|
||||
else {
|
||||
//notifyDataReady();
|
||||
readState = READ_HEADER;
|
||||
}
|
||||
break;
|
||||
|
||||
case READ_PACKET_ID:
|
||||
packetID = byteRead;
|
||||
if (data->isValidPacketID(packetID)) {
|
||||
expectedNumDataBytes = data->getPacket(packetID)->nbytes;
|
||||
assert(expectedNumDataBytes == 1 || expectedNumDataBytes == 2);
|
||||
numDataBytesRead = 0;
|
||||
packetBytes = 0;
|
||||
readState = READ_PACKET_BYTES;
|
||||
}
|
||||
else {
|
||||
//notifyDataReady();
|
||||
readState = READ_HEADER;
|
||||
}
|
||||
break;
|
||||
|
||||
case READ_PACKET_BYTES:
|
||||
numDataBytesRead++;
|
||||
if (expectedNumDataBytes == 2 && numDataBytesRead == 1) {
|
||||
// High byte first
|
||||
packetBytes = (byteRead << 8) & 0xff00;
|
||||
}
|
||||
else {
|
||||
// Low byte
|
||||
packetBytes += byteRead;
|
||||
}
|
||||
if (numDataBytesRead >= expectedNumDataBytes) {
|
||||
data->getPacket(packetID)->setTempData(packetBytes);
|
||||
if (numBytesRead >= expectedNumBytes)
|
||||
readState = READ_CHECKSUM;
|
||||
else
|
||||
readState = READ_PACKET_ID;
|
||||
}
|
||||
break;
|
||||
|
||||
case READ_CHECKSUM:
|
||||
if ((byteSum & 0xFF) == 0) {
|
||||
notifyDataReady();
|
||||
}
|
||||
else {
|
||||
// Corrupt data
|
||||
corruptPackets++;
|
||||
}
|
||||
totalPackets++;
|
||||
// Start again
|
||||
readState = READ_HEADER;
|
||||
break;
|
||||
} // end switch (readState)
|
||||
processByte(byteRead);
|
||||
} // end if (size == 1)
|
||||
|
||||
// Read the next byte
|
||||
|
|
|
|||
70
src/serial_query.cpp
Normal file
70
src/serial_query.cpp
Normal file
|
|
@ -0,0 +1,70 @@
|
|||
#include <iostream>
|
||||
|
||||
#include "create/serial_query.h"
|
||||
#include "create/types.h"
|
||||
|
||||
#define SENSORS_RESPONSE_LENGTH 20
|
||||
|
||||
namespace create {
|
||||
|
||||
SerialQuery::SerialQuery(boost::shared_ptr<Data> d) : Serial(d),
|
||||
streamRecoveryTimer(io),
|
||||
packetID(ID_BUMP_WHEELDROP),
|
||||
packetByte(0),
|
||||
packetData(0),
|
||||
maxPacketID(ID_CAPACITY) {
|
||||
}
|
||||
|
||||
bool SerialQuery::startSensorStream() {
|
||||
if (!started) {
|
||||
requestSensorData();
|
||||
started = true;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void SerialQuery::requestSensorData() {
|
||||
static const uint8_t requestPacket[2] = { OC_SENSORS, ID_GROUP_0 };
|
||||
// Prevents previous packet from corrupting next one
|
||||
flushInput();
|
||||
send(requestPacket, 2);
|
||||
// Automatically resend request if no response is received
|
||||
streamRecoveryTimer.expires_from_now(boost::posix_time::milliseconds(50));
|
||||
streamRecoveryTimer.async_wait(boost::bind(&SerialQuery::restartSensorStream, this, _1));
|
||||
}
|
||||
|
||||
void SerialQuery::restartSensorStream(const boost::system::error_code& err) {
|
||||
if (err != boost::asio::error::operation_aborted) {
|
||||
if (packetID != ID_BUMP_WHEELDROP) {
|
||||
++corruptPackets;
|
||||
}
|
||||
requestSensorData();
|
||||
}
|
||||
}
|
||||
|
||||
void SerialQuery::flushInput() {
|
||||
// Only works with POSIX support
|
||||
tcflush(port.lowest_layer().native(), TCIFLUSH);
|
||||
}
|
||||
|
||||
void SerialQuery::processByte(uint8_t byteRead) {
|
||||
packetData |= (static_cast<uint16_t>(byteRead) << (8 * packetByte));
|
||||
|
||||
if (packetByte > 0) {
|
||||
--packetByte;
|
||||
} else if (packetID < maxPacketID) {
|
||||
// New packet
|
||||
data->getPacket(packetID)->setTempData(packetData);
|
||||
packetData = 0;
|
||||
++packetID;
|
||||
packetByte = data->getPacket(packetID)->nbytes - 1;
|
||||
} else {
|
||||
// Response finished
|
||||
packetID = ID_BUMP_WHEELDROP;
|
||||
packetByte = 0;
|
||||
packetData = 0;
|
||||
notifyDataReady();
|
||||
requestSensorData();
|
||||
}
|
||||
}
|
||||
} // namespace create
|
||||
103
src/serial_stream.cpp
Normal file
103
src/serial_stream.cpp
Normal file
|
|
@ -0,0 +1,103 @@
|
|||
#include <iostream>
|
||||
|
||||
#include "create/serial_stream.h"
|
||||
#include "create/types.h"
|
||||
|
||||
namespace create {
|
||||
|
||||
SerialStream::SerialStream(boost::shared_ptr<Data> d, const uint8_t& header) : Serial(d), headerByte(header){
|
||||
}
|
||||
|
||||
bool SerialStream::startSensorStream() {
|
||||
// Request from Create that we want a stream containing all packets
|
||||
uint8_t numPackets = data->getNumPackets();
|
||||
std::vector<uint8_t> packetIDs = data->getPacketIDs();
|
||||
uint8_t streamReq[2 + numPackets];
|
||||
streamReq[0] = OC_STREAM;
|
||||
streamReq[1] = numPackets;
|
||||
int i = 2;
|
||||
for (std::vector<uint8_t>::iterator it = packetIDs.begin(); it != packetIDs.end(); ++it) {
|
||||
streamReq[i] = *it;
|
||||
i++;
|
||||
}
|
||||
|
||||
// Start streaming data
|
||||
send(streamReq, 2 + numPackets);
|
||||
|
||||
expectedNumBytes = data->getTotalDataBytes() + numPackets;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void SerialStream::processByte(uint8_t byteRead) {
|
||||
numBytesRead++;
|
||||
byteSum += byteRead;
|
||||
switch (readState) {
|
||||
case READ_HEADER:
|
||||
if (byteRead == headerByte) {
|
||||
readState = READ_NBYTES;
|
||||
byteSum = byteRead;
|
||||
}
|
||||
break;
|
||||
|
||||
case READ_NBYTES:
|
||||
if (byteRead == expectedNumBytes) {
|
||||
readState = READ_PACKET_ID;
|
||||
numBytesRead = 0;
|
||||
}
|
||||
else {
|
||||
//notifyDataReady();
|
||||
readState = READ_HEADER;
|
||||
}
|
||||
break;
|
||||
|
||||
case READ_PACKET_ID:
|
||||
packetID = byteRead;
|
||||
if (data->isValidPacketID(packetID)) {
|
||||
expectedNumDataBytes = data->getPacket(packetID)->nbytes;
|
||||
assert(expectedNumDataBytes == 1 || expectedNumDataBytes == 2);
|
||||
numDataBytesRead = 0;
|
||||
packetBytes = 0;
|
||||
readState = READ_PACKET_BYTES;
|
||||
}
|
||||
else {
|
||||
//notifyDataReady();
|
||||
readState = READ_HEADER;
|
||||
}
|
||||
break;
|
||||
|
||||
case READ_PACKET_BYTES:
|
||||
numDataBytesRead++;
|
||||
if (expectedNumDataBytes == 2 && numDataBytesRead == 1) {
|
||||
// High byte first
|
||||
packetBytes = (byteRead << 8) & 0xff00;
|
||||
}
|
||||
else {
|
||||
// Low byte
|
||||
packetBytes += byteRead;
|
||||
}
|
||||
if (numDataBytesRead >= expectedNumDataBytes) {
|
||||
data->getPacket(packetID)->setTempData(packetBytes);
|
||||
if (numBytesRead >= expectedNumBytes)
|
||||
readState = READ_CHECKSUM;
|
||||
else
|
||||
readState = READ_PACKET_ID;
|
||||
}
|
||||
break;
|
||||
|
||||
case READ_CHECKSUM:
|
||||
if ((byteSum & 0xFF) == 0) {
|
||||
notifyDataReady();
|
||||
}
|
||||
else {
|
||||
// Corrupt data
|
||||
corruptPackets++;
|
||||
}
|
||||
totalPackets++;
|
||||
// Start again
|
||||
readState = READ_HEADER;
|
||||
break;
|
||||
} // end switch (readState)
|
||||
}
|
||||
|
||||
} // namespace create
|
||||
52
src/types.cpp
Normal file
52
src/types.cpp
Normal file
|
|
@ -0,0 +1,52 @@
|
|||
#include "create/types.h"
|
||||
|
||||
namespace create {
|
||||
|
||||
RobotModel::RobotModel(const ProtocolVersion version, const float axleLength, const unsigned int baud, const float maxVelocity, const float wheelDiameter):
|
||||
id(nextId),
|
||||
version(version),
|
||||
axleLength(axleLength),
|
||||
baud(baud),
|
||||
maxVelocity(maxVelocity),
|
||||
wheelDiameter(wheelDiameter) {
|
||||
nextId <<= 1;
|
||||
}
|
||||
|
||||
bool RobotModel::operator ==(RobotModel& other) const {
|
||||
return id == other.id;
|
||||
}
|
||||
|
||||
RobotModel::operator uint32_t() const {
|
||||
return id;
|
||||
}
|
||||
|
||||
uint32_t RobotModel::getId() const {
|
||||
return id;
|
||||
}
|
||||
|
||||
ProtocolVersion RobotModel::getVersion() const {
|
||||
return version;
|
||||
}
|
||||
|
||||
float RobotModel::getAxleLength() const {
|
||||
return axleLength;
|
||||
}
|
||||
|
||||
unsigned int RobotModel::getBaud() const {
|
||||
return baud;
|
||||
}
|
||||
|
||||
float RobotModel::getMaxVelocity() const {
|
||||
return maxVelocity;
|
||||
}
|
||||
|
||||
float RobotModel::getWheelDiameter() const {
|
||||
return wheelDiameter;
|
||||
}
|
||||
|
||||
uint32_t RobotModel::nextId = 1;
|
||||
|
||||
RobotModel RobotModel::ROOMBA_400(V_1, 0.258, 57600);
|
||||
RobotModel RobotModel::CREATE_1(V_2, 0.258, 57600);
|
||||
RobotModel RobotModel::CREATE_2(V_3, 0.235, 115200);
|
||||
}
|
||||
Loading…
Add table
Add a link
Reference in a new issue