From f0734586243dc1823fc1ac4264ec0f1b85f178f6 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Mon, 14 Mar 2016 20:04:34 -0700 Subject: [PATCH] Add support for first generation Create (Roomba 400 series) --- examples/create_demo.cpp | 14 +- include/create/create.h | 6 +- include/create/data.h | 2 +- include/create/types.h | 5 + src/create.cpp | 365 +++++++++++++++++++++++++++++---------- src/data.cpp | 86 ++++----- src/serial.cpp | 2 + 7 files changed, 348 insertions(+), 132 deletions(-) diff --git a/examples/create_demo.cpp b/examples/create_demo.cpp index 9d4cd86..91a7369 100644 --- a/examples/create_demo.cpp +++ b/examples/create_demo.cpp @@ -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); diff --git a/include/create/create.h b/include/create/create.h index 31c9911..d9d00e0 100644 --- a/include/create/create.h +++ b/include/create/create.h @@ -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(); diff --git a/include/create/data.h b/include/create/data.h index 93864f4..02728f8 100644 --- a/include/create/data.h +++ b/include/create/data.h @@ -48,7 +48,7 @@ namespace create { std::vector ids; public: - Data(); + Data(RobotModel = CREATE_2); ~Data(); bool isValidPacketID(const uint8_t id) const; diff --git a/include/create/types.h b/include/create/types.h index 6888389..3fcb3b1 100644 --- a/include/create/types.h +++ b/include/create/types.h @@ -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, diff --git a/src/create.cpp b/src/create.cpp index 333184c..98603e4 100644 --- a/src/create.cpp +++ b/src/create.cpp @@ -33,15 +33,15 @@ namespace create { vel.x = 0; vel.y = 0; vel.yaw = 0; - data = boost::shared_ptr(new Data()); + data = boost::shared_ptr(new Data(model)); serial = boost::make_shared(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); + 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 { diff --git a/src/data.cpp b/src/data.cpp index a213ca6..e17e803 100644 --- a/src/data.cpp +++ b/src/data.cpp @@ -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,59 @@ 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"); + std::cout << "Adding Create 1 packets" << std::endl; + } + else if (model == CREATE_2) { + std::cout << "Adding Create 2 packets" << std::endl; + //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 >::iterator it = packets.begin(); @@ -85,6 +92,7 @@ namespace create { if (isValidPacketID(id)) { return packets[id]; } + std::cout << "Invalid packet " << (int) id << " requested" << std::endl; return boost::shared_ptr(); } diff --git a/src/serial.cpp b/src/serial.cpp index 4c98111..62206f5 100644 --- a/src/serial.cpp +++ b/src/serial.cpp @@ -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();