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) { int main(int argc, char** argv) {
std::string port = "/dev/ttyUSB0"; 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 // Attempt to connect to Create
if (robot->connect(port, 115200)) if (robot->connect(port, baud))
std::cout << "Successfully connected to Create" << std::endl; std::cout << "Successfully connected to Create" << std::endl;
else { else {
std::cout << "Failed to connect to Create on port " << port.c_str() << std::endl; 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++) { //for (int i = 0; i < songLength; i++) {
// durations[i] = 0.25; // durations[i] = 0.25;
//} //}
//robot->createSong(0, songLength, notes, durations); //robot->defineSong(0, songLength, notes, durations);
//usleep(1000000); //usleep(1000000);
//robot->playSong(0); //robot->playSong(0);

6
include/create/create.h
View file

@ -51,6 +51,8 @@ namespace create {
LED_CHECK = 8 LED_CHECK = 8
}; };
RobotModel model;
uint8_t mainMotorPower; uint8_t mainMotorPower;
uint8_t sideMotorPower; uint8_t sideMotorPower;
uint8_t vacuumMotorPower; uint8_t vacuumMotorPower;
@ -85,11 +87,11 @@ namespace create {
/* Default constructor. /* Default constructor.
* Does not attempt to establish serial connection to Create. * Does not attempt to establish serial connection to Create.
*/ */
Create(); Create(RobotModel = CREATE_2);
/* Attempts to establish serial connection to Create. /* 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(); ~Create();

2
include/create/data.h
View file

@ -48,7 +48,7 @@ namespace create {
std::vector<uint8_t> ids; std::vector<uint8_t> ids;
public: public:
Data(); Data(RobotModel = CREATE_2);
~Data(); ~Data();
bool isValidPacketID(const uint8_t id) const; 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 #define CREATE_TYPES_H
namespace create { namespace create {
enum RobotModel {
CREATE_1 = 0, // Roomba 400 series
CREATE_2 // Roomba 600 series
};
enum SensorPacketID { enum SensorPacketID {
ID_GROUP_0 = 0, ID_GROUP_0 = 0,
ID_GROUP_1 = 1, ID_GROUP_1 = 1,

365
src/create.cpp
View file

@ -33,15 +33,15 @@ namespace create {
vel.x = 0; vel.x = 0;
vel.y = 0; vel.y = 0;
vel.yaw = 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); serial = boost::make_shared<Serial>(data);
} }
Create::Create() { Create::Create(RobotModel m) : model(m) {
init(); init();
} }
Create::Create(const std::string& dev, const int& baud) { Create::Create(const std::string& dev, const int& baud, RobotModel m) : model(m) {
init(); init();
serial->connect(dev, baud); serial->connect(dev, baud);
} }
@ -52,9 +52,11 @@ namespace create {
void Create::onData() { void Create::onData() {
if (firstOnData) { if (firstOnData) {
// Initialize tick counts if (model == CREATE_2) {
prevTicksLeft = GET_DATA(ID_LEFT_ENC); // Initialize tick counts
prevTicksRight = GET_DATA(ID_RIGHT_ENC); prevTicksLeft = GET_DATA(ID_LEFT_ENC);
prevTicksRight = GET_DATA(ID_RIGHT_ENC);
}
prevOnDataTime = util::getTimestamp(); prevOnDataTime = util::getTimestamp();
firstOnData = false; firstOnData = false;
} }
@ -62,67 +64,73 @@ namespace create {
// Get current time // Get current time
util::timestamp_t curTime = util::getTimestamp(); util::timestamp_t curTime = util::getTimestamp();
float dt = (curTime - prevOnDataTime) / 1000000.0; float dt = (curTime - prevOnDataTime) / 1000000.0;
float deltaDist, deltaX, deltaY, deltaYaw;
// Get cumulative ticks (wraps around at 65535) if (model == CREATE_1) {
uint16_t totalTicksLeft = GET_DATA(ID_LEFT_ENC); deltaDist = GET_DATA(ID_DISTANCE) / 1000.0; //mm -> m
uint16_t totalTicksRight = GET_DATA(ID_RIGHT_ENC); deltaYaw = GET_DATA(ID_ANGLE) * util::PI / 180.0; // D2R
// 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) {
deltaX = deltaDist * cos(pose.yaw); deltaX = deltaDist * cos(pose.yaw);
deltaY = deltaDist * sin(pose.yaw); deltaY = deltaDist * sin(pose.yaw);
vel.yaw = 0;
} }
else { else if (model == CREATE_2) {
float turnRadius = (util::CREATE_2_AXLE_LENGTH / 2.0) * (leftWheelDist + rightWheelDist) / wheelDistDiff; // Get cumulative ticks (wraps around at 65535)
float deltaYaw = (rightWheelDist - leftWheelDist) / util::CREATE_2_AXLE_LENGTH; uint16_t totalTicksLeft = GET_DATA(ID_LEFT_ENC);
deltaX = turnRadius * (sin(pose.yaw + deltaYaw) - sin(pose.yaw)); uint16_t totalTicksRight = GET_DATA(ID_RIGHT_ENC);
deltaY = turnRadius * (cos(pose.yaw + deltaYaw) - cos(pose.yaw)); // Compute ticks since last update
pose.yaw = util::normalizeAngle(pose.yaw + deltaYaw); int ticksLeft = totalTicksLeft - prevTicksLeft;
if (fabs(dt) > util::EPS) { int ticksRight = totalTicksRight - prevTicksRight;
vel.yaw = deltaYaw / dt; 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 { 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) { if (fabs(dt) > util::EPS) {
vel.x = deltaX / dt; vel.x = deltaX / dt;
vel.y = deltaY / dt; vel.y = deltaY / dt;
vel.yaw = deltaYaw / dt;
} }
else { else {
vel.x = 0; vel.x = 0.0;
vel.y = 0; vel.y = 0.0;
vel.yaw = 0.0;
} }
pose.x += deltaDist * cos(pose.yaw); pose.x += deltaX;
pose.y += deltaDist * sin(pose.yaw); pose.y += deltaY;
pose.yaw = util::normalizeAngle(pose.yaw + deltaYaw);
prevOnDataTime = curTime; prevOnDataTime = curTime;
// Make user registered callbacks, if any // Make user registered callbacks, if any
// TODO // TODO
} }
@ -370,131 +378,314 @@ namespace create {
} }
bool Create::isWheeldrop() const { 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 { 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 { 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 { 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 { bool Create::isCliff() const {
return GET_DATA(ID_CLIFF_LEFT) == 1 || if (data->isValidPacketID(ID_CLIFF_LEFT) &&
GET_DATA(ID_CLIFF_FRONT_LEFT) == 1 || data->isValidPacketID(ID_CLIFF_FRONT_LEFT) &&
GET_DATA(ID_CLIFF_FRONT_RIGHT) == 1 || data->isValidPacketID(ID_CLIFF_FRONT_RIGHT) &&
GET_DATA(ID_CLIFF_RIGHT) == 1; 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 { 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 { 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 { 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 { 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 { ChargingState Create::getChargingState() const {
uint8_t chargeState = GET_DATA(ID_CHARGE_STATE); if (data->isValidPacketID(ID_CHARGE_STATE)) {
assert(chargeState >= 0); uint8_t chargeState = GET_DATA(ID_CHARGE_STATE);
assert(chargeState <= 5); assert(chargeState >= 0);
return (ChargingState) chargeState; assert(chargeState <= 5);
return (ChargingState) chargeState;
}
else {
CERR("[create::Create] ", "Charging state not supported!");
return CHARGE_FAULT;
}
} }
bool Create::isCleanButtonPressed() const { 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 // Not working
bool Create::isClockButtonPressed() const { 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 // Not working
bool Create::isScheduleButtonPressed() const { 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 { 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 { 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 { 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 { 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 { 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 { 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 { 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 { 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 { 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 { 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 { 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 { 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 { 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 { 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 { 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 { 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 { 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 { const Pose& Create::getPose() const {

84
src/data.cpp
View file

@ -4,7 +4,7 @@
namespace create { namespace create {
Data::Data() { Data::Data(RobotModel model) {
// Populate data map // Populate data map
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
* WARNING: Adding too many packets will flood the serial * * 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_LEFT, 1, "cliff_front_left");
ADD_PACKET(ID_CLIFF_FRONT_RIGHT, 1, "cliff_front_right"); ADD_PACKET(ID_CLIFF_FRONT_RIGHT, 1, "cliff_front_right");
ADD_PACKET(ID_CLIFF_RIGHT, 1, "cliff_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_IR_OMNI, 1, "ir_opcode");
ADD_PACKET(ID_BUTTONS, 1, "buttons"); 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_CHARGE_STATE, 1, "charging_state");
ADD_PACKET(ID_VOLTAGE, 2, "voltage"); ADD_PACKET(ID_VOLTAGE, 2, "voltage");
ADD_PACKET(ID_CURRENT, 2, "current"); ADD_PACKET(ID_CURRENT, 2, "current");
ADD_PACKET(ID_TEMP, 1, "temperature"); ADD_PACKET(ID_TEMP, 1, "temperature");
ADD_PACKET(ID_CHARGE , 2, "battery_charge"); ADD_PACKET(ID_CHARGE , 2, "battery_charge");
ADD_PACKET(ID_CAPACITY, 2, "battery_capacity"); 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"); if (model == CREATE_1) {
//ADD_PACKET(ID_CLIFF_FRONT_LEFT_SIGNAL, 2, "cliff_front_left_signal"); ADD_PACKET(ID_DISTANCE, 2, "distance");
//ADD_PACKET(ID_CLIFF_FRONT_RIGHT_SIGNAL, 2, "cliff_front_right_signal"); ADD_PACKET(ID_ANGLE, 2, "angle");
//ADD_PACKET(ID_CLIFF_RIGHT_SIGNAL, 2, "cliff_right_signal"); }
//ADD_PACKET(ID_UNUSED_2, 1, "unused 2"); else if (model == CREATE_2) {
//ADD_PACKET(ID_UNUSED_3, 2, "unused 3"); //ADD_PACKET(ID_VIRTUAL_WALL, 1, "virtual_wall");
//ADD_PACKET(ID_CHARGE_SOURCE, 1, "charger_available"); //ADD_PACKET(ID_OVERCURRENTS, 1, "overcurrents");
//ADD_PACKET(ID_IO_MODE, 1, "oi_mode"); ADD_PACKET(ID_DIRT_DETECT, 1, "dirt_detect");
//ADD_PACKET(ID_SONG_NUM, 1, "song_number"); //ADD_PACKET(ID_UNUSED_1, 1, "unused 1");
//ADD_PACKET(ID_PLAYING, 1, "song_playing"); //ADD_PACKET(ID_WALL_SIGNAL, 2, "wall_signal");
//ADD_PACKET(ID_NUM_STREAM_PACKETS, 1, "oi_stream_num_packets"); //ADD_PACKET(ID_CLIFF_LEFT_SIGNAL, 2, "cliff_left_signal");
//ADD_PACKET(ID_VEL, 2, "velocity"); //ADD_PACKET(ID_CLIFF_FRONT_LEFT_SIGNAL, 2, "cliff_front_left_signal");
//ADD_PACKET(ID_RADIUS, 2, "radius"); //ADD_PACKET(ID_CLIFF_FRONT_RIGHT_SIGNAL, 2, "cliff_front_right_signal");
//ADD_PACKET(ID_RIGHT_VEL, 2, "velocity_right"); //ADD_PACKET(ID_CLIFF_RIGHT_SIGNAL, 2, "cliff_right_signal");
//ADD_PACKET(ID_LEFT_VEL, 2, "velocity_left"); //ADD_PACKET(ID_UNUSED_2, 1, "unused 2");
ADD_PACKET(ID_LEFT_ENC, 2, "enc_counts_left"); //ADD_PACKET(ID_UNUSED_3, 2, "unused 3");
ADD_PACKET(ID_RIGHT_ENC, 2, "enc_counts_right"); //ADD_PACKET(ID_CHARGE_SOURCE, 1, "charger_available");
ADD_PACKET(ID_LIGHT, 1, "light_bumper"); //ADD_PACKET(ID_IO_MODE, 1, "oi_mode");
//ADD_PACKET(ID_LIGHT_LEFT, 2, "light_bumper_left"); //ADD_PACKET(ID_SONG_NUM, 1, "song_number");
//ADD_PACKET(ID_LIGHT_FRONT_LEFT, 2, "light_bumper_front_left"); //ADD_PACKET(ID_PLAYING, 1, "song_playing");
//ADD_PACKET(ID_LIGHT_CENTER_LEFT, 2, "light_bumper_center_left"); //ADD_PACKET(ID_NUM_STREAM_PACKETS, 1, "oi_stream_num_packets");
//ADD_PACKET(ID_LIGHT_CENTER_RIGHT, 2, "light_bumper_center_right"); //ADD_PACKET(ID_VEL, 2, "velocity");
//ADD_PACKET(ID_LIGHT_FRONT_RIGHT, 2, "light_bumper_front_right"); //ADD_PACKET(ID_RADIUS, 2, "radius");
//ADD_PACKET(ID_LIGHT_RIGHT, 2, "light_bumper_right"); //ADD_PACKET(ID_RIGHT_VEL, 2, "velocity_right");
ADD_PACKET(ID_IR_LEFT, 1, "ir_opcode_left"); //ADD_PACKET(ID_LEFT_VEL, 2, "velocity_left");
ADD_PACKET(ID_IR_RIGHT, 1, "ir_opcode_right"); ADD_PACKET(ID_LEFT_ENC, 2, "enc_counts_left");
//ADD_PACKET(ID_LEFT_MOTOR_CURRENT, 2, "left_motor_current"); ADD_PACKET(ID_RIGHT_ENC, 2, "enc_counts_right");
//ADD_PACKET(ID_RIGHT_MOTOR_CURRENT, 2, "right_motor_current"); ADD_PACKET(ID_LIGHT, 1, "light_bumper");
//ADD_PACKET(ID_MAIN_BRUSH_CURRENT, 2, "main_brush_current"); //ADD_PACKET(ID_LIGHT_LEFT, 2, "light_bumper_left");
//ADD_PACKET(ID_SIDE_BRUSH_CURRENT, 2, "side_brush_current"); //ADD_PACKET(ID_LIGHT_FRONT_LEFT, 2, "light_bumper_front_left");
ADD_PACKET(ID_STASIS, 1, "stasis"); //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; totalDataBytes = 0;
for (std::map<uint8_t, boost::shared_ptr<Packet> >::iterator it = packets.begin(); for (std::map<uint8_t, boost::shared_ptr<Packet> >::iterator it = packets.begin();
@ -85,6 +90,7 @@ namespace create {
if (isValidPacketID(id)) { if (isValidPacketID(id)) {
return packets[id]; return packets[id];
} }
std::cout << "Invalid packet " << (int) id << " requested" << std::endl;
return boost::shared_ptr<Packet>(); 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::baud_rate(baud));
port.set_option(serial_port::flow_control(serial_port::flow_control::none)); port.set_option(serial_port::flow_control(serial_port::flow_control::none));
usleep(1000000);
if (port.is_open()) { if (port.is_open()) {
callback = cb; callback = cb;
bool startReadSuccess = startReading(); bool startReadSuccess = startReading();