CreateDriver.cc

Go to the documentation of this file.

#include "Shared/RobotInfo.h"
#if defined(TGT_IS_CREATE)

#include "CreateDriver.h"
#include "CreateCommands.h"
#include "Shared/MarkScope.h"
#include "Shared/get_time.h"
#include "Shared/debuget.h"
#include "Shared/TimeET.h"

#include <arpa/inet.h>
#include <stdio.h>

using namespace std; 

const std::string CreateDriver::autoRegisterCreateDriver = DeviceDriver::getRegistry().registerType<CreateDriver>("Create");

void CreateDriver::motionStarting()
{
  //  std::cout << "motionStarting called!" << std::endl;
  MotionHook::motionStarting();
  CommPort * comm = CommPort::getRegistry().getInstance(commName);
  if(comm==NULL) {
    std::cerr << "CreateDriver \"" << instanceName << "\": could not find CommPort \"" << commName << "\"" << std::endl;
  } else if(!comm->open()) {
    std::cerr << "CreateDriver \"" << instanceName << "\": unable to open comm port \"" << commName << "\"" << std::endl;
  } else {
    connect();
  }
  motionActive=true;
  commName.addPrimitiveListener(this);
}

bool CreateDriver::isConnected() {
  CommPort * comm = CommPort::getRegistry().getInstance(commName);
  return (comm!=NULL && comm->isWriteable());
}

void CreateDriver::motionStopping() {
  motionActive=false;
  if(!sensorsActive) // listener count is not recursive, so only remove if we're the last one
    commName.removePrimitiveListener(this);
  CommPort * comm = CommPort::getRegistry().getInstance(commName);
  if(comm!=NULL)
    comm->close(); // this *is* recursive, so we always close it to match our open() in motionStarting()
  MotionHook::motionStopping();
}

void CreateDriver::motionCheck(const float outputs[][NumOutputs]) {
  CommPort * comm = CommPort::getRegistry().getInstance(commName);
  if(comm==NULL || !comm->isWriteable())
    return;
  
  vector<unsigned char> commands(0);
  
  commands.push_back(CREATE_DRIVE_DIRECT);
  
  short output;
  output = (short)outputs[NumFrames-1][RobotInfo::RWheelOffset];
  commands.push_back((unsigned char)(output >> 8));
  commands.push_back((unsigned char)(output & 0xFF));
  
  output = (short)outputs[NumFrames-1][RobotInfo::LWheelOffset];
  commands.push_back((unsigned char)(output >> 8));
  commands.push_back((unsigned char)(output & 0xFF));
  
  commands.push_back(CREATE_LEDS);
  
  unsigned char led = 0;
  if (calcLEDValue(RobotInfo::PlayLEDOffset-LEDOffset,outputs[NumFrames-1][RobotInfo::PlayLEDOffset]))
    led = led | 0x2;
  if (calcLEDValue(RobotInfo::AdvanceLEDOffset-LEDOffset,outputs[NumFrames-1][RobotInfo::AdvanceLEDOffset]))
    led = led | 0x8;
  commands.push_back(led);
  
  float red = outputs[NumFrames-1][RobotInfo::PowerRedLEDOffset];
  float green = outputs[NumFrames-1][RobotInfo::PowerGreenLEDOffset];
  if (red == 0 && green == 0)
    commands.push_back((unsigned char)0);
  else
    commands.push_back((unsigned char)(255 * red / (red + green)));
  commands.push_back((unsigned char)(255 * max(red, green)));
  
  /*
   unsigned char desiredMode = (unsigned char)outputs[NumFrames-1][RobotInfo::ModeOffset];
   if (desiredMode != 0 && desiredMode != lastDesiredMode) {
   if (desiredMode == RobotInfo::MODE_SAFE)
   commands.push_back(CREATE_SAFE);
   if (desiredMode == RobotInfo::MODE_PASSIVE)
   commands.push_back(CREATE_START);
   if (desiredMode == RobotInfo::MODE_FULL)
   commands.push_back(CREATE_FULL);
   
   lastDesiredMode = desiredMode;
   }
   */
  
  unsigned int dt = static_cast<unsigned int>(NumFrames*FrameTime/((getTimeScale()>0)?getTimeScale():1.f));
  sendCommand(commands, dt*3/4);
  
  MotionHook::motionCheck(outputs); // updates lastOutputs and isFirstCheck, we ignore its motionUpdated() call
}

bool CreateDriver::sendCommand(vector<unsigned char> bytes, unsigned int timeout)
{
  CommPort * comm = CommPort::getRegistry().getInstance(commName);
  if(comm==NULL)
    return false;
  Thread::Lock& l = comm->getLock();
  
  //  cout << "CREATE: Trying to get lock for write." << endl;
  
  unsigned int t = get_time();
  unsigned int giveup = t + timeout;
  
  while (!l.trylock()) {
    if (get_time() >= giveup) {
      if(MotionHook::verbose>0)
        cerr << "CREATE: Unable to send command: couldn't get lock on comm port" << endl;
      return false;
    }
    usleep(1000);
  }
  
  MarkScope autolock(l); l.unlock(); //transfer lock to MarkScope
  
  std::ostream os(&comm->getWriteStreambuf());
  //cout << endl << "W";
  for (unsigned int i = 0; i < bytes.size(); i++) {
    os << bytes[i];
    //cout << " " << (int)bytes[i];
  }
  os << std::flush;
  //cout << endl;
  
  return true;
}

void CreateDriver::connect()
{
  vector<unsigned char> commands(2);
  commands[0] = CREATE_START;
  commands[1] = CREATE_SAFE;
  sendCommand(commands, 3000000);
  // cout << "Connecting to CREATE\n" << flush;
}

unsigned int CreateDriver::nextTimestamp() {
  CommPort * comm = CommPort::getRegistry().getInstance(commName);
  if(comm==NULL || !comm->isReadable())
    return -1U;
  return get_time();
}

char CreateDriver::readChar(std::istream &is, unsigned char &chk)
{
  char b;
  
  // do non-blocking read until given timeout
  // wait for 100 ms
  TimeET giveup = TimeET() + TimeET((long)100);
  while (!is.readsome(&b,1)) {
    if (giveup < TimeET()) {
      throw std::runtime_error("CREATE DRIVER: Nothing to read");
    }
  }
  chk += (unsigned char)b;
  
  // cout << "R " << int((unsigned char)b) << " ";
  return b;
}

unsigned char CreateDriver::readUnsignedChar(std::istream &is, unsigned char &chk)
{
  return (unsigned char)readChar(is,chk);
}

short CreateDriver::readShort(std::istream &is, unsigned char &chk){
  unsigned char a = readUnsignedChar(is, chk);
  unsigned char b = readUnsignedChar(is, chk);
  
  short ret = ((short) a << 8) | ((short) b);
  return ret;
}

unsigned short CreateDriver::readUnsignedShort(std::istream &is, unsigned char &chk){
  unsigned char a = readUnsignedChar(is, chk);
  unsigned char b = readUnsignedChar(is, chk);
  
  unsigned short ret = ((unsigned short) a << 8) | ((unsigned short) b);
  return ret;
}

int CreateDriver::readPacket(std::istream &is, const char &type, CreateStatus &createStatus, unsigned char &chk)
{
  int total = 0;
  switch(type){
    case CREATE_SENSOR_GROUP_0:
      total += readPacket(is, CREATE_SENSOR_DROP, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_WALL, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_CLIFF_LEFT, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_CLIFF_FRONT_LEFT, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_CLIFF_FRONT_RIGHT, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_CLIFF_RIGHT, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_VIRTUAL_WALL, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_OVERCURRENT, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_UNUSED_1, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_UNUSED_2, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_IR, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_BUTTONS, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_DISTANCE, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_ANGLE, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_CHANGING_STATE, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_VOLTAGE, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_CURRENT, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_BATTERY_TEMP, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_BATTERY_CHARGE, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_BATTERY_CAPACITY, createStatus, chk);
      break; 
    case CREATE_SENSOR_GROUP_1:
      total += readPacket(is, CREATE_SENSOR_DROP, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_WALL, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_CLIFF_LEFT, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_CLIFF_FRONT_LEFT, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_CLIFF_FRONT_RIGHT, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_CLIFF_RIGHT, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_VIRTUAL_WALL, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_OVERCURRENT, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_UNUSED_1, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_UNUSED_2, createStatus, chk);
      break;
    case CREATE_SENSOR_GROUP_2:
      total += readPacket(is, CREATE_SENSOR_IR, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_BUTTONS, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_DISTANCE, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_ANGLE, createStatus, chk);
      break;
    case CREATE_SENSOR_GROUP_3:
      total += readPacket(is, CREATE_SENSOR_CHANGING_STATE, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_VOLTAGE, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_CURRENT, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_BATTERY_TEMP, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_BATTERY_CHARGE, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_BATTERY_CAPACITY, createStatus, chk);
      break;
    case CREATE_SENSOR_GROUP_4:
      total += readPacket(is, CREATE_SENSOR_WALL_SIGNAL, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_CLIFF_L_SIGNAL, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_CLIFF_FL_SIGNAL, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_CLIFF_FR_SIGNAL, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_CLIFF_R_SIGNAL, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_DIGITAL_IN, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_ANALOG, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_CAN_CHARGE, createStatus, chk);
      break;
    case CREATE_SENSOR_GROUP_5:
      total += readPacket(is, CREATE_SENSOR_OI_MODE, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_SONG_NUMBER, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_SONG_PLAYING, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_STREAM_SIZE, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_REQ_VELOCITY, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_REQ_RADIUS, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_REQ_RIGHT_VELOCITY, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_REQ_LEFT_VELOCITY, createStatus, chk);
      break;
    case CREATE_SENSOR_GROUP_6:
      total += readPacket(is, CREATE_SENSOR_DROP, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_WALL, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_CLIFF_LEFT, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_CLIFF_FRONT_LEFT, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_CLIFF_FRONT_RIGHT, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_CLIFF_RIGHT, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_VIRTUAL_WALL, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_OVERCURRENT, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_UNUSED_1, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_UNUSED_2, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_IR, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_BUTTONS, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_DISTANCE, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_ANGLE, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_CHANGING_STATE, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_VOLTAGE, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_CURRENT, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_BATTERY_TEMP, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_BATTERY_CHARGE, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_BATTERY_CAPACITY, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_WALL_SIGNAL, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_CLIFF_L_SIGNAL, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_CLIFF_FL_SIGNAL, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_CLIFF_FR_SIGNAL, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_CLIFF_R_SIGNAL, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_DIGITAL_IN, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_ANALOG, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_CAN_CHARGE, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_OI_MODE, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_SONG_NUMBER, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_SONG_PLAYING, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_STREAM_SIZE, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_REQ_VELOCITY, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_REQ_RADIUS, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_REQ_RIGHT_VELOCITY, createStatus, chk);
      total += readPacket(is, CREATE_SENSOR_REQ_LEFT_VELOCITY, createStatus, chk);
      break;
    case CREATE_SENSOR_DROP:
      total = 1;
      createStatus.bumpsWheelDrops = readUnsignedChar(is, chk);
      break;
    case CREATE_SENSOR_WALL:
      total = 1;
      createStatus.wall = readUnsignedChar(is, chk);
      break;
    case CREATE_SENSOR_CLIFF_LEFT:
      total = 1;
      createStatus.cliffLeft = readUnsignedChar(is, chk);
      break;
    case CREATE_SENSOR_CLIFF_FRONT_LEFT:
      total = 1;
      createStatus.cliffFrontLeft = readUnsignedChar(is, chk);
      break;
    case CREATE_SENSOR_CLIFF_FRONT_RIGHT:
      total = 1;
      createStatus.cliffFrontRight = readUnsignedChar(is, chk);
      break;
    case CREATE_SENSOR_CLIFF_RIGHT:
      total = 1;
      createStatus.cliffRight = readUnsignedChar(is, chk);
      break;
    case CREATE_SENSOR_VIRTUAL_WALL:
      total = 1;
      createStatus.virtualWall = readUnsignedChar(is, chk);
      break;
    case CREATE_SENSOR_OVERCURRENT:
      total = 1;
      createStatus.overcurrents = readUnsignedChar(is, chk);
      break;
    case CREATE_SENSOR_UNUSED_1:
    case CREATE_SENSOR_UNUSED_2:
      total = 1;
      readUnsignedChar(is, chk);
      break;
    case CREATE_SENSOR_IR:
      total = 1;
      createStatus.ir = readUnsignedChar(is, chk);
      break;
    case CREATE_SENSOR_BUTTONS:
      total = 1;
      createStatus.buttons = readUnsignedChar(is, chk);
      break;
    case CREATE_SENSOR_DISTANCE:
      total = 2;
      createStatus.distance = readShort(is, chk);
      break;
    case CREATE_SENSOR_ANGLE:
      total = 2;
      createStatus.angle = readShort(is, chk);
      break;
    case CREATE_SENSOR_CHANGING_STATE:
      total = 1;
      createStatus.chargingState = readUnsignedChar(is, chk);
      break;
    case CREATE_SENSOR_VOLTAGE:
      total = 2;
      createStatus.voltage = readUnsignedShort(is, chk);
      break;
    case CREATE_SENSOR_CURRENT:
      total = 2;
      createStatus.current = readShort(is, chk);
      break;
    case CREATE_SENSOR_BATTERY_TEMP:
      total = 1;
      createStatus.batteryTemperature = readChar(is, chk);
      break;
    case CREATE_SENSOR_BATTERY_CHARGE:
      total = 2;
      createStatus.batteryCharge = readUnsignedShort(is, chk);
      break;
    case CREATE_SENSOR_BATTERY_CAPACITY:
      total = 2;
      createStatus.batteryCapacity = readUnsignedShort(is, chk);
      break;
    case CREATE_SENSOR_WALL_SIGNAL:
      total = 2;
      createStatus.wallSignal = readUnsignedShort(is, chk);
      break;
    case CREATE_SENSOR_CLIFF_L_SIGNAL:
      total = 2;
      createStatus.cliffLeftSignal = readUnsignedShort(is, chk);
      break;
    case CREATE_SENSOR_CLIFF_FL_SIGNAL:
      total = 2;
      createStatus.cliffFrontLeftSignal = readUnsignedShort(is, chk);
      break;
    case CREATE_SENSOR_CLIFF_FR_SIGNAL:
      total = 2;
      createStatus.cliffFrontRightSignal = readUnsignedShort(is, chk);
      break;
    case CREATE_SENSOR_CLIFF_R_SIGNAL:
      total = 2;
      createStatus.cliffRightSignal = readUnsignedShort(is, chk);
      break;
    case CREATE_SENSOR_DIGITAL_IN:
      total = 1;
      createStatus.userDigitalInputs= readUnsignedChar(is, chk);
      break;
    case CREATE_SENSOR_ANALOG:
      total = 2;
      createStatus.userAnalogInput = readUnsignedShort(is, chk);
      break;
    case CREATE_SENSOR_CAN_CHARGE:
      total = 1;
      createStatus.chargingSourcesAvailable = readUnsignedChar(is, chk);
      break;
    case CREATE_SENSOR_OI_MODE:
      total = 1;
      createStatus.oiMode = readUnsignedChar(is, chk);
      break;
    case CREATE_SENSOR_SONG_NUMBER:
      total = 1;
      createStatus.songNumber = readUnsignedChar(is, chk);
      break;
    case CREATE_SENSOR_SONG_PLAYING:
      total = 1;
      createStatus.songPlay = readUnsignedChar(is, chk);
      break;
    case CREATE_SENSOR_STREAM_SIZE:
      total = 1;
      createStatus.streamSize = readUnsignedChar(is, chk);
      break;
    case CREATE_SENSOR_REQ_VELOCITY:
      total = 2;
      createStatus.velocity = readShort(is, chk);
      break;
    case CREATE_SENSOR_REQ_RADIUS:
      total = 2;
      createStatus.radius = readShort(is, chk);
      break;
    case CREATE_SENSOR_REQ_RIGHT_VELOCITY:
      total = 2;
      createStatus.rightVelocity = readShort(is, chk);
      break;
    case CREATE_SENSOR_REQ_LEFT_VELOCITY:
      total = 2;
      createStatus.leftVelocity = readShort(is, chk);
      break;
    default:
      //std::cerr << "CREATE DRIVER: unknown packet type " << (int) type << std::endl;
      break;
  }
  
  return total;
}

bool CreateDriver::attemptPacketRead(std::istream &is, CreateStatus &createStatus)
{
  // Begin packet reading...
  unsigned char temp;
  
  if(!is)
    return false;
  
  // cout << "CREATE: Attempting packet read: ";
  
  try {
    readPacket(is, CREATE_SENSOR_GROUP_6, createStatus, temp);
    // cout << " Good packet\n" << flush;
  } catch (std::runtime_error &e) {
    // cout << "CREATE: Exception while trying to read data.\n";
    // cout << e.what() << endl;
    return false;
  }
  return true;
}

// This code is for reading in streaming mode, which we're presently not using
// because of the Create odometry bug.
bool CreateDriver::attemptStreamRead(std::istream &is, CreateStatus &createStatus)
{
  // Begin packet reading...
  unsigned char in = 0;
  unsigned char type = 0;
  unsigned char numPackets = 0;
  unsigned char chk = 0;
  int i;
  
  if(!is)
    return false;
  
  // cout << "CREATE: Attempting packet read: ";
  
  try {
    type = readUnsignedChar(is, chk);
    int dropped = 0;
    while (type != 19 && dropped < PACKET_LENGTH) {
      dropped++;
      type = readUnsignedChar(is, chk);
    }
    
    if ( dropped > 0 )
      cout << "CREATE: Dropped " << dropped << " bytes searching for start of packet.\n";
    
    if (type != 19) {
      cout << "CREATE: 19 Not found, available data exhausted.\n";
      return false;
    }
    
    // cout << "CREATE: Reading packet...\n";
    
    chk = 19;
    numPackets = readUnsignedChar(is, chk);
    
    for(i = 0; i < numPackets; i+=in){
      type = readUnsignedChar(is, chk);
      
      in = readPacket(is, type, createStatus, chk) + 1;
    }
    
    if(i != numPackets){
      //cout << "CREATE: read in different amount than expected! Read " << i << " Expected " << (int)numPackets << endl;
      return false;
    }
    
    readUnsignedChar(is, chk);
    // unsigned char const checksum = readUnsignedChar(is, chk);
    // cout << "CREATE: Finished reading: chksum is " << (int)chk << " and checksum byte is " << (int)checksum << endl;
    
    if (chk != 0) {
      cout << "CREATE: Checksum does not match, discarding packet.\n";
      return false;
    }
  } catch (std::runtime_error &e) {
    // cout << "CREATE: Exception while trying to read data.\n";
    // cout << e.what() << endl;
    return false;
  }
  
  return true;
}

void CreateDriver::resetStatus(CreateStatus &status)
{
  status.bumpsWheelDrops = 0;
  status.wall = 0;
  status.cliffLeft = 0;
  status.cliffFrontLeft = 0;
  status.cliffFrontRight = 0;
  status.cliffRight = 0;
  status.virtualWall = 0;
  status.overcurrents = 0;
  status.ir = 0;
  status.buttons = 0;
  status.distance = 0;
  status.angle = 0;
  status.chargingState = 0;
  status.voltage = 0;
  status.current = 0;
  status.batteryTemperature = 0;
  status.batteryCharge = 0;
  status.batteryCapacity = 0;
  status.wallSignal = 0;
  status.cliffLeftSignal = 0;
  status.cliffFrontLeftSignal = 0;
  status.cliffFrontRightSignal = 0;
  status.cliffRightSignal = 0;
  status.userDigitalInputs = 0;
  status.userAnalogInput = 0;
  status.chargingSourcesAvailable = 0;
  status.oiMode = 0;
  status.songNumber = 0;
  status.songPlay = 0;
  status.streamSize = 0;
  status.velocity = 0;
  status.radius = 0;
  status.rightVelocity = 0;
  status.leftVelocity = 0;
}

void CreateDriver::mergeStatus(CreateStatus &oldStatus, CreateStatus &newStatus)
{
  oldStatus.bumpsWheelDrops = newStatus.bumpsWheelDrops;
  oldStatus.wall = newStatus.wall;
  oldStatus.cliffLeft = newStatus.cliffLeft;
  oldStatus.cliffFrontLeft = newStatus.cliffFrontLeft;
  oldStatus.cliffFrontRight = newStatus.cliffFrontRight;
  oldStatus.cliffRight = newStatus.cliffRight;
  oldStatus.virtualWall = newStatus.virtualWall;
  oldStatus.overcurrents = newStatus.overcurrents;
  oldStatus.ir = newStatus.ir;
  oldStatus.buttons = newStatus.buttons;
  oldStatus.distance = oldStatus.distance + newStatus.distance;
  oldStatus.angle = oldStatus.angle + newStatus.angle;
  oldStatus.chargingState = newStatus.chargingState;
  oldStatus.voltage = newStatus.voltage;
  oldStatus.current = newStatus.current;
  oldStatus.batteryTemperature = newStatus.batteryTemperature;
  oldStatus.batteryCharge = newStatus.batteryCharge;
  oldStatus.batteryCapacity = newStatus.batteryCapacity;
  oldStatus.wallSignal = newStatus.wallSignal;
  oldStatus.cliffLeftSignal = newStatus.cliffLeftSignal;
  oldStatus.cliffFrontLeftSignal = newStatus.cliffFrontLeftSignal;
  oldStatus.cliffFrontRightSignal = newStatus.cliffFrontRightSignal;
  oldStatus.cliffRightSignal = newStatus.cliffRightSignal;
  oldStatus.userDigitalInputs = newStatus.userDigitalInputs;
  oldStatus.userAnalogInput = newStatus.userAnalogInput;
  oldStatus.chargingSourcesAvailable = newStatus.chargingSourcesAvailable;
  oldStatus.oiMode = newStatus.oiMode;
  oldStatus.songNumber = newStatus.songNumber;
  oldStatus.songPlay = newStatus.songPlay;
  oldStatus.streamSize = newStatus.streamSize;
  oldStatus.velocity = newStatus.velocity;
  oldStatus.radius = newStatus.radius;
  oldStatus.rightVelocity = newStatus.rightVelocity;
  oldStatus.leftVelocity = newStatus.leftVelocity;
}

bool CreateDriver::advance() {
  CommPort * comm = CommPort::getRegistry().getInstance(commName);
  if(comm==NULL || !comm->isReadable() || !comm->isWriteable())
    return false;
  
  Thread::testCurrentCancel();
  
  CreateStatus tempStatus;
  resetStatus(tempStatus);
  
  // poll for next packet if it's time
  if ( get_time() > lastPollTime + pollInterval ) {
    lastPollTime = get_time();
    vector<unsigned char> commands(2);
    commands[0] = CREATE_SENSORS;
    commands[1] = CREATE_SENSOR_GROUP_6;
    sendCommand(commands, 1000);
    // cout << "Poll\n" << flush;
  }

  // now see if we have anything to read
  std::istream is(&comm->getReadStreambuf());
  
  //cout << "" << get_time() << ": ";
  
  bool valid = false;
  {
    //    MarkScope autolock(comm->getLock());
    valid = attemptPacketRead(is, tempStatus);
  }
  
  if (!valid) {
    packetFailures++;
    
    if (packetFailures > 0 && packetFailures % 10 == 0) {
      if (packetFailures == 10)
        cout << "CREATE DRIVER: Dropped 10 consecutive packets, attempting to reconnect to Create...\n";
      if (packetFailures % 20 == 0)
        cout << "CREATE DRIVER: Still attempting to reconnect to Create...\n";
      connect();
      usleep(500000);
    }
    return false;
  }
  else {
    packetFailures = 0;
    mergeStatus(globalStatus, tempStatus);
  }
  // std::cout << "got packet\n" << std::flush;
  
  // set back to safe if we need to
  if (tempStatus.oiMode == RobotInfo::MODE_PASSIVE) {
    vector<unsigned char> commands(1); commands[0] = CREATE_SAFE;
    sendCommand(commands, 1000);
  }
  
  setSensorValue(DigitalInput0Offset, ((globalStatus.userDigitalInputs >> 0) & 0x1));
  setSensorValue(DigitalInput1Offset, ((globalStatus.userDigitalInputs >> 1) & 0x1));
  setSensorValue(DigitalInput2Offset, ((globalStatus.userDigitalInputs >> 2) & 0x1));
  setSensorValue(DigitalInput3Offset, ((globalStatus.userDigitalInputs >> 3) & 0x1));
  setSensorValue(AnalogSignalOffset,globalStatus.userAnalogInput);
  setSensorValue(WallSignalOffset,globalStatus.wallSignal);
  setSensorValue(IRCommOffset,globalStatus.ir);
  setSensorValue(CliffLeftSignalOffset,globalStatus.cliffLeftSignal);
  setSensorValue(CliffFrontLeftSignalOffset,globalStatus.cliffFrontLeftSignal);
  setSensorValue(CliffFrontRightSignalOffset,globalStatus.cliffFrontRightSignal);
  setSensorValue(CliffRightSignalOffset,globalStatus.cliffRightSignal);
  setSensorValue(EncoderDistanceOffset,globalStatus.distance);
  setSensorValue(EncoderAngleOffset,globalStatus.angle);
  setSensorValue(VoltageOffset,globalStatus.voltage);
  setSensorValue(CurrentOffset,globalStatus.current);
  setSensorValue(BatteryChargeOffset,globalStatus.batteryCharge);
  setSensorValue(BatteryTempOffset,globalStatus.batteryTemperature);
  setSensorValue(ChargingStateOffset,globalStatus.chargingState);
  setSensorValue(ModeStateOffset,globalStatus.oiMode);
  
  setButtonValue(PlayButOffset,(globalStatus.buttons & 0x1));
  setButtonValue(AdvanceButOffset,((globalStatus.buttons >> 2) & 0x1));
  setButtonValue(WallButOffset,globalStatus.wall);
  setButtonValue(DropCasterButOffset,((globalStatus.bumpsWheelDrops >> 4) & 0x1));
  setButtonValue(DropLeftWheelButOffset,((globalStatus.bumpsWheelDrops >> 3) & 0x1));
  setButtonValue(DropRightWheelButOffset,((globalStatus.bumpsWheelDrops >> 2) & 0x1));
  setButtonValue(BumpLeftButOffset,((globalStatus.bumpsWheelDrops >> 1) & 0x1));
  setButtonValue(BumpRightButOffset,((globalStatus.bumpsWheelDrops >> 0) & 0x1));
  setButtonValue(OvercurrentLeftWheelOffset,((globalStatus.overcurrents >> 4) & 0x1));
  setButtonValue(OvercurrentRightWheelOffset,((globalStatus.overcurrents >> 3) & 0x1));
  setButtonValue(LowSideDriver0ButOffset,((globalStatus.overcurrents >> 2) & 0x1));
  setButtonValue(LowSideDriver1ButOffset,((globalStatus.overcurrents >> 1) & 0x1));
  setButtonValue(LowSideDriver2ButOffset,((globalStatus.overcurrents >> 0) & 0x1));
  setButtonValue(BaseChargerButOffset,((globalStatus.chargingSourcesAvailable >> 1) & 0x1));
  setButtonValue(InternalChargerButOffset,((globalStatus.chargingSourcesAvailable >> 0) & 0x1));
  
  ++frameNumber;
  return true;
}

void CreateDriver::registerSource() {
  CommPort * comm = CommPort::getRegistry().getInstance(commName);
  if(comm==NULL) {
    std::cerr << "CreateDriver \"" << instanceName << "\": could not find CommPort \"" << commName << "\"" << std::endl;
  } else if(!comm->open()) {
    std::cerr << "CreateDriver \"" << instanceName << "\": unable to open comm port \"" << commName << "\"" << std::endl;
  } else {
    connect();
  }
  sensorsActive=true;
  commName.addPrimitiveListener(this);
}

void CreateDriver::deregisterSource() {
  CommPort * comm = CommPort::getRegistry().getInstance(commName);
  if(comm!=NULL)
    comm->close();
  sensorsActive=false;
  if(!motionActive) // listener count is not recursive, so only remove if we're the last one
    commName.removePrimitiveListener(this);
}

void CreateDriver::doUnfreeze() {
  MarkScope sl(poller.getStartLock());
  if(!poller.isStarted()) {
    //poller.resetPeriod(1.0/(*sensorFramerate));
    poller.start();
  }
  sensorFramerate->addPrimitiveListener(this);
}

void CreateDriver::doFreeze() {
  MarkScope sl(poller.getStartLock());
  if(poller.isStarted())
    poller.stop().join();
  sensorFramerate->removePrimitiveListener(this);
}

void CreateDriver::plistValueChanged(const plist::PrimitiveBase& pl) {
  // std::cout << "plistvalueChanged Called!" << std::endl;
  if(&pl==&commName) {
    // if here, then motionStarted or setDataSourceThread has been called, thus when commName changes,
    // need to close old one and reopen new one
    if(poller.isStarted())
      poller.stop().join();
    
    CommPort * comm = CommPort::getRegistry().getInstance(commName.getPreviousValue());
    if(comm!=NULL) {
      // close each of our old references
      if(sensorsActive)
        comm->close();
      if(motionActive)
        comm->close();
    }
    comm = CommPort::getRegistry().getInstance(commName);
    if(comm==NULL) {
      std::cerr << "CreateDriver \"" << instanceName << "\": could not find CommPort \"" << commName << "\"" << std::endl;
    } else {
      // open each of our new references
      if(sensorsActive)
        comm->open();
      if(motionActive) {
        if(!comm->open())
          std::cerr << "CreateDriver \"" << instanceName << "\": unable to open comm port \"" << commName << "\"" << std::endl;
        else
          connect();
      }
    }
    
    if(getTimeScale()>0) {
      //poller.resetPeriod(1.0/(*sensorFramerate));
      poller.start();
    }
  } else if(&pl==sensorFramerate) {
    //poller.resetPeriod(1.0/(*sensorFramerate));
  }
}

float CreateDriver::getAnalog(unsigned int /*inputIdx*/, unsigned char s) {
  return s*5.f/256;
}

float CreateDriver::getDigital(unsigned int /*inputIdx*/, unsigned char cur, unsigned char latch) {
  if(cur=='0')
    return 0;
  return (latch=='0') ? 0.5f : 1;
}

/*! @file
 * @brief 
 * @author Benson Tsai (btsai)
 * @author Ethan Tira-Thompson (ejt)
 * @author Alex Grubb (agrubb1)
 */

#endif