ArRobot.cpp

/*
MobileRobots Advanced Robotics Interface for Applications (ARIA)
Copyright (C) 2004, 2005 ActivMedia Robotics LLC
Copyright (C) 2006, 2007 MobileRobots Inc.

     This program is free software; you can redistribute it and/or modify
     it under the terms of the GNU General Public License as published by
     the Free Software Foundation; either version 2 of the License, or
     (at your option) any later version.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program; if not, write to the Free Software
     Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

If you wish to redistribute ARIA under different terms, contact 
MobileRobots for information about a commercial version of ARIA at 
robots@mobilerobots.com or 
MobileRobots Inc, 19 Columbia Drive, Amherst, NH 03031; 800-639-9481
*/

#include "ArExport.h"
#include "ariaOSDef.h"
#include <time.h>
#include <ctype.h>

#include "ArRobot.h"
#include "ArLog.h"
#include "ArDeviceConnection.h"
#include "ArTcpConnection.h"
#include "ArSerialConnection.h"
#include "ArLogFileConnection.h"
#include "ariaUtil.h"
#include "ArSocket.h"
#include "ArCommands.h"
#include "ArRobotTypes.h"
#include "ArSignalHandler.h"
#include "ArPriorityResolver.h"
#include "ArAction.h"
#include "ArRangeDevice.h"
#include "ArRobotConfigPacketReader.h"
#include "ariaInternal.h"

AREXPORT ArRobot::ArRobot(const char *name, bool obsolete, 
              bool doSigHandle, bool normalInit,
              bool addAriaExitCallback) :
  myMotorPacketCB(this, &ArRobot::processMotorPacket),
  myEncoderPacketCB(this, &ArRobot::processEncoderPacket),
  myIOPacketCB(this, &ArRobot::processIOPacket),
  myPacketHandlerCB(this, &ArRobot::packetHandler),
  myActionHandlerCB(this, &ArRobot::actionHandler),
  myStateReflectorCB(this, &ArRobot::stateReflector),
  myRobotLockerCB(this, &ArRobot::robotLocker),
  myRobotUnlockerCB(this, &ArRobot::robotUnlocker),
  myKeyHandlerExitCB(this, &ArRobot::keyHandlerExit),
  myGetCycleWarningTimeCB(this, &ArRobot::getCycleWarningTime),
  myGetNoTimeWarningThisCycleCB(this, &ArRobot::getNoTimeWarningThisCycle),
  myBatteryAverager(20),
  myRealBatteryAverager(20),
  myAriaExitCB(this, &ArRobot::ariaExitCallback)
{
  setName(name);
  myAriaExitCB.setName("ArRobotExit");
  myNoTimeWarningThisCycle = false;
  myGlobalPose.setPose(0, 0, 0);

  myParams = new ArRobotGeneric("");

  myMotorPacketCB.setName("ArRobot::motorPacket");
  myEncoderPacketCB.setName("ArRobot::encoderPacket");
  myIOPacketCB.setName("ArRobot::IOPacket");

  myPtz = NULL;
  myKeyHandler = NULL;
  myKeyHandlerCB = NULL;

  myConn = NULL;

  myOwnTheResolver = false;

  myBlockingConnectRun = false;
  myAsyncConnectFlag = false;

  myLogMovementSent = false;
  myLogMovementReceived = false;
  myLogVelocitiesReceived = false;
  myLogActions = false;
  myLastVel = 0;
  myLastRotVel = 0;
  myLastHeading = 0;
  myLastCalculatedRotVel = 0;

  myPacketsSentTracking = false;
  myPacketsReceivedTracking = false;
  myPacketsReceivedTrackingCount = false;
  myPacketsReceivedTrackingStarted.setToNow();

  myCycleTime = 100;
  myCycleWarningTime = 250;
  myConnectionCycleMultiplier = 2;
  myTimeoutTime = 8000;
  myStabilizingTime = 0;
  myCounter = 1;
  myResolver = NULL;
  myNumSonar = 0;

  myRequestedIOPackets = false;
  myRequestedEncoderPackets = false;
  myEncoderCorrectionCB = NULL;

  myCycleChained = true;

  myMoveDoneDist = 40;
  myHeadingDoneDiff = 3;

  myOrigRobotConfig = NULL;
  reset();
  if (normalInit)
    init();
  
  mySyncLoop.setRobot(this);

  if (doSigHandle)
    Aria::addRobot(this);
  if (addAriaExitCallback)
  {
    Aria::addExitCallback(&myAriaExitCB, 0);
    myAddedAriaExitCB = true;
  }
  else
  {
    myAddedAriaExitCB = false;
  }

  myConnectWithNoParams = false;
}



AREXPORT ArRobot::~ArRobot()
{
  ArResolver::ActionMap::iterator it;

  stopRunning();
  delete mySyncTaskRoot;
  ArUtil::deleteSetPairs(mySonars.begin(), mySonars.end());
  Aria::delRobot(this);

  if (myKeyHandlerCB != NULL)
    delete myKeyHandlerCB;

  for (it = myActions.begin(); it != myActions.end(); ++it)
  {
    (*it).second->setRobot(NULL);
  }
}


AREXPORT void ArRobot::init(void)
{
  setUpPacketHandlers();
  setUpSyncList();
  myOwnTheResolver = true;
  myResolver = new ArPriorityResolver;
}

AREXPORT void ArRobot::run(bool stopRunIfNotConnected)
{
  if (mySyncLoop.getRunning())
  {
    ArLog::log(ArLog::Terse, 
           "The robot is already running, cannot run it again.");
    return;
  }
  mySyncLoop.setRunning(true);
  mySyncLoop.stopRunIfNotConnected(stopRunIfNotConnected);
  mySyncLoop.runInThisThread();
}

AREXPORT bool ArRobot::isRunning(void) const
{
  return mySyncLoop.getRunning();
}

AREXPORT void ArRobot::runAsync(bool stopRunIfNotConnected)
{
  if (mySyncLoop.getRunning())
  {
    ArLog::log(ArLog::Terse, 
           "The robot is already running, cannot run it again.");
    return;
  }
  mySyncLoop.stopRunIfNotConnected(stopRunIfNotConnected);
  // Joinable, but do NOT lower priority. The robot thread is the most
  // important one around.
  mySyncLoop.create(true, false);
}

AREXPORT void ArRobot::stopRunning(bool doDisconnect)
{
  if (myKeyHandler != NULL)
    myKeyHandler->restore();
  mySyncLoop.stopRunning();
  myBlockingConnectRun = false;
  if (doDisconnect && (isConnected() || myIsStabilizing))
  {
    waitForRunExit();
    disconnect();
  }
  wakeAllWaitingThreads();
}

AREXPORT void ArRobot::setUpSyncList(void)
{
  mySyncTaskRoot = new ArSyncTask("SyncTasks");
  mySyncTaskRoot->setWarningTimeCB(&myGetCycleWarningTimeCB);
  mySyncTaskRoot->setNoTimeWarningCB(&myGetNoTimeWarningThisCycleCB);
  mySyncTaskRoot->addNewLeaf("Packet Handler", 85, &myPacketHandlerCB);
  mySyncTaskRoot->addNewLeaf("Robot Locker", 70, &myRobotLockerCB);
  mySyncTaskRoot->addNewBranch("Sensor Interp", 65);
  mySyncTaskRoot->addNewLeaf("Action Handler", 55, &myActionHandlerCB);
  mySyncTaskRoot->addNewLeaf("State Reflector", 45, &myStateReflectorCB);
  mySyncTaskRoot->addNewBranch("User Tasks", 25);
  mySyncTaskRoot->addNewLeaf("Robot Unlocker", 20, &myRobotUnlockerCB);
}


AREXPORT void ArRobot::setUpPacketHandlers(void)
{
  addPacketHandler(&myMotorPacketCB, ArListPos::FIRST);
  addPacketHandler(&myEncoderPacketCB, ArListPos::LAST);
  addPacketHandler(&myIOPacketCB, ArListPos::LAST);
}

void ArRobot::reset(void)
{
  resetOdometer();
  myInterpolation.reset();
  myEncoderInterpolation.reset();
  if (myOrigRobotConfig != NULL)
    delete myOrigRobotConfig;
  myOrigRobotConfig = new ArRobotConfigPacketReader(this, true);
  myFirstEncoderPose = true;
  //myEncoderPose.setPose(0, 0, 0);
  //myEncoderPoseTaken.setToNow();
  //myRawEncoderPose.setPose(0, 0, 0);
  //myGlobalPose.setPose(0, 0, 0);
  //myEncoderTransform.setTransform(myEncoderPose, myGlobalPose);
  myTransVelMax = 0;
  myTransAccel = 0;
  myTransDecel = 0;
  myRotVelMax = 0;
  myRotAccel = 0;
  myRotDecel = 0;

  myRobotLengthFront = 0;
  myRobotLengthRear = 0;

  myLeftVel = 0;
  myRightVel = 0;
  myBatteryVoltage = 13;
  myBatteryAverager.clear();
  myBatteryAverager.add(myBatteryVoltage);
  myStallValue = 0;
  myControl = 0;
  myFlags = 0;
  myFaultFlags = 0;
  myHasFaultFlags = 0;
  myCompass = 0;
  myAnalogPortSelected = 0;
  myAnalog = 0;
  myDigIn = 0;
  myDigOut = 0;
  myIOAnalogSize = 0;
  myIODigInSize = 0;
  myIODigOutSize = 0;
  myLastIOPacketReceivedTime.setSec(0);
  myLastIOPacketReceivedTime.setMSec(0);
  myVel = 0;
  myRotVel = 0;
  myChargeState = CHARGING_UNKNOWN;
  myLastX = 0;
  myLastY = 0;
  myLastTh = 0;
  mySentPulse = false;
  myIsConnected = false;
  myIsStabilizing = false;

  myTransVal = 0;
  myTransVal2 = 0;
  myTransType = TRANS_NONE;
  myLastTransVal = 0;
  myLastTransVal2 = 0;
  myLastTransType = TRANS_NONE;
  myLastTransSent.setToNow();
  myActionTransSet = false;
  myLastActionRotStopped = false;
  myLastActionRotHeading = false;

  myRotVal = 0;
  myLastRotVal = 0;
  myRotType = ROT_NONE;
  myLastRotType = ROT_NONE;
  myLastRotSent.setToNow();
  myActionRotSet = false;

  myLastPulseSent.setToNow();

  myDirectPrecedenceTime = 0;
  myStateReflectionRefreshTime = 500;

  myActionDesired.reset();

  myMotorPacCurrentCount = 0;
  myMotorPacCount = 0;
  myTimeLastMotorPacket = 0;

  mySonarPacCurrentCount = 0;
  mySonarPacCount = 0;
  myTimeLastSonarPacket = 0;

  myLeftEncoder = 0;
  myRightEncoder = 0;

  myWarnedAboutExtraSonar = false;
  myOdometryDelay = 0;
}

AREXPORT void ArRobot::setTransVelMax(double vel)
{
  if (vel > myAbsoluteMaxTransVel)
  {
    ArLog::log(ArLog::Terse, "ArRobot: setTransVelMax of %g is over the absolute max of %g, capping it", vel, myAbsoluteMaxTransVel);
    vel = myAbsoluteMaxTransVel;
  }
  myTransVelMax = vel;
}

AREXPORT void ArRobot::setTransAccel(double acc)
{
  if (acc > myAbsoluteMaxTransAccel)
  {
    ArLog::log(ArLog::Terse, "ArRobot: setTransAccel of %g is over the absolute max of %g, capping it", acc, myAbsoluteMaxTransAccel);
    acc = myAbsoluteMaxTransAccel;
  }
  myTransAccel = acc;
}

AREXPORT void ArRobot::setTransDecel(double decel)
{
  if (fabs(decel) > myAbsoluteMaxTransDecel)
  {
    ArLog::log(ArLog::Terse, "ArRobot: setTransDecel of %g is over the absolute max of %g, capping it", decel, myAbsoluteMaxTransDecel);
    decel = myAbsoluteMaxTransDecel;
  }
  myTransDecel = ArMath::fabs(decel);
}

AREXPORT void ArRobot::setRotVelMax(double vel)
{
  if (vel > myAbsoluteMaxRotVel)
  {
    ArLog::log(ArLog::Terse, "ArRobot: rotVelMax of %g is over the absolute max of %g, capping it", vel, myAbsoluteMaxRotVel);
    vel = myAbsoluteMaxRotVel;
  }
  myRotVelMax = vel;
}

AREXPORT void ArRobot::setRotAccel(double acc)
{
  if (acc > myAbsoluteMaxRotAccel)
  {
    ArLog::log(ArLog::Terse, "ArRobot: setRotAccel of %g is over the absolute max of %g, capping it", acc, myAbsoluteMaxRotAccel);
    acc = myAbsoluteMaxRotAccel;
  }
  myRotAccel = acc;
}

AREXPORT void ArRobot::setRotDecel(double decel)
{
  if (fabs(decel) > myAbsoluteMaxRotDecel)
  {
    ArLog::log(ArLog::Terse, "ArRobot: setRotDecel of %g is over the absolute max of %g, capping it", decel, myAbsoluteMaxRotDecel);
    decel = myAbsoluteMaxRotDecel;
  }
  myRotDecel = ArMath::fabs(decel);
}

AREXPORT double ArRobot::getTransVelMax(void) const
{
  return myTransVelMax;
}

AREXPORT double ArRobot::getTransAccel(void) const
{
  return myTransAccel;
}

AREXPORT double ArRobot::getTransDecel(void) const
{
  return myTransDecel;
}

AREXPORT double ArRobot::getRotVelMax(void) const
{
  return myRotVelMax;
}

AREXPORT double ArRobot::getRotAccel(void) const
{
  return myRotAccel;
}

AREXPORT double ArRobot::getRotDecel(void) const
{
  return myRotDecel;
}

AREXPORT void ArRobot::setDeviceConnection(ArDeviceConnection *connection)
{
  myConn = connection;
  mySender.setDeviceConnection(myConn);
  myReceiver.setDeviceConnection(myConn);
}

AREXPORT ArDeviceConnection *ArRobot::getDeviceConnection(void) const
{
  return myConn;
}

AREXPORT void ArRobot::setConnectionTimeoutTime(int mSecs)
{
  if (mSecs > 0)
    myTimeoutTime = mSecs;
  else
    myTimeoutTime = 0;
}

AREXPORT int ArRobot::getConnectionTimeoutTime(void) const
{
  return myTimeoutTime;
}

AREXPORT ArTime ArRobot::getLastPacketTime(void) const
{
  return myLastPacketReceivedTime;
}
AREXPORT bool ArRobot::asyncConnect(void)
{
  if (!mySyncLoop.getRunning() || isConnected())
    return false;
  
  myAsyncConnectFlag = true;
  myBlockingConnectRun = false;
  myAsyncConnectState = -1;
  return true;
}

AREXPORT bool ArRobot::blockingConnect(void)
{
  int ret = 0;

  lock();
  if (myIsConnected)
    disconnect();
  myBlockingConnectRun = true;
  myAsyncConnectState = -1;
  while ((ret = asyncConnectHandler(true)) == 0 && myBlockingConnectRun)
    ArUtil::sleep(ArMath::roundInt(getCycleTime() * .80));
  unlock();
  if (ret == 1)
    return true;
  else
    return false;
}

AREXPORT int ArRobot::asyncConnectHandler(bool tryHarderToConnect)
{
  int ret = 0;
  ArRobotPacket *packet;
  ArRobotPacket *tempPacket = NULL;
  char robotSubType[255];
  int i;
  int len;
  std::string str;
  ArTime endTime;
  int timeToWait;
  ArSerialConnection *serConn;

  endTime.setToNow();
  endTime.addMSec(getCycleTime()*myConnectionCycleMultiplier);
  
  myNoTimeWarningThisCycle = true;

  // if this is -1, then we're doing initialization stuff, then returning
  if (myAsyncConnectState == -1)
  {
    myAsyncConnectSentChangeBaud = false;
    myAsyncConnectNoPacketCount = 0;
    myAsyncConnectTimesTried = 0;
    reset();
    if (myConn == NULL)
    {
      ArLog::log(ArLog::Terse, "Cannot connect, no connection has been set.");
      failedConnect();
      return 2;
    }
    
    if (myConn->getStatus() != ArDeviceConnection::STATUS_OPEN) 
    {
      if (!myConn->openSimple())
      {
    /*str = myConn->getStatusMessage(myConn->getStatus());
      ArLog::log(ArLog::Terse, "Trying to connect to robot but connection not opened, it is %s", str.c_str());*/
        ArLog::log(ArLog::Terse, 
                   "Could not connect, because open on the device connection failed.");
        failedConnect();
        return 2;
      }
    }

    // check for log file connection: just return success
    if (dynamic_cast<ArLogFileConnection *>(myConn))
    {
      ArLogFileConnection *con = dynamic_cast<ArLogFileConnection *>(myConn);
      myRobotName = con->myName;
      myRobotType = con->myType;
      myRobotSubType = con->mySubtype;
      if (con->havePose)        // we know where to move
        moveTo(con->myPose);
      setCycleChained(false);   // don't process packets all at once
      madeConnection();
      finishedConnection();
      return 1;
    }


    if (dynamic_cast<ArSerialConnection *>(myConn))
    {
      myConn->write("WMS2\15", strlen("WMS2\15"));
    }
    // here we're flushing out all previously received packets
    while ( endTime.mSecTo() > 0 && myReceiver.receivePacket(0) != NULL);

    myAsyncConnectState = 0;
    return 0;
  }

  if (myAsyncConnectState >= 3)
  {
    bool handled;
    std::list<ArRetFunctor1<bool, ArRobotPacket *> *>::iterator it;
    while ((packet = myReceiver.receivePacket(0)) != NULL)
    {
      //printf("0x%x\n", packet->getID());
      for (handled = false, it = myPacketHandlerList.begin(); 
       it != myPacketHandlerList.end() && handled == false; 
       it++)
      {
    if ((*it) != NULL && (*it)->invokeR(packet)) 
      handled = true;
    else
      packet->resetRead();
      }
    }
  }

  if (myAsyncConnectState == 3)
  {
    //if (!myOrigRobotConfig->hasPacketBeenRequested())
    if (!myOrigRobotConfig->hasPacketArrived())
    {
      myOrigRobotConfig->requestPacket();
    }
    // if we've gotten our config packet or if we've timed out then
    // set our vel and acc/decel params and skip to the next part
    if (myOrigRobotConfig->hasPacketArrived() || 
    myAsyncStartedConnection.mSecSince() > 1000)
    {
      bool gotConfig;
      // if we have data from the robot use that
      if (myOrigRobotConfig->hasPacketArrived())
      {
    gotConfig = true;
    setAbsoluteMaxTransVel(myOrigRobotConfig->getTransVelTop());
    setAbsoluteMaxTransAccel(myOrigRobotConfig->getTransAccelTop());
    setAbsoluteMaxTransDecel(myOrigRobotConfig->getTransAccelTop());
    setAbsoluteMaxRotVel(myOrigRobotConfig->getRotVelTop());
    setAbsoluteMaxRotAccel(myOrigRobotConfig->getRotAccelTop());
    setAbsoluteMaxRotDecel(myOrigRobotConfig->getRotAccelTop());
    setTransVelMax(myOrigRobotConfig->getTransVelMax());
    setTransAccel(myOrigRobotConfig->getTransAccel());
    setTransDecel(myOrigRobotConfig->getTransDecel());
    setRotVelMax(myOrigRobotConfig->getRotVelMax());
    setRotAccel(myOrigRobotConfig->getRotAccel());
    setRotDecel(myOrigRobotConfig->getRotDecel());
      }
      // if our absolute maximums weren't set then set them
      else
      {
    gotConfig = false;
    setAbsoluteMaxTransVel(myParams->getAbsoluteMaxVelocity());
    setAbsoluteMaxTransAccel(1000);
    setAbsoluteMaxTransDecel(1000);
    setAbsoluteMaxRotVel(myParams->getAbsoluteMaxRotVelocity());
    setAbsoluteMaxRotAccel(100);
    setAbsoluteMaxRotDecel(100);
      }
      // okay we got in that data, now put over it any of the things
      // that we got from the robot parameter file... if we don't have
      // max vels from above or the parameters then use the absolutes
      // which we do have for everything
      if (ArMath::fabs(myParams->getTransVelMax()) > 1)
    setTransVelMax(myParams->getTransVelMax());
      else if (!gotConfig)
    setTransVelMax(myParams->getAbsoluteMaxVelocity());
      if (ArMath::fabs(myParams->getRotVelMax()) > 1)
    setRotVelMax(myParams->getRotVelMax());
      else if (!gotConfig)
    setRotVelMax(myParams->getAbsoluteMaxRotVelocity());
      if (ArMath::fabs(myParams->getTransAccel()) > 1)
    setTransAccel(myParams->getTransAccel());
      if (ArMath::fabs(myParams->getTransDecel()) > 1)
    setTransDecel(myParams->getTransDecel());
      if (ArMath::fabs(myParams->getRotAccel()) > 1)
    setRotAccel(myParams->getRotAccel());
      if (ArMath::fabs(myParams->getRotDecel()) > 1)
    setRotDecel(myParams->getRotDecel());
      myAsyncConnectState = 4;
    }
    else
      return 0;
  }
  // we want to see if we should switch baud
  if (myAsyncConnectState == 4)
  {
    serConn = dynamic_cast<ArSerialConnection *>(myConn);
    // if we didn't get a config packet or we can't change the baud or
    // we aren't using a serial port then don't switch the baud
    if (!myOrigRobotConfig->hasPacketArrived() || 
    !myOrigRobotConfig->getResetBaud() || 
    serConn == NULL ||
    myParams->getSwitchToBaudRate() == 0)
    {
      // if we're using a serial connection store our baud rate
      if (serConn != NULL)
    myAsyncConnectStartBaud = serConn->getBaud();
      myAsyncConnectState = 5;
    }
    // if we did get a packet and can change baud send the command
    else if (!myAsyncConnectSentChangeBaud)
    {
      // if we're using a serial connection store our baud rate
      if (serConn != NULL)
    myAsyncConnectStartBaud = serConn->getBaud();

      int baudNum = -1;
      
      // first suck up all the packets we have now
      while ((packet = myReceiver.receivePacket(0)) != NULL);
      if (myParams->getSwitchToBaudRate() == 9600)
    baudNum = 0;
      else if (myParams->getSwitchToBaudRate() == 19200)
    baudNum = 1;
      else if (myParams->getSwitchToBaudRate() == 38400)
    baudNum = 2;
      else if (myParams->getSwitchToBaudRate() == 57600)
    baudNum = 3;
      else if (myParams->getSwitchToBaudRate() == 115200)
    baudNum = 4;
      else
      {
    ArLog::log(ArLog::Normal, "Warning: SwitchToBaud is set to %d baud, ignoring.",
           myParams->getSwitchToBaudRate());
    ArLog::log(ArLog::Normal, "\tGood bauds are 9600 19200 38400 56800 116200.");
    myAsyncConnectState = 5;
    baudNum = -1;
    return 0;
      }
      if (baudNum != -1)
      {
    // now switch it over
    comInt(ArCommands::HOSTBAUD, baudNum);
    ArUtil::sleep(10);
    myAsyncConnectSentChangeBaud = true;
    myAsyncConnectStartedChangeBaud.setToNow();
    serConn->setBaud(myParams->getSwitchToBaudRate());
    //serConn->setBaud(19200);
    ArUtil::sleep(10);
    com(0);
    return 0;
      }
    }
    // if we did send the command then wait and see if we get any packets back
    else
    {
      packet = myReceiver.receivePacket(100);
      com(0);
      // if we got any packet we're good
      if (packet != NULL)
      {
    myAsyncConnectState = 5;
      }
      // if we didn't get it and its been 500 ms then fail
      else if (myAsyncConnectStartedChangeBaud.mSecSince() > 900)
      {
    ArLog::log(ArLog::Verbose, "Controller did not switch to baud, reset to %d baud.", myAsyncConnectStartBaud);
    serConn->setBaud(myAsyncConnectStartBaud);
    myAsyncConnectState = 5;
      }
      else
    return 0;
    }    
  }

  if (myAsyncConnectState == 5)
  {
    if (!myIsStabilizing)
      startStabilization();
    if (myStabilizingTime == 0 || 
    myStartedStabilizing.mSecSince() > myStabilizingTime)
    {
      finishedConnection();
      return 1;
    }
    else
      return 0;
  }

  // here we've gone beyond the initialization, so read set a time limit,
  // read in one packet, then if its a bad packet type, read in all the 
  // packets there are to read... if its a good packet, continue with sequence
  myAsyncConnectTimesTried++;
  ArLog::log(ArLog::Normal, "Syncing %d", myAsyncConnectState);
  mySender.com(myAsyncConnectState);
  //  packet = myReceiver.receivePacket(endTime.mSecTo());
  packet = myReceiver.receivePacket(1000);
  
  if (packet != NULL) 
  {
    ret = packet->getID();
    //printf("Got a packet %d\n", ret);
    
    if (ret == 50)
    {
      ArLog::log(ArLog::Normal, "Attempting to close previous connection.");
      comInt(ArCommands::CLOSE,1);
      while (endTime.mSecTo() > 0)
      {
    timeToWait = endTime.mSecTo();
    if (timeToWait < 0)
      timeToWait = 0;
    tempPacket = myReceiver.receivePacket(timeToWait);
    if (tempPacket != NULL)
      ArLog::log(ArLog::Verbose, "Got in another packet!");
    if (tempPacket != NULL && tempPacket->getID() == 50)
      comInt(ArCommands::CLOSE,1);
      }
      myAsyncConnectState = 0;
    } 
    else if (ret == 255)
    {
      ArLog::log(ArLog::Normal, "Attempting to correct syncCount");
      /*
      while (endTime.mSecTo() > 0)
      {
    timeToWait = endTime.mSecTo();
    if (timeToWait < 0)
      timeToWait = 0;
    tempPacket = myReceiver.receivePacket(timeToWait);
    if (tempPacket != NULL)
      ArLog::log(ArLog::Verbose, "Got in another packet!");
      }
      */
      while ((tempPacket = myReceiver.receivePacket(0)) != NULL);

      if (tempPacket != NULL && tempPacket->getID() == 0)
    myAsyncConnectState = 1;
      else
    myAsyncConnectState = 0;
      return 0;
    } 
    else if  (ret != myAsyncConnectState++)
    {
      myAsyncConnectState = 0;
    }
  }
  else 
  {
    ArLog::log(ArLog::Normal, "No packet.");
    myAsyncConnectNoPacketCount++;
    if ((myAsyncConnectNoPacketCount > 5
     && myAsyncConnectNoPacketCount >= myAsyncConnectTimesTried)
    || (myAsyncConnectNoPacketCount > 10))
    {
      ArLog::log(ArLog::Terse, "Could not connect, no robot responding.");
      failedConnect();
      return 2;
    }

      /* This code to connect the radio modems should never really be needed 
     so is being removed for now
     if (myAsyncConnectNoPacketCount >= 4)
     {
     
     if (tryHarderToConnect && dynamic_cast<ArSerialConnection *>(myConn))
     {
     ArLog::log(ArLog::Normal, 
     "Radio modem may not connected, trying to connect it.");
     ArUtil::sleep(1000);
     myConn->write("|||\15", strlen("!!!\15"));
     ArUtil::sleep(60);
     myConn->write("WMN\15", strlen("WMN\15"));
     ArUtil::sleep(60);
     myConn->write("WMS2\15", strlen("WMS2\15"));
     ArUtil::sleep(1000);
     } 
     else if (dynamic_cast<ArSerialConnection *>(myConn))
     {
     ArLog::log(ArLog::Normal,
     "Radio modem may not connected, trying to connect it.");
     myConn->write("WMS2\15", strlen("WMS2\15"));
     ArUtil::sleep(60);
     }
     }
      */
    
    // If we get no response first we dump close commands at the thing
    // in different bauds (if its a serial connection)
    if (myAsyncConnectNoPacketCount == 2 && 
    myAsyncConnectNoPacketCount >= myAsyncConnectTimesTried &&
    (serConn = dynamic_cast<ArSerialConnection *>(myConn)) != NULL)
    {
      int origBaud;
      ArLog::log(ArLog::Normal, "Trying to close possible old connection");
      origBaud = serConn->getBaud();
      serConn->setBaud(9600);
      comInt(ArCommands::CLOSE,1);
      ArUtil::sleep(3);
      serConn->setBaud(38400);
      comInt(ArCommands::CLOSE,1);
      ArUtil::sleep(3);
      serConn->setBaud(115200);
      comInt(ArCommands::CLOSE,1);
      ArUtil::sleep(3);
      serConn->setBaud(19200);
      comInt(ArCommands::CLOSE,1);
      ArUtil::sleep(3);
      serConn->setBaud(57600);
      comInt(ArCommands::CLOSE,1);
      ArUtil::sleep(3);
      serConn->setBaud(origBaud);
    }

    if (myAsyncConnectNoPacketCount > 3)
    {
      ArLog::log(ArLog::Normal,
         " Robot may be connected but not open, trying to dislodge.");
      mySender.comInt(ArCommands::OPEN,1);
    }
    
    myAsyncConnectState = 0;
  }
  // if we've connected and have a packet get the connection
  // information from it
  if (myAsyncConnectState == 3 && packet != NULL) 
  {
    char nameBuf[512];
    ArLog::log(ArLog::Terse, "Connected to robot.");
    packet->bufToStr(nameBuf, 512);
    myRobotName = nameBuf;
    ArLog::log(ArLog::Normal, "Name: %s", myRobotName.c_str());
    packet->bufToStr(nameBuf, 512);
    myRobotType = nameBuf;
    ArLog::log(ArLog::Normal, "Type: %s", myRobotType.c_str());
    packet->bufToStr(nameBuf, 512);
    myRobotSubType = nameBuf;
    strcpy(robotSubType, myRobotSubType.c_str());
    len = strlen(robotSubType);
    for (i = 0; i < len; i++)
      robotSubType[i] = tolower(robotSubType[i]);
    myRobotSubType = robotSubType;
    ArLog::log(ArLog::Normal, "Subtype: %s", myRobotSubType.c_str());
    ArUtil::sleep(getCycleTime());
    mySender.comInt(ArCommands::OPEN,1);
    if (!madeConnection())
    {
      mySender.comInt(ArCommands::CLOSE,1);
      failedConnect();
      return 2;
    }

    // now we return off so we can handle the rest of connecting, if
    // you're just using this for your own connections you can skip
    // this part because its really connected now
    myAsyncStartedConnection.setToNow();
    return asyncConnectHandler(tryHarderToConnect);
  }
  if (myAsyncConnectTimesTried > 50)
  {
    failedConnect();
    return 2;
  }
  return 0;
}

AREXPORT bool ArRobot::loadParamFile(const char *file)
{
  if (myParams != NULL)
    delete myParams;

  myParams = new ArRobotGeneric("");
  if (!myParams->parseFile(file, false, true))
  {
    ArLog::log(ArLog::Normal, "ArRobot::loadParamFile: Could not find file '%s' to load.", file);
    return false;
  }
  processParamFile();
  ArLog::log(ArLog::Normal, "Loaded robot parameters from %s.", file);
  return true;
}

AREXPORT void ArRobot::processParamFile(void)
{
  for (int i = 0; i < myParams->getNumSonar(); ++i)
  {
    //printf("sonar %d %d %d %d\n", i, myParams->getSonarX(i),
    //myParams->getSonarY(i), myParams->getSonarTh(i));
    if (mySonars.find(i) == mySonars.end())
    {
      mySonars[i] = new ArSensorReading(myParams->getSonarX(i),
                    myParams->getSonarY(i), 
                    myParams->getSonarTh(i));
      mySonars[i]->setIgnoreThisReading(true);
    }
    else
    {
      mySonars[i]->resetSensorPosition(myParams->getSonarX(i),
                      myParams->getSonarY(i), 
                      myParams->getSonarTh(i));
      mySonars[i]->setIgnoreThisReading(true);
    }
    if ((i + 1) > myNumSonar)
      myNumSonar = i + 1;
  }
  //myRobotType = myParams->getClassName();
  //myRobotSubType = myParams->getSubClassName();
  myAbsoluteMaxTransVel = myParams->getAbsoluteMaxVelocity();
  myAbsoluteMaxRotVel = myParams->getAbsoluteMaxRotVelocity();

  if (myParams->getRobotLengthFront() == 0)
    myRobotLengthFront = myParams->getRobotLength() / 2.0;
  else
    myRobotLengthFront = myParams->getRobotLengthFront();

  if (myParams->getRobotLengthRear() == 0)
    myRobotLengthRear = myParams->getRobotLength() / 2.0;
  else
    myRobotLengthRear = myParams->getRobotLengthRear();
}


AREXPORT bool ArRobot::madeConnection(void)
{
  std::string subTypeParamName;
  std::string paramFileName;
  bool loadedSubTypeParam;
  bool loadedNameParam;
  bool hadDefault = true;
  
  if (myParams != NULL)
    delete myParams;
  
  // Find the robot parameters to load and get them into the structure we have
  if (ArUtil::strcmp(myRobotSubType, "p2dx") == 0)
    myParams = new ArRobotP2DX;
  else if (ArUtil::strcmp(myRobotSubType, "p2ce") == 0)
    myParams = new ArRobotP2CE;
  else if (ArUtil::strcmp(myRobotSubType, "p2de") == 0)
    myParams = new ArRobotP2DXe;
  else if (ArUtil::strcmp(myRobotSubType, "p2df") == 0)
    myParams = new ArRobotP2DF;
  else if (ArUtil::strcmp(myRobotSubType, "p2d8") == 0)
    myParams = new ArRobotP2D8;
  else if (ArUtil::strcmp(myRobotSubType, "amigo") == 0)
    myParams = new ArRobotAmigo;
  else if (ArUtil::strcmp(myRobotSubType, "amigo-sh") == 0)
    myParams = new ArRobotAmigoSh;
  else if (ArUtil::strcmp(myRobotSubType, "p2at") == 0)
    myParams = new ArRobotP2AT;
  else if (ArUtil::strcmp(myRobotSubType, "p2at8") == 0)
    myParams = new ArRobotP2AT8;
  else if (ArUtil::strcmp(myRobotSubType, "p2it") == 0)
    myParams = new ArRobotP2IT;
  else if (ArUtil::strcmp(myRobotSubType, "p2pb") == 0)
    myParams = new ArRobotP2PB;
  else if (ArUtil::strcmp(myRobotSubType, "p2pp") == 0)
    myParams = new ArRobotP2PP;
  else if (ArUtil::strcmp(myRobotSubType, "p3at") == 0)
    myParams = new ArRobotP3AT;
  else if (ArUtil::strcmp(myRobotSubType, "p3dx") == 0)
    myParams = new ArRobotP3DX;
  else if (ArUtil::strcmp(myRobotSubType, "perfpb") == 0)
    myParams = new ArRobotPerfPB;
  else if (ArUtil::strcmp(myRobotSubType, "pion1m") == 0)
    myParams = new ArRobotPion1M;
  else if (ArUtil::strcmp(myRobotSubType, "pion1x") == 0)
    myParams = new ArRobotPion1X;
  else if (ArUtil::strcmp(myRobotSubType, "psos1m") == 0)
    myParams = new ArRobotPsos1M;
  else if (ArUtil::strcmp(myRobotSubType, "psos43m") == 0)
    myParams = new ArRobotPsos43M;
  else if (ArUtil::strcmp(myRobotSubType, "psos1x") == 0)
    myParams = new ArRobotPsos1X;
  else if (ArUtil::strcmp(myRobotSubType, "pionat") == 0)
    myParams = new ArRobotPionAT;
  else if (ArUtil::strcmp(myRobotSubType, "mappr") == 0)
    myParams = new ArRobotMapper;
  else if (ArUtil::strcmp(myRobotSubType, "powerbot") == 0)
    myParams = new ArRobotPowerBot;
  else if (ArUtil::strcmp(myRobotSubType, "p2d8+") == 0)
    myParams = new ArRobotP2D8Plus;
  else if (ArUtil::strcmp(myRobotSubType, "p2at8+") == 0)
    myParams = new ArRobotP2AT8Plus;
  else if (ArUtil::strcmp(myRobotSubType, "perfpb+") == 0)
    myParams = new ArRobotPerfPBPlus;
  else if (ArUtil::strcmp(myRobotSubType, "p3dx-sh") == 0)
    myParams = new ArRobotP3DXSH;
  else if (ArUtil::strcmp(myRobotSubType, "p3at-sh") == 0)
    myParams = new ArRobotP3ATSH;
  else if (ArUtil::strcmp(myRobotSubType, "p3atiw-sh") == 0)
    myParams = new ArRobotP3ATIWSH;
  else if (ArUtil::strcmp(myRobotSubType, "patrolbot-sh") == 0)
    myParams = new ArRobotPatrolBotSH;
  else if (ArUtil::strcmp(myRobotSubType, "peoplebot-sh") == 0)
    myParams = new ArRobotPeopleBotSH;
  else if (ArUtil::strcmp(myRobotSubType, "powerbot-sh") == 0)
    myParams = new ArRobotPowerBotSH;
  else if (ArUtil::strcmp(myRobotSubType, "wheelchair-sh") == 0)
    myParams = new ArRobotWheelchairSH;
  else
  {
    hadDefault = false;
    myParams = new ArRobotGeneric(myRobotName.c_str());
  }

  // load up the param file for the subtype
  paramFileName = Aria::getDirectory();
  paramFileName += "params/";
  paramFileName += myRobotSubType;
  paramFileName += ".p";
  if ((loadedSubTypeParam = myParams->parseFile(paramFileName.c_str(), true, true)))
      ArLog::log(ArLog::Normal, 
         "Loaded robot parameters from %s.p", 
         myRobotSubType.c_str());
  /* If the above line was replaced with this one line
     paramFile->load(); 
     then the sonartest (and lots of other stuff probably) would break
  */
  // then the one for the particular name, if we can
  paramFileName = Aria::getDirectory();
  paramFileName += "params/";
  paramFileName += myRobotName;
  paramFileName += ".p";
  if ((loadedNameParam = myParams->parseFile(paramFileName.c_str(),
                         true, true)))
  {
    if (loadedSubTypeParam)
      ArLog::log(ArLog::Normal, 
 "Loaded robot parameters from %s.p on top of %s.p robot parameters", 
         myRobotName.c_str(), myRobotSubType.c_str());
    else
      ArLog::log(ArLog::Normal, "Loaded robot parameters from %s.p", 
         myRobotName.c_str());
  }
   
  if (!loadedSubTypeParam && !loadedNameParam)
  {
    if (hadDefault)
      ArLog::log(ArLog::Normal, "Using default parameters for a %s robot", 
         myRobotSubType.c_str());
    else
    {
      ArLog::log(ArLog::Terse, "Error: Have no parameters for this robot, bad configuration or out of date Aria");
      // in the default state (not connecting if we don't have params)
      // we will return false... if we can connect without params then
      // we'll keep going (this really shouldn't be used except by
      // downloaders and such)
      if (!myConnectWithNoParams)
    return false;
    }
  }

  processParamFile();

  if (myParams->getRequestIOPackets() || myRequestedIOPackets)
    comInt(ArCommands::IOREQUEST, 2);

  if(myParams->getRequestEncoderPackets() || myRequestedEncoderPackets)
    comInt(ArCommands::ENCODER, 2);

  return true;
}

AREXPORT void ArRobot::requestEncoderPackets(void)
{
  comInt(ArCommands::ENCODER, 2);
  myRequestedEncoderPackets = true;
}

AREXPORT void ArRobot::requestIOPackets(void)
{
  comInt(ArCommands::IOREQUEST, 2);
  myRequestedIOPackets = true;
}

AREXPORT void ArRobot::stopEncoderPackets(void)
{
  comInt(ArCommands::ENCODER, 0);
  myRequestedEncoderPackets = false;
}

AREXPORT void ArRobot::stopIOPackets(void)
{
  comInt(ArCommands::IOREQUEST, 0);
  myRequestedIOPackets = false;
}

AREXPORT void ArRobot::startStabilization(void)
{
  std::list<ArFunctor *>::iterator it;
  myIsStabilizing = true;
  myStartedStabilizing.setToNow();

  for (it = myStabilizingCBList.begin(); 
       it != myStabilizingCBList.end(); 
       it++)
    (*it)->invoke();

}

AREXPORT void ArRobot::finishedConnection(void)
{
  std::list<ArFunctor *>::iterator it;

  myIsStabilizing = false;
  myIsConnected = true;
  myAsyncConnectFlag = false;
  myBlockingConnectRun = false;
  resetOdometer();
  
  for (it = myConnectCBList.begin(); it != myConnectCBList.end(); it++)
    (*it)->invoke();
  myLastPacketReceivedTime.setToNow();

  wakeAllConnWaitingThreads();
}

AREXPORT void ArRobot::failedConnect(void)
{
  std::list<ArFunctor *>::iterator it;  
  
  myAsyncConnectFlag = false;
  myBlockingConnectRun = false;
  ArLog::log(ArLog::Terse, "Failed to connect to robot.");
  myIsConnected = false;
  for (it = myFailedConnectCBList.begin(); 
       it != myFailedConnectCBList.end(); 
       it++)
    (*it)->invoke();

  if (myConn != NULL)
    myConn->close();
  wakeAllConnOrFailWaitingThreads();
}

AREXPORT bool ArRobot::disconnect(void)
{
  std::list<ArFunctor *>::iterator it;  
  bool ret;
  ArSerialConnection *serConn;

  if (!myIsConnected && !myIsStabilizing)
    return true;
  
  ArLog::log(ArLog::Terse, "Disconnecting from robot.");
  myNoTimeWarningThisCycle = true;
  myIsConnected = false;
  myIsStabilizing = false;
  if (myIsConnected)
  {
    for (it = myDisconnectNormallyCBList.begin(); 
     it != myDisconnectNormallyCBList.end(); 
     it++)
      (*it)->invoke();
  }
  ret = mySender.comInt(ArCommands::CLOSE, 1);
  ArUtil::sleep(1000);
  if (ret == true)
  {
    if (myConn != NULL)
    {
      ret = myConn->close();
      if ((serConn = dynamic_cast<ArSerialConnection *>(myConn)) != NULL)
        serConn->setBaud(myAsyncConnectStartBaud);
    } 
    else
      ret = false;
  } 
  else if (myConn != NULL)
    myConn->close();
    
  return ret;
}

AREXPORT void ArRobot::dropConnection(void)
{
  std::list<ArFunctor *>::iterator it;  

  if (!myIsConnected)
    return;

  ArLog::log(ArLog::Terse, "Lost connection to the robot because of error.");
  myIsConnected = false;
  for (it = myDisconnectOnErrorCBList.begin(); 
       it != myDisconnectOnErrorCBList.end(); 
       it++)
    (*it)->invoke();

  if (myConn != NULL)
    myConn->close();
  return;
}

AREXPORT void ArRobot::cancelConnection(void)
{
  //std::list<ArFunctor *>::iterator it;  

  ArLog::log(ArLog::Verbose, "Cancelled connection to the robot because of command.");
  myIsConnected = false;
  myNoTimeWarningThisCycle = true;
  myIsStabilizing = false;
  return;
}

AREXPORT void ArRobot::stop(void)
{
  comInt(ArCommands::VEL, 0);
  comInt(ArCommands::RVEL, 0);
  setVel(0);
  setRotVel(0);
  myLastActionTransVal = 0;
  myLastActionRotStopped = true;
  myLastActionRotHeading = false;
}

AREXPORT void ArRobot::setVel(double velocity)
{
  myTransType = TRANS_VEL;
  myTransVal = velocity;
  myTransVal2 = 0;
  myTransSetTime.setToNow();
}

AREXPORT void ArRobot::setVel2(double leftVelocity, double rightVelocity)
{
  myTransType = TRANS_VEL2;
  myTransVal = leftVelocity;
  myTransVal2 = rightVelocity;
  myRotType = ROT_IGNORE;
  myRotVal = 0;
  myTransSetTime.setToNow();
}

AREXPORT void ArRobot::move(double distance)
{
  myTransType = TRANS_DIST_NEW;
  myTransDistStart = getPose();
  myTransVal = distance;
  myTransVal2 = 0;
  myTransSetTime.setToNow();
}

AREXPORT bool ArRobot::isMoveDone(double delta)
{
  if (fabs(delta) < 0.001)
    delta = myMoveDoneDist;
  if (myTransType != TRANS_DIST && myTransType != TRANS_DIST_NEW)
    return true;        // no distance command operative
  if (myTransDistStart.findDistanceTo(getPose()) <
      fabs(myTransVal) - delta)
    return false;
  return true;
}


AREXPORT bool ArRobot::isHeadingDone(double delta) const
{
  if (fabs(delta) < 0.001)
    delta = myHeadingDoneDiff;
  if (myRotType != ROT_HEADING)
    return true;        // no heading command operative
  if(fabs(ArMath::subAngle(getTh(), myRotVal)) > delta)
    return false;
  return true;
}



AREXPORT void ArRobot::setHeading(double heading)
{
  myRotVal = heading;
  myRotType = ROT_HEADING;
  myRotSetTime.setToNow();
  if (myTransType == TRANS_VEL2)
  {
    myTransType = TRANS_IGNORE;
    myTransVal = 0;
    myTransVal2 = 0;
  }
}

AREXPORT void ArRobot::setRotVel(double velocity)
{
  myRotVal = velocity;
  myRotType = ROT_VEL;
  myRotSetTime.setToNow();
  if (myTransType == TRANS_VEL2)
  {
    myTransType = TRANS_IGNORE;
    myTransVal = 0;
    myTransVal2 = 0;
  }
}

AREXPORT void ArRobot::setDeltaHeading(double deltaHeading)
{
  myRotVal = ArMath::addAngle(getTh(), deltaHeading);
  myRotType = ROT_HEADING;
  myRotSetTime.setToNow();
  if (myTransType == TRANS_VEL2)
  {
    myTransType = TRANS_IGNORE;
    myTransVal = 0;
    myTransVal2 = 0;
  }
}

AREXPORT bool ArRobot::setAbsoluteMaxTransVel(double maxVel)
{
  if (maxVel < 0)
    return false;
  myAbsoluteMaxTransVel = maxVel;
  if (getTransVelMax() > myAbsoluteMaxTransVel)
    setTransVelMax(myAbsoluteMaxTransVel);
  return true;
}

AREXPORT bool ArRobot::setAbsoluteMaxTransAccel(double maxAccel)
{
  if (maxAccel < 0)
    return false;
  myAbsoluteMaxTransAccel = maxAccel;
  if (getTransAccel() > myAbsoluteMaxTransAccel)
    setTransAccel(myAbsoluteMaxTransAccel);
  return true;
}

AREXPORT bool ArRobot::setAbsoluteMaxTransDecel(double maxDecel)
{
  if (maxDecel < 0)
    return false;
  myAbsoluteMaxTransDecel = maxDecel;
  if (getTransDecel() > myAbsoluteMaxTransDecel)
    setTransDecel(myAbsoluteMaxTransDecel);
  return true;
}

AREXPORT bool ArRobot::setAbsoluteMaxRotVel(double maxVel)
{
  if (maxVel < 0)
    return false;
  myAbsoluteMaxRotVel = maxVel;
  if (getRotVelMax() > myAbsoluteMaxRotVel)
    setRotVelMax(myAbsoluteMaxRotVel);
  return true;
}

AREXPORT bool ArRobot::setAbsoluteMaxRotAccel(double maxAccel)
{
  if (maxAccel < 0)
    return false;
  myAbsoluteMaxRotAccel = maxAccel;
  if (getRotAccel() > myAbsoluteMaxRotAccel)
    setRotAccel(myAbsoluteMaxRotAccel);
  return true;
}

AREXPORT bool ArRobot::setAbsoluteMaxRotDecel(double maxDecel)
{
  if (maxDecel < 0)
    return false;
  myAbsoluteMaxRotDecel = maxDecel;
  if (getRotDecel() > myAbsoluteMaxRotDecel)
    setRotDecel(myAbsoluteMaxRotDecel);
  return true;
}

AREXPORT int ArRobot::getIOAnalog(int num) const
{
  if (num <= getIOAnalogSize())
    return myIOAnalog[num];
  else
    return 0;
}

AREXPORT double ArRobot::getIOAnalogVoltage(int num) const
{
  if (num <= getIOAnalogSize())
  {
    return (myIOAnalog[num] & 0xfff) * .0048828;
  }
  else
    return 0;
}

AREXPORT unsigned char ArRobot::getIODigIn(int num) const
{
  if (num <= getIODigInSize())
    return myIODigIn[num];
  else
    return (unsigned char) 0;
}

AREXPORT unsigned char  ArRobot::getIODigOut(int num) const
{
  if (num <= getIODigOutSize())
    return myIODigOut[num];
  else
    return (unsigned char) 0;
}

AREXPORT const ArRobotParams *ArRobot::getRobotParams(void) const
{
  return myParams;
}

AREXPORT const ArRobotConfigPacketReader *ArRobot::getOrigRobotConfig(void) const
{
  return myOrigRobotConfig;
}
  
AREXPORT void ArRobot::addPacketHandler(
    ArRetFunctor1<bool, ArRobotPacket *> *functor, 
    ArListPos::Pos position) 
{
  if (position == ArListPos::FIRST)
    myPacketHandlerList.push_front(functor);
  else if (position == ArListPos::LAST)
    myPacketHandlerList.push_back(functor);
  else
    ArLog::log(ArLog::Terse, "ArRobot::addPacketHandler: Invalid position.");

}

AREXPORT void ArRobot::remPacketHandler(
    ArRetFunctor1<bool, ArRobotPacket *> *functor)
{
  myPacketHandlerList.remove(functor);
}

AREXPORT void ArRobot::addConnectCB(ArFunctor *functor, 
                    ArListPos::Pos position)
{
  if (position == ArListPos::FIRST)
    myConnectCBList.push_front(functor);
  else if (position == ArListPos::LAST)
    myConnectCBList.push_back(functor);
  else
    ArLog::log(ArLog::Terse, 
           "ArRobot::addConnectCallback: Invalid position.");
}

AREXPORT void ArRobot::remConnectCB(ArFunctor *functor)
{
  myConnectCBList.remove(functor);
}


AREXPORT void ArRobot::addFailedConnectCB(ArFunctor *functor, 
                      ArListPos::Pos position)
{
  if (position == ArListPos::FIRST)
    myFailedConnectCBList.push_front(functor);
  else if (position == ArListPos::LAST)
    myFailedConnectCBList.push_back(functor);
  else
    ArLog::log(ArLog::Terse, 
           "ArRobot::addFailedCallback: Invalid position.");
}

AREXPORT void ArRobot::remFailedConnectCB(ArFunctor *functor)
{
  myFailedConnectCBList.remove(functor);
}

AREXPORT void ArRobot::addDisconnectNormallyCB(ArFunctor *functor, 
                           ArListPos::Pos position)
{
  if (position == ArListPos::FIRST)
    myDisconnectNormallyCBList.push_front(functor);
  else if (position == ArListPos::LAST)
    myDisconnectNormallyCBList.push_back(functor);
  else
    ArLog::log(ArLog::Terse, 
           "ArRobot::addDisconnectNormallyCB: Invalid position.");
}

AREXPORT void ArRobot::remDisconnectNormallyCB(ArFunctor *functor)
{
  myDisconnectNormallyCBList.remove(functor);
}

AREXPORT void ArRobot::addDisconnectOnErrorCB(ArFunctor *functor, 
                          ArListPos::Pos position)
{
  if (position == ArListPos::FIRST)
    myDisconnectOnErrorCBList.push_front(functor);
  else if (position == ArListPos::LAST)
    myDisconnectOnErrorCBList.push_back(functor);
  else
    ArLog::log(ArLog::Terse, 
           "ArRobot::addDisconnectOnErrorCB: Invalid position");
}

AREXPORT void ArRobot::remDisconnectOnErrorCB(ArFunctor *functor)
{
  myDisconnectOnErrorCBList.remove(functor);
}

AREXPORT void ArRobot::addRunExitCB(ArFunctor *functor,
                    ArListPos::Pos position)
{
  if (position == ArListPos::FIRST)
    myRunExitCBList.push_front(functor);
  else if (position == ArListPos::LAST)
    myRunExitCBList.push_back(functor);
  else
    ArLog::log(ArLog::Terse, "ArRobot::addRunExitCB: Invalid position");
}

AREXPORT void ArRobot::remRunExitCB(ArFunctor *functor)
{
  myRunExitCBList.remove(functor);
}

AREXPORT void ArRobot::addStabilizingCB(ArFunctor *functor, 
                       ArListPos::Pos position)
{
  if (position == ArListPos::FIRST)
    myStabilizingCBList.push_front(functor);
  else if (position == ArListPos::LAST)
    myStabilizingCBList.push_back(functor);
  else
    ArLog::log(ArLog::Terse, 
           "ArRobot::addConnectCallback: Invalid position.");
}

AREXPORT void ArRobot::remStabilizingCB(ArFunctor *functor)
{
  myStabilizingCBList.remove(functor);
}


AREXPORT std::list<ArFunctor *> * ArRobot::getRunExitListCopy()
{
  return(new std::list<ArFunctor *>(myRunExitCBList));
}

AREXPORT ArRobot::WaitState ArRobot::waitForConnect(unsigned int msecs)
{
  int ret;

  if (isConnected())
    return(WAIT_CONNECTED);

  if (msecs == 0)
    ret=myConnectCond.wait();
  else
    ret=myConnectCond.timedWait(msecs);

  if (ret == ArCondition::STATUS_WAIT_INTR)
    return(WAIT_INTR);
  else if (ret == ArCondition::STATUS_WAIT_TIMEDOUT)
    return(WAIT_TIMEDOUT);
  else if (ret == 0)
    return(WAIT_CONNECTED);
  else
    return(WAIT_FAIL);
}

AREXPORT ArRobot::WaitState
ArRobot::waitForConnectOrConnFail(unsigned int msecs)
{
  int ret;

  if (isConnected())
    return(WAIT_CONNECTED);

  if (msecs == 0)
    ret=myConnOrFailCond.wait();
  else
    ret=myConnOrFailCond.timedWait(msecs);

  if (ret == ArCondition::STATUS_WAIT_INTR)
    return(WAIT_INTR);
  else if (ret == ArCondition::STATUS_WAIT_TIMEDOUT)
    return(WAIT_TIMEDOUT);
  else if (ret == 0)
  {
    if (isConnected())
      return(WAIT_CONNECTED);
    else
      return(WAIT_FAILED_CONN);
  }
  else
    return(WAIT_FAIL);
}

AREXPORT ArRobot::WaitState ArRobot::waitForRunExit(unsigned int msecs)
{
  int ret;

  if (!isRunning())
    return(WAIT_RUN_EXIT);

  if (msecs == 0)
    ret=myRunExitCond.wait();
  else
    ret=myRunExitCond.timedWait(msecs);

  if (ret == ArCondition::STATUS_WAIT_INTR)
    return(WAIT_INTR);
  else if (ret == ArCondition::STATUS_WAIT_TIMEDOUT)
    return(WAIT_TIMEDOUT);
  else if (ret == 0)
    return(WAIT_RUN_EXIT);
  else
    return(WAIT_FAIL);
}

AREXPORT void ArRobot::wakeAllWaitingThreads()
{
  wakeAllConnWaitingThreads();
  wakeAllRunExitWaitingThreads();
}

AREXPORT void ArRobot::wakeAllConnWaitingThreads()
{
  myConnectCond.broadcast();
  myConnOrFailCond.broadcast();
}

AREXPORT void ArRobot::wakeAllConnOrFailWaitingThreads()
{
  myConnOrFailCond.broadcast();
}

AREXPORT void ArRobot::wakeAllRunExitWaitingThreads()
{
  myRunExitCond.broadcast();
}

AREXPORT ArSyncTask *ArRobot::getSyncTaskRoot(void)
{
  return mySyncTaskRoot;
}

AREXPORT bool ArRobot::addUserTask(const char *name, int position, 
                      ArFunctor *functor, 
                      ArTaskState::State *state)
{
  ArSyncTask *proc;
  if (mySyncTaskRoot == NULL)
    return false;

  proc = mySyncTaskRoot->findNonRecursive("User Tasks");
  if (proc == NULL)
    return false;
  
  proc->addNewLeaf(name, position, functor, state);
  return true;  
}

AREXPORT void ArRobot::remUserTask(const char *name)
{
  ArSyncTask *proc;
  ArSyncTask *userProc;

  if (mySyncTaskRoot == NULL)
    return;

  proc = mySyncTaskRoot->findNonRecursive("User Tasks");
  if (proc == NULL)
    return;
  
  userProc = proc->findNonRecursive(name);
  if (userProc == NULL)
    return;

  
  delete userProc;

}

AREXPORT void ArRobot::remUserTask(ArFunctor *functor)
{
  ArSyncTask *proc;
  ArSyncTask *userProc;

  if (mySyncTaskRoot == NULL)
    return;

  proc = mySyncTaskRoot->findNonRecursive("User Tasks");
  if (proc == NULL)
    return;
  
  
  userProc = proc->findNonRecursive(functor);
  if (userProc == NULL)
    return;

  
  delete userProc;

}

AREXPORT bool ArRobot::addSensorInterpTask(const char *name, int position, 
                          ArFunctor *functor,
                          ArTaskState::State *state)
{
  ArSyncTask *proc;
  if (mySyncTaskRoot == NULL)
    return false;

  proc = mySyncTaskRoot->findNonRecursive("Sensor Interp");
  if (proc == NULL)
    return false;
  
  proc->addNewLeaf(name, position, functor, state);
  return true;  
}

AREXPORT void ArRobot::remSensorInterpTask(const char *name)
{
  ArSyncTask *proc;
  ArSyncTask *sensorInterpProc;

  if (mySyncTaskRoot == NULL)
    return;

  proc = mySyncTaskRoot->findNonRecursive("Sensor Interp");
  if (proc == NULL)
    return;
  
  sensorInterpProc = proc->findNonRecursive(name);
  if (sensorInterpProc == NULL)
    return;

  
  delete sensorInterpProc;

}

AREXPORT void ArRobot::remSensorInterpTask(ArFunctor *functor)
{
  ArSyncTask *proc;
  ArSyncTask *sensorInterpProc;

  if (mySyncTaskRoot == NULL)
    return;

  proc = mySyncTaskRoot->findNonRecursive("Sensor Interp");
  if (proc == NULL)
    return;
  
  
  sensorInterpProc = proc->findNonRecursive(functor);
  if (sensorInterpProc == NULL)
    return;

  
  delete sensorInterpProc;

}

AREXPORT void ArRobot::logUserTasks(void) const
{
  ArSyncTask *proc;
  if (mySyncTaskRoot == NULL)
    return;

  proc = mySyncTaskRoot->findNonRecursive("User Tasks");
  if (proc == NULL)
    return;

  proc->log();
}

AREXPORT void ArRobot::logAllTasks(void) const
{
  if (mySyncTaskRoot != NULL)
    mySyncTaskRoot->log();
}

AREXPORT ArSyncTask *ArRobot::findUserTask(const char *name)
{
  ArSyncTask *proc;
  if (mySyncTaskRoot == NULL)
    return NULL;

  proc = mySyncTaskRoot->findNonRecursive("User Tasks");
  if (proc == NULL)
    return NULL;

  return proc->find(name);
}

AREXPORT ArSyncTask *ArRobot::findUserTask(ArFunctor *functor)
{
  ArSyncTask *proc;
  if (mySyncTaskRoot == NULL)
    return NULL;

  proc = mySyncTaskRoot->findNonRecursive("User Tasks");
  if (proc == NULL)
    return NULL;

  return proc->find(functor);
}

AREXPORT ArSyncTask *ArRobot::findTask(const char *name)
{
  if (mySyncTaskRoot != NULL)
    return mySyncTaskRoot->find(name);
  else
    return NULL;

}

AREXPORT ArSyncTask *ArRobot::findTask(ArFunctor *functor)
{
  if (mySyncTaskRoot != NULL)
    return mySyncTaskRoot->find(functor);
  else
    return NULL;

}

AREXPORT bool ArRobot::addAction(ArAction *action, int priority)
{
  if (action == NULL)
  {
    ArLog::log(ArLog::Terse, 
      "ArRobot::addAction: an attempt was made to add a NULL action pointer");
    return false;
  }
  
  action->setRobot(this);
  myActions.insert(std::pair<int, ArAction *>(priority, action));
  return true;
}

AREXPORT bool ArRobot::remAction(const char *actionName)
{
  ArResolver::ActionMap::iterator it;
  ArAction *act;

  for (it = myActions.begin(); it != myActions.end(); ++it)
  {
    act = (*it).second;
    if (strcmp(actionName, act->getName()) == 0)
      break;
  }
  if (it != myActions.end())
  {
    myActions.erase(it);
    return true;
  }
  return false;

}

AREXPORT bool ArRobot::remAction(ArAction *action)
{
  ArResolver::ActionMap::iterator it;
  ArAction *act;

  for (it = myActions.begin(); it != myActions.end(); ++it)
  {
    act = (*it).second;
    if (act == action)
      break;
  }
  if (it != myActions.end())
  {
    myActions.erase(it);
    return true;
  }
  return false;

}


AREXPORT ArAction *ArRobot::findAction(const char *actionName)
{
  ArResolver::ActionMap::reverse_iterator it;
  ArAction *act;
  
  for (it = myActions.rbegin(); it != myActions.rend(); ++it)
  {
    act = (*it).second;
    if (strcmp(actionName, act->getName()) == 0)
      return act;
  }
  return NULL;
}

AREXPORT ArResolver::ActionMap *ArRobot::getActionMap(void)
{
  return &myActions;
}

AREXPORT void ArRobot::deactivateActions(void)
{
  ArResolver::ActionMap *am;
  ArResolver::ActionMap::iterator amit;
  
  am = getActionMap();
  if (am == NULL)
  {
    ArLog::log(ArLog::Terse, 
            "ArRobot::deactivateActions: NULL action map... failed.");
    return;
  }
  for (amit = am->begin(); amit != am->end(); amit++)
    (*amit).second->deactivate();
  

}


AREXPORT void ArRobot::logActions(bool logDeactivated) const
{
  ArResolver::ActionMap::const_reverse_iterator it;
  int lastPriority;
  bool first = true;
  const ArAction *action;

  if (logDeactivated)
    ArLog::log(ArLog::Terse, "The action list (%d total):", 
           myActions.size());
  else
    ArLog::log(ArLog::Terse, "The active action list:");

  for (it = myActions.rbegin(); it != myActions.rend(); ++it)
  {
    action = (*it).second;
    if ((logDeactivated || action->isActive()) &&
    (first || lastPriority != (*it).first))
    {
      ArLog::log(ArLog::Terse, "Priority %d:", (*it).first);
      first = false;
      lastPriority = (*it).first;
    }
    action->log(false);
  }
}

AREXPORT ArResolver *ArRobot::getResolver(void)
{
  return myResolver;
}

AREXPORT void ArRobot::setResolver(ArResolver *resolver)
{
  if (myOwnTheResolver)
  {
    delete myResolver;
    myResolver = NULL;
  }

  myResolver = resolver;
}

AREXPORT void ArRobot::stateReflector(void)
{
  short transVal;
  short transVal2;
  short maxVel;
  short maxNegVel;
  double maxTransVel;
  double maxNegTransVel;
  double maxRotVel;
  double transAccel;
  double transDecel;
  short rotVal;
  double rotAccel;
  double rotDecel;
  bool rotStopped = false;
  bool rotHeading = false;
  double encTh;
  double rawTh;


  if (!myIsConnected)
    return;

  myTryingToMove = false;

  // if this is true actions can't go
  if ((myTransType != TRANS_NONE && myDirectPrecedenceTime == 0) ||
      (myTransType != TRANS_NONE && myDirectPrecedenceTime != 0 && 
       myTransSetTime.mSecSince() < myDirectPrecedenceTime))
  {
    myActionTransSet = false;
    transVal = ArMath::roundShort(myTransVal);
    transVal2 = 0;

    if (hasSettableVelMaxes() && 
    ArMath::fabs(myLastSentTransVelMax - myTransVelMax) >= 1)
    {
      comInt(ArCommands::SETV,
         ArMath::roundShort(myTransVelMax));
      myLastSentTransVelMax = myTransVelMax;
      if (myLogMovementSent)
    ArLog::log(ArLog::Normal, "Non-action trans max vel of %d", 
           ArMath::roundShort(myTransVelMax));
    }

    if (hasSettableAccsDecs() && ArMath::fabs(myTransAccel) > 1 && 
    ArMath::fabs(myLastSentTransAccel - myTransAccel) >= 1)
    {
      comInt(ArCommands::SETA,
         ArMath::roundShort(myTransAccel));
      myLastSentTransAccel = myTransAccel;
      if (myLogMovementSent)
    ArLog::log(ArLog::Normal, "Non-action trans accel of %d", 
           ArMath::roundShort(myTransAccel));
    }

    if (hasSettableAccsDecs() && ArMath::fabs(myTransDecel) > 1 &&
    ArMath::fabs(myLastSentTransDecel - myTransDecel) >= 1)
    {
      comInt(ArCommands::SETA,
         -ArMath::roundShort(myTransDecel));
      myLastSentTransDecel = myTransDecel;
      if (myLogMovementSent)
    ArLog::log(ArLog::Normal, "Non-action trans decel of %d", 
           -ArMath::roundShort(myTransDecel));
    }

    if (myTransType == TRANS_VEL)
    {
      maxVel = ArMath::roundShort(myTransVelMax);
      if (transVal > maxVel)
    transVal = maxVel;
      if (transVal < -maxVel)
    transVal = -maxVel;
      if (myLastTransVal != transVal || myLastTransType != myTransType ||
      (myLastTransSent.mSecSince() >= myStateReflectionRefreshTime))
      {
    comInt(ArCommands::VEL, ArMath::roundShort(transVal));
    myLastTransSent.setToNow();
    if (myLogMovementSent)
      ArLog::log(ArLog::Normal, "Non-action trans vel of %d", 
             ArMath::roundShort(transVal));
    //printf("Sent command vel!\n");
      }
      if (fabs((double)transVal) > (double).5)
    myTryingToMove = true;
    }
    else if (myTransType == TRANS_VEL2)
    {
      if (ArMath::roundShort(myTransVal/myParams->getVel2Divisor()) > 128)
    transVal = 128;
      else if (ArMath::roundShort(myTransVal/myParams->getVel2Divisor()) < -128)
    transVal = -128;
      else 
    transVal = ArMath::roundShort(myTransVal/myParams->getVel2Divisor());
      if (ArMath::roundShort(myTransVal2/myParams->getVel2Divisor()) > 128)
    transVal2 = 128;
      else if (ArMath::roundShort(myTransVal2/myParams->getVel2Divisor()) < -128)
    transVal2 = -128;
      else 
    transVal2 = ArMath::roundShort(myTransVal2/myParams->getVel2Divisor());
      if (myLastTransVal != transVal || myLastTransVal2 != transVal2 || 
      myLastTransType != myTransType ||
      (myLastTransSent.mSecSince() >= myStateReflectionRefreshTime))
      {
    com2Bytes(ArCommands::VEL2, transVal, transVal2);
    myLastTransSent.setToNow();
    if (myLogMovementSent)
      ArLog::log(ArLog::Normal, "Non-action vel2 of %d %d", 
             transVal, transVal2);
    //printf("Sent command vel2!\n");
      }
      if (fabs((double)transVal) > (double).5 || fabs((double)transVal2) > (double).5)
    myTryingToMove = true;
    }
    else if (myTransType == TRANS_DIST_NEW || myTransType == TRANS_DIST)
    {
      // if the robot doesn't have its own distance command
      if (!myParams->hasMoveCommand())
      {
    double distGone;
    double distToGo;
    double vel;

    myTransType = TRANS_DIST;
    distGone = myTransDistStart.findDistanceTo(getPose());
    distToGo = fabs(fabs(myTransVal) - distGone);
    if (distGone > fabs(myTransVal) || 
        (distToGo < 10 && fabs(getVel()) < 30))
    {
      comInt(ArCommands::VEL, 0);
      myTransType = TRANS_VEL;
      myTransVal = 0;
    }
    else
      myTryingToMove = true;
    vel = sqrt(distToGo * 200 * 2);
    if (vel > getTransVelMax())
      vel = getTransVelMax();
    if (myTransVal < 0)
      vel *= -1;
    comInt(ArCommands::VEL, ArMath::roundShort(vel));
    if (myLogMovementSent)
      ArLog::log(ArLog::Normal, "Non-action move-helper of %d", 
             ArMath::roundShort(vel));
      }
      else if (myParams->hasMoveCommand() && myTransType == TRANS_DIST_NEW) 
      {
    comInt(ArCommands::MOVE, transVal);
    myLastTransSent.setToNow();
    myTransType = TRANS_DIST;
    if (myLogMovementSent)
      ArLog::log(ArLog::Normal, "Non-action move of %d", 
             transVal);
    myTryingToMove = true;
      }
      else if (myTransType == TRANS_DIST && 
      (myLastTransSent.mSecSince() >= myStateReflectionRefreshTime))
      {
    com(0);
    myLastPulseSent.setToNow();
    myLastTransSent.setToNow();
    //printf("Sent pulse for dist!\n");
    if (myLogMovementSent)
      ArLog::log(ArLog::Normal, "Non-action pulse for dist");
      }
      //printf("Sent command move!\n");
    }
    else if (myTransType == TRANS_IGNORE)
    {
      //printf("No trans command sent\n");
    }
    else
      ArLog::log(ArLog::Terse, 
         "ArRobot::stateReflector: Invalid translational type %d.",
         myTransType);
    myLastTransVal = transVal;
    myLastTransVal2 = transVal2;
    myLastTransType = myTransType;
  }
  else // if actions can go
  {
    if (hasSettableVelMaxes() && 
    ArMath::fabs(myLastSentTransVelMax - myTransVelMax) >= 1)
    {
      comInt(ArCommands::SETV,
         ArMath::roundShort(myTransVelMax));
      myLastSentTransVelMax = myTransVelMax;
      if (myLogMovementSent)
    ArLog::log(ArLog::Normal, "Action-but-robot trans max vel of %d", 
           ArMath::roundShort(myTransVelMax));
    }
    
    // first we'll handle all of the accel decel things
    if (myActionDesired.getTransAccelStrength() >= 
    ArActionDesired::MIN_STRENGTH)
    {
      transAccel = ArMath::roundShort(myActionDesired.getTransAccel());
      if (hasSettableAccsDecs() && ArMath::fabs(transAccel) > 1 &&
      ArMath::fabs(myLastSentTransAccel - transAccel) >= 1)
      {
    comInt(ArCommands::SETA,
           ArMath::roundShort(transAccel));
    myLastSentTransAccel = transAccel;
    if (myLogMovementSent)
      ArLog::log(ArLog::Normal, "Action trans accel of %d", 
             ArMath::roundShort(transAccel));
      }
    }
    else if (hasSettableAccsDecs() && ArMath::fabs(myTransAccel) > 1 && 
    ArMath::fabs(myLastSentTransAccel - myTransAccel) >= 1)
    {
      comInt(ArCommands::SETA,
         ArMath::roundShort(myTransAccel));
      myLastSentTransAccel = myTransAccel;
      if (myLogMovementSent)
    ArLog::log(ArLog::Normal, "Action-but-robot trans accel of %d", 
           ArMath::roundShort(myTransAccel));
    }

    if (myActionDesired.getTransDecelStrength() >=
    ArActionDesired::MIN_STRENGTH)
    {
      transDecel = ArMath::roundShort(myActionDesired.getTransDecel());
      if (hasSettableAccsDecs() && ArMath::fabs(transDecel) > 1 &&
      ArMath::fabs(myLastSentTransDecel - transDecel) >= 1)
      {
    comInt(ArCommands::SETA,
           -ArMath::roundShort(transDecel));
    myLastSentTransDecel = transDecel;
    if (myLogMovementSent)
      ArLog::log(ArLog::Normal, "Action trans decel of %d", 
             -ArMath::roundShort(transDecel));
      }
    }
    else if (hasSettableAccsDecs() && ArMath::fabs(myTransDecel) > 1 &&
    ArMath::fabs(myLastSentTransDecel - myTransDecel) >= 1)
    {
      comInt(ArCommands::SETA,
         -ArMath::roundShort(myTransDecel));
      myLastSentTransDecel = myTransDecel;
      if (myLogMovementSent)
    ArLog::log(ArLog::Normal, "Action-but-robot trans decel of %d", 
           -ArMath::roundShort(myTransDecel));
    }

    if (myActionDesired.getMaxVelStrength() >= ArActionDesired::MIN_STRENGTH)
    {
      maxTransVel = myActionDesired.getMaxVel();
      if (maxTransVel > myTransVelMax)
    maxTransVel = myTransVelMax;
    }
    else
      maxTransVel = myTransVelMax;

    if (myActionDesired.getMaxNegVelStrength() >= 
    ArActionDesired::MIN_STRENGTH)
    {
      maxNegTransVel = -ArMath::fabs(myActionDesired.getMaxNegVel());
      if (maxNegTransVel < -myTransVelMax)
    maxNegTransVel = -myTransVelMax;
    }
    else
      maxNegTransVel = -myTransVelMax;
    
    if (myActionDesired.getVelStrength() >= ArActionDesired::MIN_STRENGTH)
    {
      transVal = ArMath::roundShort(myActionDesired.getVel());
      myActionTransSet = true;
    }
    else
    {
      transVal = myLastActionTransVal;
    }

    if (fabs((double)transVal) > (double).5)
      myTryingToMove = true;

    maxVel = ArMath::roundShort(maxTransVel);
    maxNegVel = ArMath::roundShort(maxNegTransVel);
    if (transVal > maxVel)
      transVal = maxVel;
    if (transVal < maxNegVel)
      transVal = maxNegVel;

    if (myActionTransSet && 
    (myLastTransSent.mSecSince() >= myStateReflectionRefreshTime ||
     transVal != myLastActionTransVal))              
    {
      comInt(ArCommands::VEL, ArMath::roundShort(transVal));
      myLastTransSent.setToNow();
      if (myLogMovementSent)
    ArLog::log(ArLog::Normal, "Action trans vel of %d", 
           ArMath::roundShort(transVal));      
    }
    myLastActionTransVal = transVal;
  }

  // if this is true actions can't go
  if ((myRotType != ROT_NONE && myDirectPrecedenceTime == 0) ||
      (myRotType != ROT_NONE && myDirectPrecedenceTime != 0 && 
       myRotSetTime.mSecSince() < myDirectPrecedenceTime))
  {
    if (hasSettableVelMaxes() && 
    ArMath::fabs(myLastSentRotVelMax - myRotVelMax) >= 1)
    {
      comInt(ArCommands::SETRV,
         ArMath::roundShort(myRotVelMax));
      myLastSentRotVelMax = myRotVelMax;
    }
    if (hasSettableAccsDecs() && ArMath::fabs(myRotAccel) > 1 &&
    ArMath::fabs(myLastSentRotAccel - myRotAccel) >= 1)
    {
      comInt(ArCommands::SETRA,
         ArMath::roundShort(myRotAccel));
      myLastSentRotAccel = myRotAccel;
      if (myLogMovementSent)
    ArLog::log(ArLog::Normal, "%25sNon-action rot accel of %d", "",
           ArMath::roundShort(myRotAccel));      
    }
    if (hasSettableAccsDecs() && ArMath::fabs(myRotDecel) > 1 &&
    ArMath::fabs(myLastSentRotDecel - myRotDecel) >= 1)
    {
      comInt(ArCommands::SETRA,
         -ArMath::roundShort(myRotDecel));
      myLastSentRotDecel = myRotDecel;
      if (myLogMovementSent)
    ArLog::log(ArLog::Normal, "%25sNon-action rot decel of %d", "",
           -ArMath::roundShort(myRotDecel));      
    }

    myActionRotSet = false;
    rotVal = ArMath::roundShort(myRotVal);
    if (myRotType == ROT_HEADING)
    {
      encTh = ArMath::subAngle(myRotVal, myEncoderTransform.getTh());
      rawTh = ArMath::addAngle(encTh, 
                   ArMath::subAngle(myRawEncoderPose.getTh(),
                        myEncoderPose.getTh()));
      rotVal = ArMath::roundShort(rawTh);

      // if we were using a different heading type, a different heading
      // our heading doesn't match what we want it to be, or its been a while
      // since we sent the heading, send it again
      if (myLastRotVal != rotVal || myLastRotType != myRotType ||
      fabs(ArMath::subAngle(rotVal, getTh())) > 1 ||
      (myLastRotSent.mSecSince() >= myStateReflectionRefreshTime))
      {
    comInt(ArCommands::HEAD, rotVal);
    myLastRotSent.setToNow();
    //printf("sent command, heading\n");
    if (myLogMovementSent)
      ArLog::log(ArLog::Normal, 
         "%25sNon-action rot heading of %d (encoder %d, raw %d)",
             "",
             ArMath::roundShort(myRotVal),
             ArMath::roundShort(encTh),
             ArMath::roundShort(rotVal));
      }
      if (fabs(ArMath::subAngle(rotVal, getTh())) > 1)
    myTryingToMove = true;
    }
    else if (myRotType == ROT_VEL)
    {
      if (myLastRotVal != rotVal || myLastRotType != myRotType ||
      (myLastRotSent.mSecSince() >= myStateReflectionRefreshTime))
      {
    comInt(ArCommands::RVEL, rotVal);
    myLastRotSent.setToNow();
    if (myLogMovementSent)
      ArLog::log(ArLog::Normal, "%25sNon-action rot vel of %d", "",
             rotVal);      
    //printf("sent command, rot vel\n");
    if (fabs((double)rotVal) > (double).5)
      myTryingToMove = true;
      }
    }
    else if (myRotType == ROT_IGNORE)
    {
      //printf("Not sending any command, rot is set to ignore");
    }
    else
      ArLog::log(ArLog::Terse, 
         "ArRobot::stateReflector: Invalid rotation type %d.",
         myRotType);
    myLastRotVal = rotVal;
    myLastRotType = myRotType;
  }
  else // if the action can fire
  {
    // first we'll handle all of the accel decel things
    if (myActionDesired.getMaxRotVelStrength() >=
    ArActionDesired::MIN_STRENGTH)
    {
      maxRotVel = myActionDesired.getMaxRotVel();
      if (maxRotVel > myAbsoluteMaxRotVel)
    maxRotVel = myAbsoluteMaxRotVel;
      maxRotVel = ArMath::roundShort(maxRotVel);
      if (ArMath::fabs(myLastSentRotVelMax - maxRotVel) >= 1)
      {
    myLastSentRotVelMax = maxRotVel;
    comInt(ArCommands::SETRV, 
           ArMath::roundShort(maxRotVel));
    if (myLogMovementSent)
      ArLog::log(ArLog::Normal, "%25sAction rot vel max of %d", "",
             ArMath::roundShort(maxRotVel));
      }
    }
    else if (hasSettableVelMaxes() && 
         ArMath::fabs(myLastSentRotVelMax - myRotVelMax) >= 1)
    {
      comInt(ArCommands::SETRV,
         ArMath::roundShort(myRotVelMax));
      myLastSentRotVelMax = myRotVelMax;
      if (myLogMovementSent)
    ArLog::log(ArLog::Normal, 
           "%25sAction-but-robot rot vel max of %d", 
           "",  ArMath::roundShort(myRotVelMax));      
    }

    if (myActionDesired.getRotAccelStrength() >= ArActionDesired::MIN_STRENGTH)
    {
      rotAccel = ArMath::roundShort(myActionDesired.getRotAccel());
      if (ArMath::fabs(myLastSentRotAccel - rotAccel) >= 1)
      {
    comInt(ArCommands::SETRA,
           ArMath::roundShort(rotAccel));
    myLastSentRotAccel = rotAccel;
    if (myLogMovementSent)
      ArLog::log(ArLog::Normal, "%25sAction rot accel of %d", "",
             ArMath::roundShort(rotAccel));
      }
    }
    else if (hasSettableAccsDecs() && ArMath::fabs(myRotAccel) > 1 &&
    ArMath::fabs(myLastSentRotAccel - myRotAccel) >= 1)
    {
      comInt(ArCommands::SETRA,
         ArMath::roundShort(myRotAccel));
      myLastSentRotAccel = myRotAccel;
      if (myLogMovementSent)
    ArLog::log(ArLog::Normal, "%25sAction-but-robot rot accel of %d", 
           "", ArMath::roundShort(myRotAccel));      
    }

    if (myActionDesired.getRotDecelStrength() >= ArActionDesired::MIN_STRENGTH)
    {
      rotDecel = ArMath::roundShort(myActionDesired.getRotDecel());
      if (ArMath::fabs(myLastSentRotDecel - rotDecel) >= 1)
      {
    comInt(ArCommands::SETRA,
           -ArMath::roundShort(rotDecel));
    myLastSentRotDecel = rotDecel;
    if (myLogMovementSent)
      ArLog::log(ArLog::Normal, "%25sAction rot decel of %d", "",
             -ArMath::roundShort(rotDecel));
      }
    }
    else if (hasSettableAccsDecs() && ArMath::fabs(myRotDecel) > 1 &&
    ArMath::fabs(myLastSentRotDecel - myRotDecel) >= 1)
    {
      comInt(ArCommands::SETRA,
         -ArMath::roundShort(myRotDecel));
      myLastSentRotDecel = myRotDecel;
      if (myLogMovementSent)
    ArLog::log(ArLog::Normal, "%25sAction-but-robot rot decel of %d", 
           "", -ArMath::roundShort(myRotDecel));      
    }



    if (myActionDesired.getDeltaHeadingStrength() >=
                                         ArActionDesired::MIN_STRENGTH)
    {
      if (ArMath::roundShort(myActionDesired.getDeltaHeading()) == 0)
      {
    rotStopped = true;
    rotVal = 0;
    rotHeading = false;
      }
      else
      {
    //printf("delta %.0f\n", myActionDesired.getDeltaHeading());
    //encTh = ArMath::subAngle(myRotVal, myEncoderTransform.getTh());
    encTh = ArMath::subAngle(
        ArMath::addAngle(myActionDesired.getDeltaHeading(), 
                 getTh()),
        myEncoderTransform.getTh());
    //printf("final th %.0f\n", th);
    rawTh = ArMath::addAngle(encTh, 
                 ArMath::subAngle(myRawEncoderPose.getTh(),
                          myEncoderPose.getTh()));
    rotVal = ArMath::roundShort(rawTh);
    rotStopped = false;
    rotHeading = true;
    myTryingToMove = true;
      }
      myActionRotSet = true;
    } 
    else if (myActionDesired.getRotVelStrength() >=
                                         ArActionDesired::MIN_STRENGTH)
    {
      if (ArMath::roundShort(myActionDesired.getRotVel()) == 0)
      {
    rotStopped = true;
    rotVal = 0;
    rotHeading = false;
      }
      else
      {
    rotStopped = false;
    rotVal = ArMath::roundShort(myActionDesired.getRotVel());
    rotHeading = false;
    myTryingToMove = true;
      }
      myActionRotSet = true;
    }
    else
    {
      rotStopped = myLastActionRotStopped;
      rotVal = myLastActionRotVal;
      rotHeading = myLastActionRotHeading;
    }

    if (myActionRotSet && 
    (myLastRotSent.mSecSince() > myStateReflectionRefreshTime ||
     rotStopped != myLastActionRotStopped || 
     rotVal != myLastActionRotVal || 
     rotHeading != myLastActionRotHeading))
    {
      if (rotStopped)
      {
    comInt(ArCommands::RVEL, 0);
    if (myLogMovementSent)
      ArLog::log(ArLog::Normal, 
             "%25sAction rot vel of 0 (rotStopped)",
             "");
      }
      else if (rotHeading)
      {
    comInt(ArCommands::HEAD, rotVal);
    if (myLogMovementSent)
      ArLog::log(ArLog::Normal, 
             "%25sAction rot heading of %d (encoder %d, raw %d)",
             "",
             ArMath::roundShort(ArMath::addAngle(
                 myActionDesired.getDeltaHeading(), 
                 getTh())),
             ArMath::roundShort(encTh),
             ArMath::roundShort(rotVal));
      }
      else
      {
    comInt(ArCommands::RVEL, rotVal);
    if (myLogMovementSent)
      ArLog::log(ArLog::Normal, "%25sAction rot vel of %d", "", 
             rotVal);
      }
      myLastRotSent.setToNow();
    }           
    
    myLastActionRotVal = rotVal;
    myLastActionRotStopped = rotStopped;
    myLastActionRotHeading = rotHeading;
  }

  if (myLastRotSent.mSecSince() > myStateReflectionRefreshTime &&
      myLastTransSent.mSecSince() > myStateReflectionRefreshTime &&
      myLastPulseSent.mSecSince() > myStateReflectionRefreshTime)
  {
    com(ArCommands::PULSE);
    myLastPulseSent.setToNow();
    if (myLogMovementSent)
      ArLog::log(ArLog::Normal, "Pulse"); 

  }
}

AREXPORT bool ArRobot::handlePacket(ArRobotPacket *packet)
{
  std::list<ArRetFunctor1<bool, ArRobotPacket *> *>::iterator it;
  bool handled;

  lock();
  //printf("ms since last packet %ld this type 0x%x\n", myLastPacketReceivedTime.mSecSince(packet->getTimeReceived()), packet->getID());
  myLastPacketReceivedTime = packet->getTimeReceived();
  if (packet->getID() == 0xff) 
  {
    ArLog::log(ArLog::Terse, "Losing connection because robot was reset.");
    dropConnection();
    unlock();
    return false;
  }

  for (handled = false, it = myPacketHandlerList.begin(); 
       it != myPacketHandlerList.end() && handled == false; 
       it++)
  {
    if ((*it) != NULL && (*it)->invokeR(packet)) 
      handled = true;
    else
      packet->resetRead();
  }
  if (!handled)
    ArLog::log(ArLog::Normal, 
           "No packet handler wanted packet with ID: 0x%x", 
           packet->getID());
  unlock();
  return handled;
}


AREXPORT long int ArRobot::getLeftEncoder()
{
  return myLeftEncoder;
}

AREXPORT long int ArRobot::getRightEncoder()
{
  return myRightEncoder;
}


AREXPORT void ArRobot::robotLocker(void)
{
  lock();
}

AREXPORT void ArRobot::robotUnlocker(void)
{
  unlock();
}

AREXPORT void ArRobot::packetHandler(void)
{
  ArRobotPacket *packet;
  int timeToWait;
  ArTime start;
  bool sipHandled = false;

  if (myAsyncConnectFlag)
  {
    lock();
    asyncConnectHandler(false);
    unlock();
    return;
  }

  if (!isConnected())
    return;

  start.setToNow();
  /*
    The basic idea is that if we're chained to the sip we run through
    and see if we have any packets available now (like if we got
    backed up), we only check this for half the cycle time
    though... if we know the cycle time of the robot (from config)
    then we go for half that, if we don't know the cycle time of the
    robot (from config) then we go for half of whatever our cycle time
    is set to

    if we don't have any packets waiting then we chill and wait for
    it, if we got one, just get on with it
  **/
  packet = NULL;
  // read all the packets that are available
  while ((packet = myReceiver.receivePacket(0)) != NULL)
  {
    if (myPacketsReceivedTracking)
    {
      ArLog::log(ArLog::Normal, 
         "Rcvd: prePacket (%ld) 0x%x at %ld (%ld)", 
         myPacketsReceivedTrackingCount, 
         packet->getID(), start.mSecSince(), 
         myPacketsReceivedTrackingStarted.mSecSince());
      myPacketsReceivedTrackingCount++;
    }

    handlePacket(packet);
    if ((packet->getID() & 0xf0) == 0x30)
      sipHandled = true;
    packet = NULL;

    // if we've taken too long here then break
    if ((getOrigRobotConfig()->hasPacketArrived() &&
     start.mSecSince() > getOrigRobotConfig()->getSipCycleTime() / 2) ||
    (!getOrigRobotConfig()->hasPacketArrived() &&
     (unsigned int) start.mSecSince() > myCycleTime / 2))
    {
      break;
    }
  }

  if (isCycleChained())
    timeToWait = getCycleTime() * 2 - start.mSecSince();
  
  // if we didn't get a sip and we're chained to the sip, wait for the sip
  while (isCycleChained() && !sipHandled && isRunning() && 
     (packet = myReceiver.receivePacket(timeToWait)) != NULL)
  {
    if (myPacketsReceivedTracking)
    {
      ArLog::log(ArLog::Normal, "Rcvd: Packet (%ld) 0x%x at %ld (%ld)", 
         myPacketsReceivedTrackingCount, 
         packet->getID(), start.mSecSince(), 
         myPacketsReceivedTrackingStarted.mSecSince());
      myPacketsReceivedTrackingCount++;
    }

    handlePacket(packet);
    if ((packet->getID() & 0xf0) == 0x30)
      break;
    timeToWait = getCycleTime() * 2 - start.mSecSince();
    if (timeToWait < 0)
      timeToWait = 0;
  }

  if (myTimeoutTime > 0 && 
      ((-myLastPacketReceivedTime.mSecTo()) > myTimeoutTime))
  {
    ArLog::log(ArLog::Terse, 
           "Losing connection because nothing received from robot in %d milliseconds.", 
           myTimeoutTime);
    dropConnection();
  }

  if (myPacketsReceivedTracking)
    ArLog::log(ArLog::Normal, "Rcvd: time taken %ld", start.mSecSince());

}

AREXPORT void ArRobot::actionHandler(void)
{
  ArActionDesired *actDesired;

  if (myResolver == NULL || myActions.size() == 0 || !isConnected())
    return;
  
  actDesired = myResolver->resolve(&myActions, this, myLogActions);
  
  myActionDesired.reset();

  if (actDesired == NULL)
    return;

  myActionDesired.merge(actDesired);  

  if (myLogActions)
  {
    ArLog::log(ArLog::Normal, "Final resolved desired:");
    myActionDesired.log();
  }
}

AREXPORT void ArRobot::setCycleWarningTime(unsigned int ms)
{
  myCycleWarningTime = ms;
  // we don't have to send it down because the functor gets it each cycle
}

AREXPORT unsigned int ArRobot::getCycleWarningTime(void) const
{
  return myCycleWarningTime;
}

AREXPORT unsigned int ArRobot::getCycleWarningTime(void)
{
  return myCycleWarningTime;
}

AREXPORT void ArRobot::setCycleTime(unsigned int ms)
{
  myCycleTime = ms;
}

AREXPORT void ArRobot::setStabilizingTime(int mSecs)
{
  if (mSecs > 0)
    myStabilizingTime = mSecs;
  else
    myStabilizingTime = 0;
}

AREXPORT int ArRobot::getStabilizingTime(void) const
{
  return myStabilizingTime;
}

AREXPORT unsigned int ArRobot::getCycleTime(void) const
{
  return myCycleTime;
}



AREXPORT void ArRobot::setConnectionCycleMultiplier(unsigned int multiplier)
{
  myConnectionCycleMultiplier = multiplier;
}

AREXPORT unsigned int ArRobot::getConnectionCycleMultiplier(void) const
{
  return myConnectionCycleMultiplier;
}


AREXPORT void ArRobot::loopOnce(void)
{
  if (mySyncTaskRoot != NULL)
    mySyncTaskRoot->run();
  
  incCounter();
}


// DigIn IR logic is reverse.  0 means broken, 1 means not broken

AREXPORT bool ArRobot::isLeftTableSensingIRTriggered(void) const
{
  if (myParams->haveTableSensingIR())
  {
    if (myParams->haveNewTableSensingIR() && myIODigInSize > 3)
      return !(getIODigIn(3) & ArUtil::BIT1);
    else
      return !(getDigIn() & ArUtil::BIT0);
  }
  return 0;
}

AREXPORT bool ArRobot::isRightTableSensingIRTriggered(void) const
{
  if (myParams->haveTableSensingIR())
  {
    if (myParams->haveNewTableSensingIR() && myIODigInSize > 3) 
      return !(getIODigIn(3) & ArUtil::BIT0);
    else
      return !(getDigIn() & ArUtil::BIT1);
  }
  return 0;
}

AREXPORT bool ArRobot::isLeftBreakBeamTriggered(void) const
{
  if (myParams->haveTableSensingIR())
  {
    if (myParams->haveNewTableSensingIR() && myIODigInSize > 3) 
      return !(getIODigIn(3) & ArUtil::BIT2);
    else
      return !(getDigIn() & ArUtil::BIT3);
  }
  return 0;
}

AREXPORT bool ArRobot::isRightBreakBeamTriggered(void) const
{
  if (myParams->haveTableSensingIR())
  {
    if (myParams->haveNewTableSensingIR() && myIODigInSize > 3) 
      return !(getIODigIn(3) & ArUtil::BIT3);
    else
      return !(getDigIn() & ArUtil::BIT2);
  }
  return 0;
}

AREXPORT int ArRobot::getMotorPacCount(void) const
{
  if (myTimeLastMotorPacket == time(NULL))
    return myMotorPacCount;
  if (myTimeLastMotorPacket == time(NULL) - 1)
    return myMotorPacCurrentCount;
  return 0;
}

AREXPORT int ArRobot::getSonarPacCount(void) const
{
  if (myTimeLastSonarPacket == time(NULL))
    return mySonarPacCount;
  if (myTimeLastSonarPacket == time(NULL) - 1)
    return mySonarPacCurrentCount;
  return 0;
}


AREXPORT bool ArRobot::processMotorPacket(ArRobotPacket *packet)
{
  int x, y, th, qx, qy, qth; 
  double deltaX, deltaY, deltaTh;

  int numReadings;
  int sonarNum;
  int sonarRange;

  if (packet->getID() != 0x32 && packet->getID() != 0x33) 
    return false;

  // upkeep the counting variable
  if (myTimeLastMotorPacket != time(NULL)) 
  {
    myTimeLastMotorPacket = time(NULL);
    myMotorPacCount = myMotorPacCurrentCount;
    myMotorPacCurrentCount = 0;
  }
  myMotorPacCurrentCount++;

  x = (packet->bufToUByte2() & 0x7fff);
  y = (packet->bufToUByte2() & 0x7fff);
  th = packet->bufToByte2();

  if (myFirstEncoderPose)
  {
    qx = 0;
    qy = 0;
    qth = 0;
    myFirstEncoderPose = false;
    myRawEncoderPose.setPose(
        myParams->getDistConvFactor() * x,
        myParams->getDistConvFactor() * y, 
        ArMath::radToDeg(myParams->getAngleConvFactor() * (double)th));
    myEncoderPose = myRawEncoderPose;
    myEncoderTransform.setTransform(myEncoderPose, myGlobalPose);
  }
  else 
  {
    qx = x - myLastX;
    qy = y - myLastY;
    qth = th - myLastTh;
  }


  //ArLog::log(ArLog::Terse, "Damnit qx %d qy %d,  x %d y %d,  lastx %d lasty %d", qx, qy, x, y, myLastX, myLastY);  
  myLastX = x;
  myLastY = y;
  myLastTh = th;

  if (qx > 0x1000) 
    qx -= 0x8000;
  if (qx < -0x1000)
    qx += 0x8000;

  if (qy > 0x1000) 
    qy -= 0x8000;
  if (qy < -0x1000)
    qy += 0x8000;
  
  deltaX = myParams->getDistConvFactor() * (double)qx;
  deltaY = myParams->getDistConvFactor() * (double)qy;
  deltaTh = ArMath::radToDeg(myParams->getAngleConvFactor() * (double)qth);
  //encoderTh = ArMath::radToDeg(myParams->getAngleConvFactor() * (double)(th));


  // encoder stuff was here



  myLeftVel = myParams->getVelConvFactor() * packet->bufToByte2();
  myRightVel = myParams->getVelConvFactor() * packet->bufToByte2();
  myVel = (myLeftVel + myRightVel)/2.0;

  myBatteryVoltage = packet->bufToUByte() * .1;
  myBatteryAverager.add(myBatteryVoltage);
  myStallValue = packet->bufToByte2();
  
  //ArLog::log("x %.1f y %.1f th %.1f vel %.1f voltage %.1f", myX, myY, myTh, 
  //myVel, myBatteryVoltage);

  myControl = ArMath::fixAngle(ArMath::radToDeg(myParams->getAngleConvFactor() *
                  (packet->bufToByte2() - th)));
  myFlags = packet->bufToUByte2();
  myCompass = 2*packet->bufToUByte();
  
  for (numReadings = packet->bufToByte(); numReadings > 0; numReadings--)
  {
    sonarNum = packet->bufToByte();
    sonarRange = ArMath::roundInt(
        (double)packet->bufToUByte2() * myParams->getRangeConvFactor());
    processNewSonar(sonarNum, sonarRange, packet->getTimeReceived());
  }
  
  if (packet->getDataLength() - packet->getDataReadLength() > 0)
  {
    myAnalogPortSelected = packet->bufToUByte2();
    myAnalog = packet->bufToByte();
    myDigIn = packet->bufToByte();
    myDigOut = packet->bufToByte();
  }

  if (packet->getDataLength() - packet->getDataReadLength() > 0)
    myRealBatteryVoltage = packet->bufToUByte2() * .1;
  else
    myRealBatteryVoltage = myBatteryVoltage;

  myRealBatteryAverager.add(myRealBatteryVoltage);

  if (packet->getDataLength() - packet->getDataReadLength() > 0)
    myChargeState = (ChargeState) packet->bufToUByte();
  else
    myChargeState = CHARGING_UNKNOWN;

  if (packet->getDataLength() - packet->getDataReadLength() > 0)
    myRotVel = (double)packet->bufToByte2() / 10.0;
  else
    myRotVel = ArMath::radToDeg((myRightVel - myLeftVel) / 2.0 * 
                myParams->getDiffConvFactor());

  if (packet->getDataLength() - packet->getDataReadLength() > 0)
  {
    myHasFaultFlags = true;
    myFaultFlags = packet->bufToUByte2();  
  }
  else
  {
    myHasFaultFlags = false;
    myFaultFlags = packet->bufToUByte2();  
  }
  /*
    Okay how this works is like so.  

    We keep around the raw encoder position, because we must use this
    to find differences between last position and this position.
    
    We find the difference in x and y positions (deltaX and deltaY)
    and keep these around for later use, but we also add these to our
    raw encoder readings for X and Y.  We also find the change in
    angle (deltaTh), which is used for inertial corrections, and added
    to the raw encoder heading to find which the current raw encoder
    heading.


    From here there are two paths:

    Path 1) Have a callback.  If we have a callback it means that we
    have an inertial nav device of some kind.  If this is the case,
    then we pass the callback the delta between last position and
    current position, along with the time of the current position,
    then the callback gives us back a new delta theta (deltaTh).  We
    then need to rotate the deltaX and deltaY into our corrected
    encoder space.  We do this by making a transform that takes the
    raw encoder heading and transforms it to what our new heading is
    (adding deltaTh to our current encoder th), and then applying that
    transform, taking the results as our new deltaX and deltaY.

    Path 2) We have no callback, we just use the heading that came
    back from the robot as our delta theta (deltaTh);

    From here the two paths unify again.  deltaX and deltaY are added
    to the encoder pose (this is the corrected encoder pose), and the
    encoder heading is set to the newTh.

    Note that this leaves a difference between rawEncoder heading and
    our heading, which is fine, BUT if you are sending heading
    commands to the robot you need to compenstate for the difference
    between these. 
    
    Note above that we return deltaTh instead of just heading so that
    we can turn inertial on and off without losing track of where
    we're at... since we're just adding in deltas from the heading it
    doesn't matter how we switch around the callback.
    
  **/

  myRawEncoderPose.setX(myRawEncoderPose.getX() + deltaX);
  myRawEncoderPose.setY(myRawEncoderPose.getY() + deltaY);
  myRawEncoderPose.setTh(myRawEncoderPose.getTh() + deltaTh);

  // check if there is a correction callback, if there is get the new
  // heading out of it instead of using the raw encoder heading
  if (myEncoderCorrectionCB != NULL)
  {
    ArPoseWithTime deltaPose(deltaX, deltaY, deltaTh,
                 packet->getTimeReceived());
    deltaTh = myEncoderCorrectionCB->invokeR(deltaPose);   
    ArTransform trans(ArPose(0, 0, myRawEncoderPose.getTh()),
              ArPose(0, 0,
                 ArMath::addAngle(myEncoderPose.getTh(), 
                          deltaTh)));

    ArPose rotatedDelta = trans.doTransform(ArPose(deltaX, deltaY, 0));

    deltaX = rotatedDelta.getX();
    deltaY = rotatedDelta.getY();
  }

  myEncoderPose.setTime(packet->getTimeReceived());
  myEncoderPose.setX(myEncoderPose.getX() + deltaX);
  myEncoderPose.setY(myEncoderPose.getY() + deltaY);
  myEncoderPose.setTh(ArMath::addAngle(myEncoderPose.getTh(), deltaTh));

  myGlobalPose = myEncoderTransform.doTransform(myEncoderPose);

  myOdometerDegrees += fabs(deltaTh);
  myOdometerDistance += sqrt(fabs(deltaX * deltaX + deltaY * deltaY));

  if (myLogMovementReceived)
    ArLog::log(ArLog::Normal, 
           "Global (%5.0f %5.0f %5.0f) Encoder (%5.0f %5.0f %5.0f) Raw (%5.0f %5.0f %5.0f) Rawest (%5d %5d %5d) Delta (%5.0f %5.0f %5.0f) Conv %5.2f",
           myGlobalPose.getX(), myGlobalPose.getY(),
           myGlobalPose.getTh(),
           myEncoderPose.getX(), myEncoderPose.getY(),
           myEncoderPose.getTh(),
           myRawEncoderPose.getX(), myRawEncoderPose.getY(), 
           myRawEncoderPose.getTh(), x, y, th, deltaX, deltaY, deltaTh,
           myParams->getDistConvFactor());        

  if (myLogVelocitiesReceived)
    ArLog::log(ArLog::Normal, 
           "     TransVel: %4.0f RotVel: %4.0f(%4.0f) Heading %4.0f TransAcc %4.0f RotAcc %4.0f(%4.0f)",
           myVel, myRotVel, myLastHeading - getTh(), getTh(), 
           myVel - myLastVel, myRotVel - myLastRotVel,
           myLastCalculatedRotVel - (myLastHeading - getTh()));
  myLastVel = myVel;
  myLastRotVel = myRotVel;
  myLastHeading = getTh();
  myLastCalculatedRotVel = myLastHeading - getTh();

  //ArLog::log(ArLog::Terse, "(%.0f %.0f) (%.0f %.0f)", deltaX, deltaY, myGlobalPose.getX(),         myGlobalPose.getY());
  
  myInterpolation.addReading(packet->getTimeReceived(), myGlobalPose);
  myEncoderInterpolation.addReading(packet->getTimeReceived(), myEncoderPose);

  return true;
}

AREXPORT void ArRobot::processNewSonar(char number, int range, 
                       ArTime timeReceived)
{
  std::map<int, ArSensorReading *>::iterator it;
  ArSensorReading *sonar;
  ArTransform encoderTrans;
  ArPose encoderPose;

  if ((it = mySonars.find(number)) != mySonars.end())
  {
    sonar = (*it).second;
    sonar->newData(range, getPose(), getEncoderPose(), getToGlobalTransform(), 
           getCounter(), timeReceived); 
    if (myTimeLastSonarPacket != time(NULL)) 
    {
      myTimeLastSonarPacket = time(NULL);
      mySonarPacCount = mySonarPacCurrentCount;
      mySonarPacCurrentCount = 0;
    }
    mySonarPacCurrentCount++;
  }
  else if (!myWarnedAboutExtraSonar)
  {
    ArLog::log(ArLog::Normal, "Robot gave back extra sonar reading!  Either the parameter file for the robot or the firmware needs updating.");
    myWarnedAboutExtraSonar = true;
  }
}



AREXPORT bool ArRobot::processEncoderPacket(ArRobotPacket *packet)
{
  if (packet->getID() != 0x90)
    return false;
  myLeftEncoder = packet->bufToByte4();
  myRightEncoder = packet->bufToByte4();
  return true;
}

AREXPORT bool ArRobot::processIOPacket(ArRobotPacket *packet)
{
  int i, num;

  if (packet->getID() != 0xf0)
    return false;

  myLastIOPacketReceivedTime = packet->getTimeReceived();

  // number of DigIn bytes
  num = packet->bufToUByte();
  for (i = 0; i < num; ++i)
    myIODigIn[i] = packet->bufToUByte();
  myIODigInSize = num;

  // number of DigOut bytes
  num = packet->bufToUByte();
  for (i = 0; i < num; ++i)
    myIODigOut[i] = packet->bufToUByte();
  myIODigOutSize = num;

  // number of A/D bytes
  num = packet->bufToUByte();
  for (i = 0; i < num; ++i)
    myIOAnalog[i] = packet->bufToUByte2();
  myIOAnalogSize = num;

  return true;
}

AREXPORT int ArRobot::getSonarRange(int num) const
{
  std::map<int, ArSensorReading *>::const_iterator it;
  
  if ((it = mySonars.find(num)) != mySonars.end())
    return (*it).second->getRange();
  else
    return -1;
}

AREXPORT bool ArRobot::isSonarNew(int num) const
{
  std::map<int, ArSensorReading *>::const_iterator it;
  
  if ((it = mySonars.find(num)) != mySonars.end())
    return (*it).second->isNew(getCounter());
  else
    return false;
}

AREXPORT ArSensorReading *ArRobot::getSonarReading(int num) const
{
  std::map<int, ArSensorReading *>::const_iterator it;
  
  if ((it = mySonars.find(num)) != mySonars.end())
    return (*it).second;
  else
    return NULL;
}


AREXPORT bool ArRobot::com(unsigned char command)
{
  if (myPacketsSentTracking)
    ArLog::log(ArLog::Normal, "Sent: com(%d)", command);
  return mySender.com(command);
}

AREXPORT bool ArRobot::comInt(unsigned char command, short int argument)
{
  if (myPacketsSentTracking)
    ArLog::log(ArLog::Normal, "Sent: comInt(%d, %d)", command, argument);
  return mySender.comInt(command, argument);
}

AREXPORT bool ArRobot::com2Bytes(unsigned char command, char high, char low)
{
  if (myPacketsSentTracking)
    ArLog::log(ArLog::Normal, "Sent: com2Bytes(%d, %d, %d)", command, 
           high, low);
  return mySender.com2Bytes(command, high, low);
}

AREXPORT bool ArRobot::comStr(unsigned char command, const char *argument)
{
  if (myPacketsSentTracking)
    ArLog::log(ArLog::Normal, "Sent: comStr(%d, '%s')", command, 
           argument);
  return mySender.comStr(command, argument);
}

AREXPORT bool ArRobot::comStrN(unsigned char command, const char *str, 
                   int size)
{
  if (myPacketsSentTracking)
  {
    char strBuf[512];
    strncpy(strBuf, str, size);
    strBuf[size] = '\0';
    ArLog::log(ArLog::Normal, "Sent: comStrN(%d, '%s') (size %d)",
           command, strBuf, size);
  }
  return mySender.comStrN(command, str, size);
}

AREXPORT bool ArRobot::comDataN(unsigned char command, const char* data, int size)
{
  if(myPacketsSentTracking)
  {
    ArLog::log(ArLog::Normal, "Sent: comDataN(%d, <data...>) (size %d)", command, size);
  }
  return mySender.comDataN(command, data, size);
}


AREXPORT int ArRobot::getClosestSonarRange(double startAngle, double endAngle) const
{
  int num;
  num = getClosestSonarNumber(startAngle, endAngle);
  if (num == -1)
    return -1;
  else 
    return getSonarRange(num);
}

AREXPORT int ArRobot::getClosestSonarNumber(double startAngle, double endAngle) const
{
  int i;
  ArSensorReading *sonar;
  int closestReading;
  int closestSonar;
  bool noReadings = true;

  for (i = 0; i < getNumSonar(); i++) 
  {
    sonar = getSonarReading(i);
    if (sonar == NULL)
    {
      ArLog::log(ArLog::Terse, "Have an empty sonar at number %d, there should be %d sonar.", i, getNumSonar());
      continue;
    }
    if (ArMath::angleBetween(sonar->getSensorTh(), startAngle, endAngle))
    {
      if (noReadings)
      {
    closestReading = sonar->getRange();
    closestSonar = i;
    noReadings = false;
      }
      else if (sonar->getRange() < closestReading)
      {
    closestReading = sonar->getRange();
    closestSonar = i;
      }
    }
  }

  if (noReadings)
    return -1;
  else
    return closestSonar;
}

AREXPORT void ArRobot::addRangeDevice(ArRangeDevice *device)
{
  device->setRobot(this);
  myRangeDeviceList.push_front(device);
}

AREXPORT void ArRobot::remRangeDevice(const char *name)
{
  std::list<ArRangeDevice *>::iterator it;
  for (it = myRangeDeviceList.begin(); it != myRangeDeviceList.end(); ++it)
  {
    if (strcmp(name, (*it)->getName()) == 0)
    {
      myRangeDeviceList.erase(it);
      return;
    }
  }
}

AREXPORT void ArRobot::remRangeDevice(ArRangeDevice *device)
{
  std::list<ArRangeDevice *>::iterator it;
  for (it = myRangeDeviceList.begin(); it != myRangeDeviceList.end(); ++it)
  {
    if ((*it) == device)
    {
      myRangeDeviceList.erase(it);
      return;
    }
  }
}

AREXPORT ArRangeDevice *ArRobot::findRangeDevice(const char *name)
{
  std::list<ArRangeDevice *>::iterator it;
  ArRangeDevice *device;

  for (it = myRangeDeviceList.begin(); it != myRangeDeviceList.end(); ++it)
  {
    device = (*it);
    if(strcmp(name, device->getName()) == 0)
    {
      return device;
    }
  }
  return NULL;
}

AREXPORT const ArRangeDevice *ArRobot::findRangeDevice(const char *name) const
{
  std::list<ArRangeDevice *>::const_iterator it;
  ArRangeDevice *device;

  for (it = myRangeDeviceList.begin(); it != myRangeDeviceList.end(); ++it)
  {
    device = (*it);
    if(strcmp(name, device->getName()) == 0)
    {
      return device;
    }
  }
  return NULL;
}

AREXPORT std::list<ArRangeDevice *> *ArRobot::getRangeDeviceList(void)
{
  return &myRangeDeviceList;
}

AREXPORT bool ArRobot::hasRangeDevice(ArRangeDevice *device) const
{
  std::list<ArRangeDevice *>::const_iterator it;
  for (it = myRangeDeviceList.begin(); it != myRangeDeviceList.end(); ++it)
  {
    if ((*it) == device)
      return true;
  }
  return false;
}

AREXPORT double ArRobot::checkRangeDevicesCurrentPolar(
    double startAngle, double endAngle, double *angle, 
    const ArRangeDevice **rangeDevice,
    bool useLocationDependentDevices) const
{
  double closest = 32000;
  double closeAngle, tempDist, tempAngle;
  std::list<ArRangeDevice *>::const_iterator it;
  ArRangeDevice *device;
  bool foundOne = false;
  const ArRangeDevice *closestRangeDevice = NULL;

  for (it = myRangeDeviceList.begin(); it != myRangeDeviceList.end(); ++it)
  {
    device = (*it);
    device->lockDevice();
    if (!useLocationDependentDevices && device->isLocationDependent())
    {
      device->unlockDevice();
      continue;
    }
    if (!foundOne || 
    (tempDist = device->currentReadingPolar(startAngle, endAngle,
                        &tempAngle)) < closest)
    {
      if (!foundOne)
      {
    closest = device->currentReadingPolar(startAngle, endAngle, 
                          &closeAngle);
    closestRangeDevice = device;
      }
      else
      {
    closest = tempDist;
    closeAngle = tempAngle;
    closestRangeDevice = device;
      }
      foundOne = true;
    }
    device->unlockDevice();
  }
  if (!foundOne)
    return -1;
  if (angle != NULL)
    *angle = closeAngle;
  if (rangeDevice != NULL)
    *rangeDevice = closestRangeDevice;
  return closest;
  
}


AREXPORT double ArRobot::checkRangeDevicesCumulativePolar(
    double startAngle, double endAngle, double *angle, 
    const ArRangeDevice **rangeDevice, 
    bool useLocationDependentDevices) const
{
  double closest = 32000;
  double closeAngle, tempDist, tempAngle;
  std::list<ArRangeDevice *>::const_iterator it;
  ArRangeDevice *device;
  bool foundOne = false;
  const ArRangeDevice *closestRangeDevice = NULL;

  for (it = myRangeDeviceList.begin(); it != myRangeDeviceList.end(); ++it)
  {
    device = (*it);
    device->lockDevice();
    if (!useLocationDependentDevices && device->isLocationDependent())
    {
      device->unlockDevice();
      continue;
    }
    if (!foundOne || 
    (tempDist = device->cumulativeReadingPolar(startAngle, endAngle,
                         &tempAngle)) < closest)
    {
      if (!foundOne)
      {
    closest = device->cumulativeReadingPolar(startAngle, endAngle, 
                          &closeAngle);
    closestRangeDevice = device;
      }
      else
      {
    closest = tempDist;
    closeAngle = tempAngle;
    closestRangeDevice = device;
      }
      foundOne = true;
    }
    device->unlockDevice();
  }
  if (!foundOne)
    return -1;
  if (angle != NULL)
    *angle = closeAngle;
  if (rangeDevice != NULL)
    *rangeDevice = closestRangeDevice;
  return closest;
  
}

AREXPORT double ArRobot::checkRangeDevicesCurrentBox(
    double x1, double y1, double x2, double y2, ArPose *readingPos,
    const ArRangeDevice **rangeDevice, 
    bool useLocationDependentDevices) const
{

  double closest = 32000;
  double tempDist;
  ArPose closestPos, tempPos;
  std::list<ArRangeDevice *>::const_iterator it;
  ArRangeDevice *device;
  bool foundOne = false;
  const ArRangeDevice *closestRangeDevice = NULL;

  for (it = myRangeDeviceList.begin(); it != myRangeDeviceList.end(); ++it)
  {
    device = (*it);
    device->lockDevice();
    if (!useLocationDependentDevices && device->isLocationDependent())
    {
      device->unlockDevice();
      continue;
    }
    if (!foundOne || 
    (tempDist = device->currentReadingBox(x1, y1, x2, y2, &tempPos)) < closest)
    {
      if (!foundOne)
      {
    closest = device->currentReadingBox(x1, y1, x2, y2, &closestPos);
    closestRangeDevice = device;
      }
      else
      {
    closest = tempDist;
    closestPos = tempPos;
    closestRangeDevice = device;
      }
      foundOne = true;
    }
    device->unlockDevice();
  }
  if (!foundOne)
    return -1;
  if (readingPos != NULL)
    *readingPos = closestPos;
  if (rangeDevice != NULL)
    *rangeDevice = closestRangeDevice;
  return closest;
}

AREXPORT double ArRobot::checkRangeDevicesCumulativeBox(
    double x1, double y1, double x2, double y2, ArPose *readingPos,
    const ArRangeDevice **rangeDevice, 
    bool useLocationDependentDevices) const
{

  double closest = 32000;
  double tempDist;
  ArPose closestPos, tempPos;
  std::list<ArRangeDevice *>::const_iterator it;
  ArRangeDevice *device;
  bool foundOne = false;
  const ArRangeDevice *closestRangeDevice = NULL;

  for (it = myRangeDeviceList.begin(); it != myRangeDeviceList.end(); ++it)
  {
    device = (*it);
    device->lockDevice();
    if (!useLocationDependentDevices && device->isLocationDependent())
    {
      device->unlockDevice();
      continue;
    }
    if (!foundOne || 
    (tempDist = device->cumulativeReadingBox(x1, y1, x2, y2, &tempPos)) < closest)
    {
      if (!foundOne)
      {
    closest = device->cumulativeReadingBox(x1, y1, x2, y2, &closestPos);
    closestRangeDevice = device;
      }
      else
      {
    closest = tempDist;
    closestPos = tempPos;
    closestRangeDevice = device;
      }
      foundOne = true;
    }
    device->unlockDevice();
  }
  if (!foundOne)
    return -1;
  if (readingPos != NULL)
    *readingPos = closestPos;
  if (rangeDevice != NULL)
    *rangeDevice = closestRangeDevice;
  return closest;
}

AREXPORT void ArRobot::moveTo(ArPose pose, bool doCumulative)
{
  std::list<ArRangeDevice *>::iterator it;
  ArSensorReading *son;
  int i;

  // we need to get this one now because changing the encoder
  // transform and global pose will change the local transform
  ArTransform localTransform;
  localTransform = getToLocalTransform();

  myEncoderTransform.setTransform(myEncoderPose, pose);
  myGlobalPose = myEncoderTransform.doTransform(myEncoderPose);

  for (it = myRangeDeviceList.begin(); it != myRangeDeviceList.end(); it++)
  {
    (*it)->lockDevice();
    (*it)->applyTransform(localTransform, doCumulative);
    (*it)->applyTransform(getToGlobalTransform(), doCumulative);
    (*it)->unlockDevice();
  }

  for (i = 0; i < getNumSonar(); i++)
  {
    son = getSonarReading(i);
    if (son != NULL)
    {
      son->applyTransform(localTransform);
      son->applyTransform(getToGlobalTransform());
    }
  }

  //ArLog::log(ArLog::Normal, "Robot moved to %.0f %.0f %.1f", getX(), getY(), getTh());
}

AREXPORT void ArRobot::moveTo(ArPose poseTo, ArPose poseFrom,
                  bool doCumulative)
{
  std::list<ArRangeDevice *>::iterator it;
  ArSensorReading *son;
  int i;

  ArPose result = myEncoderTransform.doInvTransform(poseFrom);

  // we need to get this one now because changing the encoder
  // transform and global pose will change the local transform
  ArTransform localTransform;
  localTransform = getToLocalTransform();

  myEncoderTransform.setTransform(result, poseTo);
  myGlobalPose = myEncoderTransform.doTransform(myEncoderPose);

  for (it = myRangeDeviceList.begin(); it != myRangeDeviceList.end(); it++)
  {
    (*it)->lockDevice();
    (*it)->applyTransform(localTransform, doCumulative);
    (*it)->applyTransform(getToGlobalTransform(), doCumulative);
    (*it)->unlockDevice();
  }

  for (i = 0; i < getNumSonar(); i++)
  {
    son = getSonarReading(i);
    if (son != NULL)
    {
      son->applyTransform(localTransform);
      son->applyTransform(getToGlobalTransform());
    }
  }

  //ArLog::log(ArLog::Normal, "Robot moved to %.0f %.0f %.1f", getX(), getY(), getTh());
}

AREXPORT void ArRobot::setEncoderTransform(ArPose deadReconPos, 
                    ArPose globalPos)
{
  myEncoderTransform.setTransform(deadReconPos, globalPos);
  myGlobalPose = myEncoderTransform.doTransform(myEncoderPose);
}

AREXPORT void ArRobot::setEncoderTransform(ArPose transformPos)
{
  myEncoderTransform.setTransform(transformPos);
  myGlobalPose = myEncoderTransform.doTransform(myEncoderPose);
}

AREXPORT ArTransform ArRobot::getEncoderTransform(void) const
{
  return myEncoderTransform;
}

AREXPORT void ArRobot::setDeadReconPose(ArPose pose)
{
  myEncoderPose.setPose(pose);
  myEncoderTransform.setTransform(myEncoderPose, myGlobalPose);
  myGlobalPose = myEncoderTransform.doTransform(myEncoderPose);
}


AREXPORT ArTransform ArRobot::getToGlobalTransform(void) const
{
  ArTransform trans;
  ArPose origin(0, 0, 0);
  ArPose pose = getPose();

  trans.setTransform(origin, pose);
  return trans;
}

AREXPORT ArTransform ArRobot::getToLocalTransform(void) const
{
  ArTransform trans;
  ArPose origin(0, 0, 0);
  ArPose pose = getPose();

  trans.setTransform(pose, origin);
  return trans;
}

AREXPORT void ArRobot::applyTransform(ArTransform trans, bool doCumulative)
{
  std::list<ArRangeDevice *>::iterator it;
  ArSensorReading *son;
  int i;
  
  for (it = myRangeDeviceList.begin(); it != myRangeDeviceList.end(); it++)
  {
      (*it)->lockDevice();
      (*it)->applyTransform(trans, doCumulative);
      (*it)->unlockDevice();
  }

  for (i = 0; i < getNumSonar(); i++)
  {
    son = getSonarReading(i);
    if (son != NULL)
      son->applyTransform(trans);
  }
}

AREXPORT const char *ArRobot::getName(void) const
{
  return myName.c_str();
}

AREXPORT void ArRobot::setName(const char * name)
{
    std::list<ArRobot *> *robotList;
    std::list<ArRobot *>::iterator it;
    int i;
    char buf[1024];
 
 if (name != NULL)
  {
    myName = name;
  }
  else
  {

    

    robotList = Aria::getRobotList();
    for (i = 1, it = robotList->begin(); it != robotList->end(); it++, i++)
    {
      if (this == (*it))
      {
        if (i == 1)
            myName = "robot";
        else
        {
            sprintf(buf, "robot%d", i);
            myName = buf;
        }
        return;
      }
    }
    sprintf(buf, "robot%d", robotList->size());
    myName = buf;
   }
}

AREXPORT void ArRobot::setEncoderCorrectionCallback(
    ArRetFunctor1<double, ArPoseWithTime> *functor)
{
  myEncoderCorrectionCB = functor;
}
AREXPORT ArRetFunctor1<double, ArPoseWithTime> * 
ArRobot::getEncoderCorrectionCallback(void) const
{
  return myEncoderCorrectionCB;
}

AREXPORT void ArRobot::setDirectMotionPrecedenceTime(int mSec)
{
  if (mSec < 0) 
    myDirectPrecedenceTime = 0;
  else
    myDirectPrecedenceTime = mSec;
}

AREXPORT unsigned int ArRobot::getDirectMotionPrecedenceTime(void) const
{
  return myDirectPrecedenceTime;
}


AREXPORT void ArRobot::clearDirectMotion(void)
{
  myTransType = TRANS_NONE;
  myLastTransType = TRANS_NONE;
  myRotType = ROT_NONE;
  myLastRotType = ROT_NONE;
  myLastActionTransVal = 0;
  myLastActionRotStopped = true;
  myLastActionRotHeading = false;
}

AREXPORT void ArRobot::stopStateReflection(void)
{
  myTransType = TRANS_IGNORE;
  myLastTransType = TRANS_IGNORE;
  myRotType = ROT_IGNORE;
  myLastRotType = ROT_IGNORE;
}

AREXPORT bool ArRobot::isDirectMotion(void) const
{
  if (myTransType ==  TRANS_NONE && myLastTransType == TRANS_NONE &&
      myRotType == ROT_NONE && myLastRotType == ROT_NONE)
    return false;
  else
    return true;
}


AREXPORT void ArRobot::enableMotors()
{
  comInt(ArCommands::ENABLE, 1);
}

AREXPORT void ArRobot::disableMotors()
{
  comInt(ArCommands::ENABLE, 0);
}

AREXPORT void ArRobot::enableSonar()
{
  comInt(ArCommands::SONAR, 1);
}

AREXPORT void ArRobot::disableSonar()
{
  comInt(ArCommands::SONAR, 0);
}


AREXPORT void ArRobot::setStateReflectionRefreshTime(int mSec)
{
  if (mSec < 0)
    myStateReflectionRefreshTime = 0;
  else
    myStateReflectionRefreshTime = mSec;
}

AREXPORT int ArRobot::getStateReflectionRefreshTime(void) const
{
  return myStateReflectionRefreshTime;
}

AREXPORT void ArRobot::attachKeyHandler(ArKeyHandler *keyHandler, 
                    bool exitOnEscape, 
                    bool useExitNotShutdown)
{
  if (myKeyHandlerCB != NULL)
    delete myKeyHandlerCB;
  myKeyHandlerCB = new ArFunctorC<ArKeyHandler>(keyHandler, 
                        &ArKeyHandler::checkKeys);
  addSensorInterpTask("Key Handler", 50, myKeyHandlerCB);

  myKeyHandler = keyHandler;
  myKeyHandlerUseExitNotShutdown = useExitNotShutdown;
  if (exitOnEscape)
    keyHandler->addKeyHandler(ArKeyHandler::ESCAPE, &myKeyHandlerExitCB);
}

AREXPORT ArKeyHandler *ArRobot::getKeyHandler(void) const
{
  return myKeyHandler;
}

AREXPORT void ArRobot::keyHandlerExit(void)
{
  ArLog::log(ArLog::Terse, "Escape was pressed, program is exiting.");
  // if we're using exit not the keyhandler then call Aria::exit
  // instead of shutdown, this call never returns
  if (myKeyHandlerUseExitNotShutdown)
    Aria::exit();
  stopRunning();
  unlock();
  Aria::shutdown();
}

AREXPORT void ArRobot::setPacketsReceivedTracking(bool packetsReceivedTracking)
{
  myPacketsReceivedTracking = packetsReceivedTracking;
  myPacketsReceivedTrackingCount = 0; 
  myPacketsReceivedTrackingStarted.setToNow(); 
}

AREXPORT void ArRobot::ariaExitCallback(void)
{
  mySyncLoop.stopRunIfNotConnected(false);
  disconnect();
}

AREXPORT ArRobot::ChargeState ArRobot::getChargeState(void) const
{
  return myChargeState;
}

AREXPORT void ArRobot::resetOdometer(void)
{
  myOdometerDistance = 0;
  myOdometerDegrees = 0;
  myOdometerStart.setToNow();
}

---