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

Merge pull request #6 from AutonomyLab/create-1

Browse source 
Create 1
This commit is contained in:
jacobperron 2016-03-24 18:51:02 -07:00
commit 43bcf690bd
7 changed files with 346 additions and 132 deletions
Download patch file Download diff file Expand all files Collapse all files

14
examples/create_demo.cpp
View file

@ -4,11 +4,19 @@ create::Create* robot;
int main(int argc, char** argv) {
std::string port = "/dev/ttyUSB0";
int baud = 115200;
create::RobotModel model = create::CREATE_2;
robot = new create::Create();
if (argc > 1 && std::string(argv[1]) == "create1") {
model = create::CREATE_1;
baud = 57600;
std::cout << "1st generation Create selected" << std::endl;
}
robot = new create::Create(model);
// Attempt to connect to Create
if (robot->connect(port, 115200))
if (robot->connect(port, baud))
std::cout << "Successfully connected to Create" << std::endl;
else {
std::cout << "Failed to connect to Create on port " << port.c_str() << std::endl;
@ -31,7 +39,7 @@ int main(int argc, char** argv) {
//for (int i = 0; i < songLength; i++) {
// durations[i] = 0.25;
//}
//robot->createSong(0, songLength, notes, durations);
//robot->defineSong(0, songLength, notes, durations);
//usleep(1000000);
//robot->playSong(0);

6
include/create/create.h
View file

@ -51,6 +51,8 @@ namespace create {
LED_CHECK = 8
};
RobotModel model;
uint8_t mainMotorPower;
uint8_t sideMotorPower;
uint8_t vacuumMotorPower;
@ -85,11 +87,11 @@ namespace create {
/* Default constructor.
* Does not attempt to establish serial connection to Create.
*/
Create();
Create(RobotModel = CREATE_2);
/* Attempts to establish serial connection to Create.
*/
Create(const std::string& port, const int& baud);
Create(const std::string& port, const int& baud, RobotModel = CREATE_2);
~Create();

2
include/create/data.h
View file

@ -48,7 +48,7 @@ namespace create {
std::vector<uint8_t> ids;
public:
Data();
Data(RobotModel = CREATE_2);
~Data();
bool isValidPacketID(const uint8_t id) const;

5
include/create/types.h
View file

@ -33,6 +33,11 @@ POSSIBILITY OF SUCH DAMAGE.
#define CREATE_TYPES_H
namespace create {
enum RobotModel {
CREATE_1 = 0, // Roomba 400 series
CREATE_2 // Roomba 600 series
};
enum SensorPacketID {
ID_GROUP_0 = 0,
ID_GROUP_1 = 1,

365
src/create.cpp
View file

@ -33,15 +33,15 @@ namespace create {
vel.x = 0;
vel.y = 0;
vel.yaw = 0;
data = boost::shared_ptr<Data>(new Data());
data = boost::shared_ptr<Data>(new Data(model));
serial = boost::make_shared<Serial>(data);
}
Create::Create() {
Create::Create(RobotModel m) : model(m) {
init();
}
Create::Create(const std::string& dev, const int& baud) {
Create::Create(const std::string& dev, const int& baud, RobotModel m) : model(m) {
init();
serial->connect(dev, baud);
}
@ -52,9 +52,11 @@ namespace create {
void Create::onData() {
if (firstOnData) {
// Initialize tick counts
prevTicksLeft = GET_DATA(ID_LEFT_ENC);
prevTicksRight = GET_DATA(ID_RIGHT_ENC);
if (model == CREATE_2) {
// Initialize tick counts
prevTicksLeft = GET_DATA(ID_LEFT_ENC);
prevTicksRight = GET_DATA(ID_RIGHT_ENC);
}
prevOnDataTime = util::getTimestamp();
firstOnData = false;
}
@ -62,67 +64,73 @@ namespace create {
// Get current time
util::timestamp_t curTime = util::getTimestamp();
float dt = (curTime - prevOnDataTime) / 1000000.0;
// Get cumulative ticks (wraps around at 65535)
uint16_t totalTicksLeft = GET_DATA(ID_LEFT_ENC);
uint16_t totalTicksRight = GET_DATA(ID_RIGHT_ENC);
// Compute ticks since last update
int ticksLeft = totalTicksLeft - prevTicksLeft;
int ticksRight = totalTicksRight - prevTicksRight;
prevTicksLeft = totalTicksLeft;
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(ticksRight) > 0.9 * util::CREATE_2_MAX_ENCODER_TICKS) {
ticksRight = (ticksRight % util::CREATE_2_MAX_ENCODER_TICKS) + 1;
}
// Compute distance travelled by each wheel
float leftWheelDist = (ticksLeft / util::CREATE_2_TICKS_PER_REV)
* util::CREATE_2_WHEEL_DIAMETER * util::PI;
float rightWheelDist = (ticksRight / util::CREATE_2_TICKS_PER_REV)
* util::CREATE_2_WHEEL_DIAMETER * util::PI;
float wheelDistDiff = rightWheelDist - leftWheelDist;
float deltaDist = (rightWheelDist + leftWheelDist) / 2.0;
// Moving straight
float deltaX, deltaY;
if (fabs(wheelDistDiff) < util::EPS) {
float deltaDist, deltaX, deltaY, deltaYaw;
if (model == CREATE_1) {
deltaDist = GET_DATA(ID_DISTANCE) / 1000.0; //mm -> m
deltaYaw = GET_DATA(ID_ANGLE) * util::PI / 180.0; // D2R
deltaX = deltaDist * cos(pose.yaw);
deltaY = deltaDist * sin(pose.yaw);
vel.yaw = 0;
}
else {
float turnRadius = (util::CREATE_2_AXLE_LENGTH / 2.0) * (leftWheelDist + rightWheelDist) / wheelDistDiff;
float deltaYaw = (rightWheelDist - leftWheelDist) / util::CREATE_2_AXLE_LENGTH;
deltaX = turnRadius * (sin(pose.yaw + deltaYaw) - sin(pose.yaw));
deltaY = turnRadius * (cos(pose.yaw + deltaYaw) - cos(pose.yaw));
pose.yaw = util::normalizeAngle(pose.yaw + deltaYaw);
if (fabs(dt) > util::EPS) {
vel.yaw = deltaYaw / dt;
else if (model == CREATE_2) {
// Get cumulative ticks (wraps around at 65535)
uint16_t totalTicksLeft = GET_DATA(ID_LEFT_ENC);
uint16_t totalTicksRight = GET_DATA(ID_RIGHT_ENC);
// Compute ticks since last update
int ticksLeft = totalTicksLeft - prevTicksLeft;
int ticksRight = totalTicksRight - prevTicksRight;
prevTicksLeft = totalTicksLeft;
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(ticksRight) > 0.9 * util::CREATE_2_MAX_ENCODER_TICKS) {
ticksRight = (ticksRight % util::CREATE_2_MAX_ENCODER_TICKS) + 1;
}
// Compute distance travelled by each wheel
float leftWheelDist = (ticksLeft / util::CREATE_2_TICKS_PER_REV)
* util::CREATE_2_WHEEL_DIAMETER * util::PI;
float rightWheelDist = (ticksRight / util::CREATE_2_TICKS_PER_REV)
* util::CREATE_2_WHEEL_DIAMETER * util::PI;
float wheelDistDiff = rightWheelDist - leftWheelDist;
deltaDist = (rightWheelDist + leftWheelDist) / 2.0;
// Moving straight
float deltaX, deltaY;
if (fabs(wheelDistDiff) < util::EPS) {
deltaX = deltaDist * cos(pose.yaw);
deltaY = deltaDist * sin(pose.yaw);
deltaYaw = 0.0;
//vel.yaw = 0;
}
else {
vel.yaw = 0;
float turnRadius = (util::CREATE_2_AXLE_LENGTH / 2.0) * (leftWheelDist + rightWheelDist) / wheelDistDiff;
deltaYaw = (rightWheelDist - leftWheelDist) / 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
if (fabs(dt) > util::EPS) {
vel.x = deltaX / dt;
vel.y = deltaY / dt;
vel.yaw = deltaYaw / dt;
}
else {
vel.x = 0;
vel.y = 0;
vel.x = 0.0;
vel.y = 0.0;
vel.yaw = 0.0;
}
pose.x += deltaDist * cos(pose.yaw);
pose.y += deltaDist * sin(pose.yaw);
pose.x += deltaX;
pose.y += deltaY;
pose.yaw = util::normalizeAngle(pose.yaw + deltaYaw);
prevOnDataTime = curTime;
// Make user registered callbacks, if any
// TODO
}
@ -370,131 +378,314 @@ namespace create {
}
bool Create::isWheeldrop() const {
return (GET_DATA(ID_BUMP_WHEELDROP) & 0x0C) != 0;
if (data->isValidPacketID(ID_BUMP_WHEELDROP)) {
return (GET_DATA(ID_BUMP_WHEELDROP) & 0x0C) != 0;
}
else {
CERR("[create::Create] ", "Wheeldrop sensor not supported!");
return false;
}
}
bool Create::isLeftBumper() const {
return (GET_DATA(ID_BUMP_WHEELDROP) & 0x02) != 0;
if (data->isValidPacketID(ID_BUMP_WHEELDROP)) {
return (GET_DATA(ID_BUMP_WHEELDROP) & 0x02) != 0;
}
else {
CERR("[create::Create] ", "Left bumper not supported!");
return false;
}
}
bool Create::isRightBumper() const {
return (GET_DATA(ID_BUMP_WHEELDROP) & 0x01) != 0;
if (data->isValidPacketID(ID_BUMP_WHEELDROP)) {
return (GET_DATA(ID_BUMP_WHEELDROP) & 0x01) != 0;
}
else {
CERR("[create::Create] ", "Right bumper not supported!");
return false;
}
}
bool Create::isWall() const {
return GET_DATA(ID_WALL) == 1;
if (data->isValidPacketID(ID_WALL)) {
return GET_DATA(ID_WALL) == 1;
}
else {
CERR("[create::Create] ", "Wall sensor not supported!");
return false;
}
}
bool Create::isCliff() const {
return GET_DATA(ID_CLIFF_LEFT) == 1 ||
GET_DATA(ID_CLIFF_FRONT_LEFT) == 1 ||
GET_DATA(ID_CLIFF_FRONT_RIGHT) == 1 ||
GET_DATA(ID_CLIFF_RIGHT) == 1;
if (data->isValidPacketID(ID_CLIFF_LEFT) &&
data->isValidPacketID(ID_CLIFF_FRONT_LEFT) &&
data->isValidPacketID(ID_CLIFF_FRONT_RIGHT) &&
data->isValidPacketID(ID_CLIFF_RIGHT)) {
return GET_DATA(ID_CLIFF_LEFT) == 1 ||
GET_DATA(ID_CLIFF_FRONT_LEFT) == 1 ||
GET_DATA(ID_CLIFF_FRONT_RIGHT) == 1 ||
GET_DATA(ID_CLIFF_RIGHT) == 1;
}
else {
CERR("[create::Create] ", "Cliff sensors not supported!");
return false;
}
}
uint8_t Create::getDirtDetect() const {
return GET_DATA(ID_DIRT_DETECT);
if (data->isValidPacketID(ID_DIRT_DETECT)) {
return GET_DATA(ID_DIRT_DETECT);
}
else {
CERR("[create::Create] ", "Dirt detector not supported!");
return -1;
}
}
uint8_t Create::getIROmni() const {
return GET_DATA(ID_IR_OMNI);
if (data->isValidPacketID(ID_IR_OMNI)) {
return GET_DATA(ID_IR_OMNI);
}
else {
CERR("[create::Create] ", "Omni IR sensor not supported!");
return -1;
}
}
uint8_t Create::getIRLeft() const {
return GET_DATA(ID_IR_LEFT);
if (data->isValidPacketID(ID_IR_LEFT)) {
return GET_DATA(ID_IR_LEFT);
}
else {
CERR("[create::Create] ", "Left IR sensor not supported!");
return -1;
}
}
uint8_t Create::getIRRight() const {
return GET_DATA(ID_IR_RIGHT);
if (data->isValidPacketID(ID_IR_RIGHT)) {
return GET_DATA(ID_IR_RIGHT);
}
else {
CERR("[create::Create] ", "Right IR sensor not supported!");
return -1;
}
}
ChargingState Create::getChargingState() const {
uint8_t chargeState = GET_DATA(ID_CHARGE_STATE);
assert(chargeState >= 0);
assert(chargeState <= 5);
return (ChargingState) chargeState;
if (data->isValidPacketID(ID_CHARGE_STATE)) {
uint8_t chargeState = GET_DATA(ID_CHARGE_STATE);
assert(chargeState >= 0);
assert(chargeState <= 5);
return (ChargingState) chargeState;
}
else {
CERR("[create::Create] ", "Charging state not supported!");
return CHARGE_FAULT;
}
}
bool Create::isCleanButtonPressed() const {
return (GET_DATA(ID_BUTTONS) & 0x01) != 0;
if (data->isValidPacketID(ID_BUTTONS)) {
return (GET_DATA(ID_BUTTONS) & 0x01) != 0;
}
else {
CERR("[create::Create] ", "Buttons not supported!");
return false;
}
}
// Not working
bool Create::isClockButtonPressed() const {
return (GET_DATA(ID_BUTTONS) & 0x80) != 0;
if (data->isValidPacketID(ID_BUTTONS)) {
return (GET_DATA(ID_BUTTONS) & 0x80) != 0;
}
else {
CERR("[create::Create] ", "Buttons not supported!");
return false;
}
}
// Not working
bool Create::isScheduleButtonPressed() const {
return (GET_DATA(ID_BUTTONS) & 0x40) != 0;
if (data->isValidPacketID(ID_BUTTONS)) {
return (GET_DATA(ID_BUTTONS) & 0x40) != 0;
}
else {
CERR("[create::Create] ", "Buttons not supported!");
return false;
}
}
bool Create::isDayButtonPressed() const {
return (GET_DATA(ID_BUTTONS) & 0x20) != 0;
if (data->isValidPacketID(ID_BUTTONS)) {
return (GET_DATA(ID_BUTTONS) & 0x20) != 0;
}
else {
CERR("[create::Create] ", "Buttons not supported!");
return false;
}
}
bool Create::isHourButtonPressed() const {
return (GET_DATA(ID_BUTTONS) & 0x10) != 0;
if (data->isValidPacketID(ID_BUTTONS)) {
return (GET_DATA(ID_BUTTONS) & 0x10) != 0;
}
else {
CERR("[create::Create] ", "Buttons not supported!");
return false;
}
}
bool Create::isMinButtonPressed() const {
return (GET_DATA(ID_BUTTONS) & 0x08) != 0;
if (data->isValidPacketID(ID_BUTTONS)) {
return (GET_DATA(ID_BUTTONS) & 0x08) != 0;
}
else {
CERR("[create::Create] ", "Buttons not supported!");
return false;
}
}
bool Create::isDockButtonPressed() const {
return (GET_DATA(ID_BUTTONS) & 0x04) != 0;
if (data->isValidPacketID(ID_BUTTONS)) {
return (GET_DATA(ID_BUTTONS) & 0x04) != 0;
}
else {
CERR("[create::Create] ", "Buttons not supported!");
return false;
}
}
bool Create::isSpotButtonPressed() const {
return (GET_DATA(ID_BUTTONS) & 0x02) != 0;
if (data->isValidPacketID(ID_BUTTONS)) {
return (GET_DATA(ID_BUTTONS) & 0x02) != 0;
}
else {
CERR("[create::Create] ", "Buttons not supported!");
return false;
}
}
uint16_t Create::getVoltage() const {
return GET_DATA(ID_VOLTAGE);
if (data->isValidPacketID(ID_VOLTAGE)) {
return GET_DATA(ID_VOLTAGE);
}
else {
CERR("[create::Create] ", "Voltage sensor not supported!");
return 0;
}
}
uint16_t Create::getCurrent() const {
return GET_DATA(ID_CURRENT);
if (data->isValidPacketID(ID_VOLTAGE)) {
return GET_DATA(ID_CURRENT);
}
else {
CERR("[create::Create] ", "Current sensor not supported!");
return 0;
}
}
uint8_t Create::getTemperature() const {
return GET_DATA(ID_TEMP);
if (data->isValidPacketID(ID_TEMP)) {
return GET_DATA(ID_TEMP);
}
else {
CERR("[create::Create] ", "Temperature sensor not supported!");
return 0;
}
}
uint16_t Create::getBatteryCharge() const {
return GET_DATA(ID_CHARGE);
if (data->isValidPacketID(ID_CHARGE)) {
return GET_DATA(ID_CHARGE);
}
else {
CERR("[create::Create] ", "Battery charge not supported!");
return 0;
}
}
uint16_t Create::getBatteryCapacity() const {
return GET_DATA(ID_CAPACITY);
if (data->isValidPacketID(ID_CAPACITY)) {
return GET_DATA(ID_CAPACITY);
}
else {
CERR("[create::Create] ", "Battery capacity not supported!");
return 0;
}
}
bool Create::isIRDetectLeft() const {
return (GET_DATA(ID_LIGHT) & 0x01) != 0;
if (data->isValidPacketID(ID_LIGHT)) {
return (GET_DATA(ID_LIGHT) & 0x01) != 0;
}
else {
CERR("[create::Create] ", "Light sensors not supported!");
return false;
}
}
bool Create::isIRDetectFrontLeft() const {
return (GET_DATA(ID_LIGHT) & 0x02) != 0;
if (data->isValidPacketID(ID_LIGHT)) {
return (GET_DATA(ID_LIGHT) & 0x02) != 0;
}
else {
CERR("[create::Create] ", "Light sensors not supported!");
return false;
}
}
bool Create::isIRDetectCenterLeft() const {
return (GET_DATA(ID_LIGHT) & 0x04) != 0;
if (data->isValidPacketID(ID_LIGHT)) {
return (GET_DATA(ID_LIGHT) & 0x04) != 0;
}
else {
CERR("[create::Create] ", "Light sensors not supported!");
return false;
}
}
bool Create::isIRDetectCenterRight() const {
return (GET_DATA(ID_LIGHT) & 0x08) != 0;
if (data->isValidPacketID(ID_LIGHT)) {
return (GET_DATA(ID_LIGHT) & 0x08) != 0;
}
else {
CERR("[create::Create] ", "Light sensors not supported!");
return false;
}
}
bool Create::isIRDetectFrontRight() const {
return (GET_DATA(ID_LIGHT) & 0x10) != 0;
if (data->isValidPacketID(ID_LIGHT)) {
return (GET_DATA(ID_LIGHT) & 0x10) != 0;
}
else {
CERR("[create::Create] ", "Light sensors not supported!");
return false;
}
}
bool Create::isIRDetectRight() const {
return (GET_DATA(ID_LIGHT) & 0x20) != 0;
if (data->isValidPacketID(ID_LIGHT)) {
return (GET_DATA(ID_LIGHT) & 0x20) != 0;
}
else {
CERR("[create::Create] ", "Light sensors not supported!");
return false;
}
}
bool Create::isMovingForward() const {
return GET_DATA(ID_STASIS) == 1;
if (data->isValidPacketID(ID_STASIS)) {
return GET_DATA(ID_STASIS) == 1;
}
else {
CERR("[create::Create] ", "Stasis sensor not supported!");
return false;
}
}
const Pose& Create::getPose() const {

84
src/data.cpp
View file

@ -4,7 +4,7 @@
namespace create {
Data::Data() {
Data::Data(RobotModel model) {
// Populate data map
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
* WARNING: Adding too many packets will flood the serial *
@ -16,52 +16,57 @@ namespace create {
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_VIRTUAL_WALL, 1, "virtual_wall");
//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_IR_OMNI, 1, "ir_opcode");
ADD_PACKET(ID_BUTTONS, 1, "buttons");
//ADD_PACKET(ID_DISTANCE, 2, "distance");
//ADD_PACKET(ID_ANGLE, 2, "angle");
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_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_IO_MODE, 1, "oi_mode");
//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_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");
//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 (model == CREATE_1) {
ADD_PACKET(ID_DISTANCE, 2, "distance");
ADD_PACKET(ID_ANGLE, 2, "angle");
}
else if (model == CREATE_2) {
//ADD_PACKET(ID_VIRTUAL_WALL, 1, "virtual_wall");
//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_IO_MODE, 1, "oi_mode");
//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_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");
//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
totalDataBytes = 0;
for (std::map<uint8_t, boost::shared_ptr<Packet> >::iterator it = packets.begin();
@ -85,6 +90,7 @@ namespace create {
if (isValidPacketID(id)) {
return packets[id];
}
std::cout << "Invalid packet " << (int) id << " requested" << std::endl;
return boost::shared_ptr<Packet>();
}

2
src/serial.cpp
View file

@ -26,6 +26,8 @@ namespace create {
port.set_option(serial_port::baud_rate(baud));
port.set_option(serial_port::flow_control(serial_port::flow_control::none));
usleep(1000000);
if (port.is_open()) {
callback = cb;
bool startReadSuccess = startReading();