Initial commit.

src/create.cpp

@@ -0,0 +1,475 @@
+#include <boost/bind.hpp>
+#include <boost/make_shared.hpp>
+#include <iostream>
+#include <cmath> 
+#include <ctime>
+#include <assert.h>
+
+#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))
+
+namespace create {
+
+  // TODO: Handle SIGINT to do clean disconnect
+
+  void Create::init() {
+    mainMotorPower = 0;
+    sideMotorPower = 0;
+    vacuumMotorPower = 0;
+    debrisLED = 0;
+    spotLED = 0;
+    dockLED = 0;
+    checkLED = 0;
+    powerLED = 0;
+    powerLEDIntensity = 0;
+    prevTicksLeft = 0;
+    prevTicksRight = 0;
+    firstOnData = true;
+    pose.x = 0;
+    pose.y = 0;
+    pose.yaw = 0;
+    data = boost::shared_ptr<Data>(new Data());
+    serial = boost::make_shared<Serial>(data);
+  }
+
+  Create::Create() {
+    init();
+  }
+
+  Create::Create(const std::string& dev, const int& baud) {
+    init();
+    serial->connect(dev, baud);
+  }
+  
+  Create::~Create() {
+    disconnect();
+  }
+  
+  void Create::onData() {
+    if (firstOnData) {
+      // Initialize tick counts
+      prevTicksLeft = GET_DATA(ID_LEFT_ENC);
+      prevTicksRight = GET_DATA(ID_RIGHT_ENC);
+      firstOnData = false;
+    }
+
+    // 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
+    if (fabs(wheelDistDiff) < util::EPS) {
+      pose.x += deltaDist * cos(pose.yaw);
+      pose.y += deltaDist * sin(pose.yaw);
+    }
+    else {
+      float turnRadius = (util::CREATE_2_AXLE_LENGTH / 2.0) * (leftWheelDist + rightWheelDist) / wheelDistDiff;
+      float deltaYaw = (rightWheelDist - leftWheelDist) / util::CREATE_2_AXLE_LENGTH; 
+      pose.x += turnRadius * (sin(pose.yaw + deltaYaw) - sin(pose.yaw));
+      pose.y += turnRadius * (cos(pose.yaw + deltaYaw) - cos(pose.yaw));
+      pose.yaw = util::normalizeAngle(pose.yaw + deltaYaw);
+    }
+  
+    // Make user registered callbacks, if any
+    // TODO
+  }
+  
+  bool Create::connect(const std::string& port, const int& baud) {
+    bool timeout = false;
+    time_t start, now;
+    float maxWait = 30; // seconds
+    float retryInterval = 5; //seconds
+    time(&start);
+    while (!serial->connect(port, baud, boost::bind(&Create::onData, this)) && !timeout) {
+      time(&now);
+      if (difftime(now, start) > maxWait) {
+        timeout = true;
+        CERR("[create::Create] ", "failed to connect over serial: timeout");
+      }
+      else {
+        usleep(retryInterval * 1000000);
+        COUT("[create::Create] ", "retrying to establish serial connection...");
+      }
+    }
+  
+    return !timeout;
+  }
+  
+  void Create::disconnect() {
+    serial->disconnect();
+    firstOnData = true;
+  }
+  
+  //void Create::reset() {
+  //  serial->sendOpcode(OC_RESET);
+  //  serial->reset(); // better
+    // TODO : Should we request reading packets again?
+  //}
+  
+  bool Create::setMode(const CreateMode& mode) {
+    return serial->sendOpcode((Opcode) mode);
+  }
+  
+  bool Create::clean(const CleanMode& mode) {
+    return serial->sendOpcode((Opcode) mode);
+  }
+  
+  bool Create::dock() const {
+    return serial->sendOpcode(OC_DOCK);
+  }
+  
+  bool Create::setDate(const DayOfWeek& day, const uint8_t& hour, const uint8_t& min) const {
+    if (day < 0 || day > 6 ||
+        hour < 0 || hour > 23 ||
+        min < 0 || min > 59)
+      return false;
+
+    uint8_t cmd[4] = { OC_DATE, day, hour, min };
+    return serial->send(cmd, 4);
+  }
+  
+  /*void Create::driveRadius(const float& vel, const float& radius) const {
+    // Expects each parameter as two bytes each and in millimeters
+    int16_t vel_mm = roundf(vel * 1000);
+    int16_t radius_mm = roundf(radius * 1000);
+    BOUND(vel_mm, -500, 500);
+
+    // Consider special cases for radius
+    if (radius_mm != 32768 && radius_mm != 32767 &&
+        radius_mm != -1 && radius_mm != 1) {
+      BOUND(radius_mm, -2000, 2000);
+    }
+
+    uint8_t cmd[5] = { OC_DRIVE,
+                       vel_mm >> 8,
+                       vel_mm & 0xff,
+                       radius_mm >> 8,
+                       radius_mm & 0xff
+                     };
+  
+    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);
+
+    uint8_t cmd[5] = { OC_DRIVE_DIRECT,
+                       rightCmd >> 8,
+                       rightCmd & 0xff,
+                       leftCmd >> 8,
+                       leftCmd & 0xff
+                     };
+    return serial->send(cmd, 5);
+  }
+  
+  /*void Create::drivePWM(const int16_t& leftPWM, const int16_t& rightPWM) const {
+    uint8_t cmd[5] = { OC_DRIVE_PWM,
+                       rightPWM >> 8,
+                       rightPWM & 0xff,
+                       leftPWM >> 8,
+                       leftPWM & 0xff
+                     };
+    serial->send(cmd, 5);
+  }
+  */
+  
+  bool Create::drive(const float& xVel, const float& angularVel) const {
+    // 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);
+    return driveWheels(leftVel, rightVel);
+  }
+  
+  bool Create::setAllMotors(const float& main, const float& side, const float& vacuum) {
+    if (main < -1.0 || main > 1.0 ||
+        side < -1.0 || side > 1.0 ||
+        vacuum < -1.0 || vacuum > 1.0)
+      return false;    
+
+    mainMotorPower = roundf(main * 127);
+    sideMotorPower = roundf(side * 127);
+    vacuumMotorPower = roundf(vacuum * 127);
+  
+    uint8_t cmd[4] = { OC_MOTORS_PWM,
+                        mainMotorPower,
+                        sideMotorPower,
+                        vacuumMotorPower
+                      };
+  
+    return serial->send(cmd, 4);
+  }
+  
+  bool Create::setMainMotor(const float& main) {
+    return setAllMotors(main, sideMotorPower, vacuumMotorPower);
+  }
+  
+  bool Create::setSideMotor(const float& side) {
+    return setAllMotors(mainMotorPower, side, vacuumMotorPower);
+  }
+  
+  bool Create::setVacuumMotor(const float& vacuum) {
+    return setAllMotors(mainMotorPower, sideMotorPower, vacuum);
+  }
+  
+  bool Create::updateLEDs() {
+    uint8_t LEDByte = debrisLED + spotLED + dockLED + checkLED;
+    uint8_t cmd[4] = { OC_LEDS,
+                        LEDByte,
+                        powerLED,
+                        powerLEDIntensity
+                      };
+  
+    return serial->send(cmd, 4);
+  }
+  
+  bool Create::enableDebrisLED(const bool& enable) {
+    if (enable)
+      debrisLED = LED_DEBRIS;
+    else
+      debrisLED = 0;
+    return updateLEDs();
+  }
+  
+  bool Create::enableSpotLED(const bool& enable) {
+    if (enable)
+      spotLED = LED_SPOT;
+    else
+      spotLED = 0;
+    return updateLEDs();
+  }
+  
+  bool Create::enableDockLED(const bool& enable) {
+    if (enable)
+      dockLED = LED_DOCK;
+    else
+      dockLED = 0;
+    return updateLEDs();
+  }
+  
+  bool Create::enableCheckRobotLED(const bool& enable) {
+    if (enable)
+      checkLED = LED_CHECK;
+    else
+      checkLED = 0;
+    return updateLEDs();
+  }
+  
+  bool Create::setPowerLED(const uint8_t& power, const uint8_t& intensity) {
+    powerLED = power;
+    powerLEDIntensity = intensity;
+    return updateLEDs();
+  }
+  
+  //void Create::setDigits(uint8_t digit1, uint8_t digit2,
+  //                       uint8_t digit3, uint8_t digit4) {
+  //}
+  
+  bool Create::setDigitsASCII(const uint8_t& digit1, const uint8_t& digit2,
+                              const uint8_t& digit3, const uint8_t& digit4) const {
+    if (digit1 < 32 || digit1 > 126 ||
+        digit2 < 32 || digit2 > 126 ||  
+        digit3 < 32 || digit3 > 126 ||  
+        digit4 < 32 || digit4 > 126)
+      return false;
+
+    uint8_t cmd[5] = { OC_DIGIT_LEDS_ASCII,
+                        digit1,
+                        digit2,
+                        digit3,
+                        digit4
+                      };
+  
+    return serial->send(cmd, 5);
+  }
+  
+  bool Create::defineSong(const uint8_t& songNumber,
+                          const uint8_t& songLength,
+                          const uint8_t* notes,
+                          const float* durations) const {
+    int i, j;
+    uint8_t duration;
+    uint8_t cmd[2 * songLength + 3];
+    cmd[0] = OC_SONG;
+    cmd[1] = songNumber;
+    cmd[2] = songLength;
+    j = 0;
+    for (i = 3; i < 2 * songLength + 3; i = i + 2) {
+      if (durations[j] < 0 || durations[j] >= 4)
+        return false;
+      duration = durations[j] * 64;
+      cmd[i] = notes[j];
+      cmd[i + 1] = duration;
+      j++;
+    }
+  
+    return serial->send(cmd, 2 * songLength + 3);
+  }
+   
+  bool Create::playSong(const uint8_t& songNumber) const {
+    if (songNumber < 0 || songNumber > 4)
+      return false;
+    uint8_t cmd[2] = { OC_PLAY, songNumber };
+    return serial->send(cmd, 2);
+  }
+  
+  bool Create::isWheeldrop() const {
+    return (GET_DATA(ID_BUMP_WHEELDROP) & 0x0C) != 0;
+  }
+  
+  bool Create::isLeftBumper() const {
+    return (GET_DATA(ID_BUMP_WHEELDROP) & 0x02) != 0;
+  }
+  
+  bool Create::isRightBumper() const {
+    return (GET_DATA(ID_BUMP_WHEELDROP) & 0x01) != 0;
+  }
+  
+  bool Create::isWall() const {
+    return GET_DATA(ID_WALL) == 1;
+  }
+  
+  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;
+  }
+  
+  uint8_t Create::getDirtDetect() const {
+    return GET_DATA(ID_DIRT_DETECT);
+  }
+  
+  uint8_t Create::getIROmni() const {
+    return GET_DATA(ID_IR_OMNI);
+  }
+
+  uint8_t Create::getIRLeft() const {
+    return GET_DATA(ID_IR_LEFT);
+  }
+
+  uint8_t Create::getIRRight() const {
+    return GET_DATA(ID_IR_RIGHT);
+  }
+  
+  ChargingState Create::getChargingState() const {
+    uint8_t chargeState = GET_DATA(ID_CHARGE_STATE);
+    assert(chargeState >= 0);
+    assert(chargeState <= 5);
+    return (ChargingState) chargeState;
+  }
+
+  bool Create::isCleanButtonPressed() const {
+    return (GET_DATA(ID_BUTTONS) & 0x01) != 0;
+  }
+  
+  // Not working. TODO Fix/report
+  bool Create::isClockButtonPressed() const {
+    return (GET_DATA(ID_BUTTONS) & 0x80) != 0;
+  }
+
+  // Not working. TODO Fix/report
+  bool Create::isScheduleButtonPressed() const {
+    return (GET_DATA(ID_BUTTONS) & 0x40) != 0;
+  }
+
+  bool Create::isDayButtonPressed() const {
+    return (GET_DATA(ID_BUTTONS) & 0x20) != 0;
+  }
+
+  bool Create::isHourButtonPressed() const {
+    return (GET_DATA(ID_BUTTONS) & 0x10) != 0;
+  }
+
+  bool Create::isMinButtonPressed() const {
+    return (GET_DATA(ID_BUTTONS) & 0x08) != 0;
+  }
+
+  bool Create::isDockButtonPressed() const {
+    return (GET_DATA(ID_BUTTONS) & 0x04) != 0;
+  }
+
+  bool Create::isSpotButtonPressed() const {
+    return (GET_DATA(ID_BUTTONS) & 0x02) != 0;
+  }
+  
+  uint16_t Create::getVoltage() const {
+    return GET_DATA(ID_VOLTAGE);
+  }
+  
+  uint16_t Create::getCurrent() const {
+    return GET_DATA(ID_CURRENT);
+  }
+  
+  uint8_t Create::getTemperature() const {
+    return GET_DATA(ID_TEMP);
+  }
+  
+  uint16_t Create::getBatteryCharge() const {
+    return GET_DATA(ID_CHARGE);
+  }
+  
+  uint16_t Create::getBatteryCapacity() const {
+    return GET_DATA(ID_CAPACITY);
+  }
+  
+  bool Create::isIRDetectLeft() const {
+    return (GET_DATA(ID_LIGHT) & 0x01) != 0;
+  }
+ 
+  bool Create::isIRDetectFrontLeft() const { 
+    return (GET_DATA(ID_LIGHT) & 0x02) != 0;
+  }
+
+  bool Create::isIRDetectCenterLeft() const { 
+    return (GET_DATA(ID_LIGHT) & 0x04) != 0;
+  }
+
+  bool Create::isIRDetectCenterRight() const { 
+    return (GET_DATA(ID_LIGHT) & 0x08) != 0;
+  }
+
+  bool Create::isIRDetectFrontRight() const { 
+    return (GET_DATA(ID_LIGHT) & 0x10) != 0;
+  }
+
+  bool Create::isIRDetectRight() const { 
+    return (GET_DATA(ID_LIGHT) & 0x20) != 0;
+  }
+
+  bool Create::isMovingForward() const {
+    return GET_DATA(ID_STASIS) == 1;
+  }
+  
+  const Pose& Create::getPose() const {
+    return pose;
+  }
+
+} // end namespace

src/data.cpp

@@ -0,0 +1,107 @@
+#include "create/data.h"
+
+#define ADD_PACKET(id,nbytes,info) (packets[id]=boost::make_shared<Packet>(nbytes,info))
+
+namespace create {
+  
+  Data::Data() {
+    // Populate data map
+    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_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");
+    
+    totalDataBytes = 0;
+    for (std::map<uint8_t, boost::shared_ptr<Packet> >::iterator it = packets.begin();
+         it != packets.end();
+         ++it) {
+      ids.push_back(it->first);
+      totalDataBytes += it->second->nbytes;
+    }
+  }
+  
+  Data::~Data() { }
+  
+  bool Data::isValidPacketID(uint8_t id) const {
+    if (packets.count(id)) {
+      return true;
+    }
+    return false;
+  }
+  
+  boost::shared_ptr<Packet> Data::getPacket(uint8_t id) {
+    if (isValidPacketID(id)) {
+      return packets[id];
+    }
+    return boost::shared_ptr<Packet>(); //NULL;
+  }
+  
+  void Data::validateAll() {
+    for (std::map<uint8_t, boost::shared_ptr<Packet> >::iterator it = packets.begin();
+         it != packets.end();
+         ++it) {
+      it->second->validate();
+    }
+  }
+  
+  unsigned int Data::getTotalDataBytes() const {
+    return totalDataBytes;
+  }
+  
+  uint8_t Data::getNumPackets() const {
+    return packets.size();
+  }
+  
+  std::vector<uint8_t> Data::getPacketIDs() {
+    return ids;
+  }
+
+} // namespace create

src/packet.cpp

@@ -0,0 +1,32 @@
+#include "create/packet.h"
+
+namespace create {
+
+  Packet::Packet(const uint8_t& numBytes, const std::string& comment) : 
+    nbytes(numBytes),
+    info(comment),
+    data(0),
+    tmpData(0) { }
+  
+  Packet::~Packet() { }
+  
+  void Packet::setTempData(const uint16_t& tmp) {
+    // mutex for tmpData ?
+    tmpData = tmp;
+  }
+  
+  void Packet::validate() {
+    setData(tmpData);
+  }
+  
+  void Packet::setData(const uint16_t& d) {
+    boost::mutex::scoped_lock lock(dataMutex);
+    data = d;
+  }
+  
+  uint16_t Packet::getData() const {
+    boost::mutex::scoped_lock lock(dataMutex);
+    return data;
+  }
+
+} // namespace create

src/serial.cpp

@@ -0,0 +1,255 @@
+#include <iostream>
+
+#include "create/serial.h"
+#include "create/types.h"
+
+namespace create {
+  
+  Serial::Serial(boost::shared_ptr<Data> d, const uint8_t& header) :
+    data(d),
+    headerByte(header),
+    port(io), 
+    readState(READ_HEADER), 
+    isReading(false),
+    dataReady(false),
+    corruptPackets(0),
+    totalPackets(0) { 
+    //std::cout << "# Serial Created" << std::endl;
+  }
+  
+  Serial::~Serial() { 
+    disconnect();
+    //std::cout << "# Serial Destroyed" << std::endl;
+  }
+  
+  bool Serial::connect(const std::string& portName, const int& baud, boost::function<void()> cb) {
+    //std::cout << "## Serial connect start" << std::endl;
+    using namespace boost::asio;
+    port.open(portName);
+    port.set_option(serial_port::baud_rate(baud));
+    port.set_option(serial_port::flow_control(serial_port::flow_control::none));
+  
+    if (port.is_open()) {
+      callback = cb;
+      bool startReadSuccess = startReading();
+      if (!startReadSuccess)
+        port.close();
+      //std::cout << "## Serial connect done" << std::endl;
+      return startReadSuccess;
+    }
+    //std::cout << "## Serial connect failed" << std::endl;
+    return false;
+  }
+  
+  void Serial::disconnect() {
+    if (isReading) {
+      stopReading();
+    }
+
+    if (connected()) {
+      //std::cout << "## Serial disconnect start" << std::endl;
+      // Ensure not in Safe/Full modes
+      sendOpcode(OC_START);
+      // Stop OI
+      sendOpcode(OC_STOP);
+      port.close();
+      //std::cout << "## Serial disconnect done" << std::endl;
+    }
+  }
+  
+  bool Serial::startReading() {
+    if (!connected()) return false;
+
+    if (!data) {
+      CERR("[create::Serial] ", "data pointer not initialized.");
+      return false;
+    }
+
+    // Only allow once
+    if (isReading) return true;
+
+    //std::cout << "### Serial start reading" << std::endl;
+    // 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
+
+    io.reset();
+
+    // Start continuously reading one byte at a time
+    boost::asio::async_read(port,
+                            boost::asio::buffer(&byteRead, 1),
+                            boost::bind(&Serial::onData, this, _1, _2));
+
+    ioThread = boost::thread(boost::bind(&boost::asio::io_service::run, &io)); 
+
+    // Wait for first complete read to finish
+    boost::unique_lock<boost::mutex> lock(dataReadyMut);
+    //std::cout << "#### Waiting for dataReady" << std::endl;
+    int attempts = 1;
+    int maxAttempts = 10;
+    while (!dataReady) {
+      if (!dataReadyCond.timed_wait(lock, boost::get_system_time() + boost::posix_time::milliseconds(500))) {
+        if (attempts >= maxAttempts) {
+          CERR("[create::Serial] ", "failed to receive data from Create. Check if robot is powered!");
+          io.stop();
+          ioThread.join();
+          return false;
+        } 
+        attempts++;
+        //std::cout << "Requesting data from Create. Attempt " << attempts << std::endl;
+        // Request data again
+        sendOpcode(OC_START);
+        send(streamReq, 2 + numPackets); 
+      }
+    }
+    //std::cout << "#### Data is ready." << std::endl;
+    isReading = true;
+    //std::cout << "### Serial start reading DONE" << std::endl;
+    return true;
+  }
+  
+  void Serial::stopReading() {
+    if (isReading) {
+      //std::cout << "### Start stopReading" << std::endl;
+      io.stop();
+      ioThread.join();
+      isReading = false;
+      {
+        boost::lock_guard<boost::mutex> lock(dataReadyMut);
+        dataReady = false;
+      }
+      //std::cout << "### End stopReading" << std::endl;
+    }
+  }
+
+  void Serial::onData(const boost::system::error_code& e, const std::size_t& size) {
+    //std::cout << "#### onData" << std::endl;
+    if (e) {
+      CERR("[create::Serial] ", "serial error - " << e.message());
+      return;
+    }
+  
+    // 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 
+            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 {
+            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) {
+            // Validate all packets
+            data->validateAll();
+
+            // Notify first data packets ready
+            {
+              boost::lock_guard<boost::mutex> lock(dataReadyMut);
+             // std::cout << "locking." << std::endl;
+              if (!dataReady) {
+                dataReady = true;
+                dataReadyCond.notify_one();
+                //std::cout << "##### Notified." << std::endl;
+              }
+            }
+            // Callback to notify data is ready
+            if (callback)
+              callback();        
+          }
+          else {
+            // Corrupt data
+            corruptPackets++;
+          }
+          totalPackets++;
+          // Start again
+          readState = READ_HEADER;
+        break;
+      } // end switch (readState)
+    } // end if (size == 1)
+
+    // Read the next byte
+    boost::asio::async_read(port,
+                            boost::asio::buffer(&byteRead, 1),
+                            boost::bind(&Serial::onData, this, _1, _2));
+  }
+  
+  bool Serial::send(const uint8_t* bytes, unsigned int numBytes) {
+    if (!connected()) {
+      CERR("[create::Serial] ", "send failed, not connected."); 
+      return false;
+    }
+    // TODO: catch boost exceptions
+    boost::asio::write(port, boost::asio::buffer(bytes, numBytes));
+    return true;
+  }
+
+  bool Serial::sendOpcode(const Opcode& code) {
+    uint8_t oc = (uint8_t) code;
+    return send(&oc, 1);
+  }
+
+} // namespace create