Add support for first generation Create (Roomba 400 series)

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

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();
 

include/create/data.h

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

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,

src/create.cpp

@@ -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) {
+      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,7 +64,14 @@ namespace create {
     // Get current time
     util::timestamp_t curTime = util::getTimestamp();
     float dt = (curTime - prevOnDataTime) / 1000000.0;
-
+    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);
+    }
+    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);
@@ -87,42 +96,41 @@ namespace create {
                               * util::CREATE_2_WHEEL_DIAMETER * util::PI;
 
       float wheelDistDiff = rightWheelDist - leftWheelDist;
-    float deltaDist = (rightWheelDist + leftWheelDist) / 2.0;
+      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);
-      vel.yaw = 0;
+        deltaYaw = 0.0;
+        //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;
+        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 {
-        vel.yaw = 0;
-      }
       }
+    } // 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.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;
+
     // Make user registered callbacks, if any
     // TODO
   }
@@ -370,132 +378,315 @@ namespace create {
   }
 
   bool Create::isWheeldrop() const {
+    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 {
+    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 {
+    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 {
+    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 {
+    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 {
+    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 {
+    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 {
+    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 {
+    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 {
+    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 {
+    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 {
+    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 {
+    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 {
+    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 {
+    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 {
+    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 {
+    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 {
+    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 {
+    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 {
+    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 {
+    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 {
+    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 {
+    if (data->isValidPacketID(ID_CAPACITY)) {
       return GET_DATA(ID_CAPACITY);
     }
+    else {
+      CERR("[create::Create] ", "Battery capacity not supported!");
+      return 0;
+    }
+  }
 
   bool Create::isIRDetectLeft() const {
+    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 {
+    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 {
+    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 {
+    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 {
+    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 {
+    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 {
+    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 {
     return pose;

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,20 +16,26 @@ 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");
+
+    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");
@@ -62,6 +68,7 @@ namespace create {
       //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 +92,7 @@ namespace create {
     if (isValidPacketID(id)) {
       return packets[id];
     }
+    std::cout << "Invalid packet " << (int) id << " requested" << std::endl;
     return boost::shared_ptr<Packet>();
   }
 

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();