ArRobot.h

/*
MobileRobots Advanced Robotics Interface for Applications (ARIA)
Copyright (C) 2004,2005 ActivMedia Robotics LLC
Copyright (C) 2006 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
*/

#ifndef ARROBOT_H
#define ARROBOT_H

#include "ariaTypedefs.h"
#include "ArRobotPacketSender.h"
#include "ArRobotPacketReceiver.h"
#include "ArFunctor.h"
#include "ArFunctor.h"
#include "ArSyncTask.h"
#include "ArSensorReading.h"
#include "ArMutex.h"
#include "ArCondition.h"
#include "ArSyncLoop.h"
#include "ArRobotParams.h"
#include "ArActionDesired.h"
#include "ArResolver.h"
#include "ArTransform.h"
#include "ArInterpolation.h"
#include "ArKeyHandler.h"
#include <list>

class ArAction;
class ArRobotConfigPacketReader;
class ArDeviceConnection;
class ArRangeDevice;
class ArRobotPacket;
class ArPTZ;


class ArRobot
{
public:

  typedef enum {
    WAIT_CONNECTED, 
    WAIT_FAILED_CONN, 
    WAIT_RUN_EXIT, 
    WAIT_TIMEDOUT, 
    WAIT_INTR, 
    WAIT_FAIL 
  } WaitState;

  enum ChargeState {
    CHARGING_UNKNOWN = -1,
    CHARGING_NOT = 0,
    CHARGING_BULK = 1,
    CHARGING_OVERCHARGE = 2,
    CHARGING_FLOAT = 3
  };
  AREXPORT ArRobot(const char * name = NULL, bool ignored = true,
           bool doSigHandle=true, 
           bool normalInit = true, bool addAriaExitCallback = true);
           
  AREXPORT ~ArRobot();

  AREXPORT void run(bool stopRunIfNotConnected);
  AREXPORT void runAsync(bool stopRunIfNotConnected);

  AREXPORT bool isRunning(void) const;

  AREXPORT void stopRunning(bool doDisconnect=true); 

  AREXPORT void setDeviceConnection(ArDeviceConnection *connection);
  AREXPORT ArDeviceConnection *getDeviceConnection(void) const;
  

  AREXPORT bool isConnected(void) const { return myIsConnected; }
  AREXPORT bool blockingConnect(void);
  AREXPORT bool asyncConnect(void);
  AREXPORT bool disconnect(void);

  AREXPORT void clearDirectMotion(void);
  AREXPORT bool isDirectMotion(void) const;

  AREXPORT void stopStateReflection(void);
  

  AREXPORT void enableMotors();
  AREXPORT void disableMotors();

  AREXPORT void enableSonar();
  AREXPORT void disableSonar();

  AREXPORT void stop(void);
  AREXPORT void setVel(double velocity);
  AREXPORT void setVel2(double leftVelocity, double rightVelocity);
  AREXPORT void move(double distance);
  AREXPORT bool isMoveDone(double delta = 0.0);
  AREXPORT void setMoveDoneDist(double dist) { myMoveDoneDist = dist; }
  AREXPORT double getMoveDoneDist(void) { return myMoveDoneDist; }
  AREXPORT void setHeading(double heading);
  AREXPORT void setRotVel(double velocity);
  AREXPORT void setDeltaHeading(double deltaHeading);
  AREXPORT bool isHeadingDone(double delta = 0.0) const;
  AREXPORT void setHeadingDoneDiff(double degrees) 
    { myHeadingDoneDiff = degrees; }
  AREXPORT double getHeadingDoneDiff(void) const { return myHeadingDoneDiff; }


  AREXPORT void setDirectMotionPrecedenceTime(int mSec);

  AREXPORT unsigned int getDirectMotionPrecedenceTime(void) const;

  AREXPORT bool com(unsigned char command);
  AREXPORT bool comInt(unsigned char command, short int argument);
  AREXPORT bool com2Bytes(unsigned char command, char high, char low);
  AREXPORT bool comStr(unsigned char command, const char *argument);
  AREXPORT bool comStrN(unsigned char command, const char *str, int size);
  AREXPORT bool comDataN(unsigned char command, const char *data, int size);

  AREXPORT const char * getRobotName(void) const { return myRobotName.c_str();}
  AREXPORT const char * getRobotType(void) const { return myRobotType.c_str();}
  AREXPORT const char * getRobotSubType(void) const 
    { return myRobotSubType.c_str(); }

  AREXPORT double getAbsoluteMaxTransVel(void) const 
    { return myAbsoluteMaxTransVel; }
  AREXPORT bool setAbsoluteMaxTransVel(double maxVel);

  AREXPORT double getAbsoluteMaxTransAccel(void) const 
    { return myAbsoluteMaxTransAccel; }
  AREXPORT bool setAbsoluteMaxTransAccel(double maxAccel);

  AREXPORT double getAbsoluteMaxTransDecel(void) const 
    { return myAbsoluteMaxTransDecel; }
  AREXPORT bool setAbsoluteMaxTransDecel(double maxDecel);

  AREXPORT double getAbsoluteMaxRotVel(void) const 
    { return myAbsoluteMaxRotVel; }
  AREXPORT bool setAbsoluteMaxRotVel(double maxVel);
  
  AREXPORT double getAbsoluteMaxRotAccel(void) const 
    { return myAbsoluteMaxRotAccel; }
  AREXPORT bool setAbsoluteMaxRotAccel(double maxAccel);

  AREXPORT double getAbsoluteMaxRotDecel(void) const 
    { return myAbsoluteMaxRotDecel; }
  AREXPORT bool setAbsoluteMaxRotDecel(double maxDecel);


  // Accessors

  ArPose getPose(void) const { return myGlobalPose; }
  double getX(void) const { return  myGlobalPose.getX(); }
  double getY(void) const { return myGlobalPose.getY(); }
  double getTh(void) const { return myGlobalPose.getTh(); }
  double findDistanceTo(const ArPose pose) 
    { return myGlobalPose.findDistanceTo(pose); }
  double findAngleTo(const ArPose pose) 
    { return myGlobalPose.findAngleTo(pose); }
  double findDeltaHeadingTo(const ArPose pose) 
    { return ArMath::subAngle(myGlobalPose.findAngleTo(pose),
                  myGlobalPose.getTh()); }


  double getVel(void) const { return myVel; }

  double getRotVel(void) const { return myRotVel; }
  double getRobotRadius(void) const { return myParams->getRobotRadius(); }
  double getRobotWidth(void) const { return myParams->getRobotWidth(); }
  double getRobotLength(void) const { return myParams->getRobotLength(); }
  double getRobotLengthFront(void) const { return myRobotLengthFront; }
  double getRobotLengthRear(void) const { return myRobotLengthRear; }
  double getRobotDiagonal(void) const { return myParams->getRobotDiagonal(); }

  double getBatteryVoltage(void) const {return myBatteryAverager.getAverage();}

  double getBatteryVoltageNow(void) const { return myBatteryVoltage; }

  double getRealBatteryVoltage(void) const 
    { return myRealBatteryAverager.getAverage(); }

  double getRealBatteryVoltageNow(void) const { return myRealBatteryVoltage; }
  double getLeftVel(void) const { return myLeftVel; }
  double getRightVel(void) const { return myRightVel; }
  int getStallValue(void) const { return myStallValue; }
  bool isLeftMotorStalled(void) const 
    { return (myStallValue & 0xff) & ArUtil::BIT0; }
  bool isRightMotorStalled(void) const
    { return ((myStallValue & 0xff00) >> 8) & ArUtil::BIT0; }

  double getControl(void) const { return myControl; }
  int getFlags(void) const { return myFlags; }
  int getFaultFlags(void) const { return myFaultFlags; }
  bool hasFaultFlags(void) const { return myHasFaultFlags; }
  bool areMotorsEnabled(void) const { return (myFlags & ArUtil::BIT0); }
  bool areSonarsEnabled(void) const { 
    return (myFlags & 
        (ArUtil::BIT1 | ArUtil::BIT2 | ArUtil::BIT3 | ArUtil::BIT4)); }
  bool isEStopPressed(void) const { return (myFlags & ArUtil::BIT5); }
  double getCompass(void) const { return myCompass; }
  int getAnalogPortSelected(void) const { return myAnalogPortSelected; }
  unsigned char getAnalog(void) const { return myAnalog; }
  unsigned char getDigIn(void) const { return myDigIn; }
  unsigned char getDigOut(void) const { return myDigOut; }
  AREXPORT ChargeState getChargeState(void) const;

  int getIOAnalogSize(void) const { return myIOAnalogSize; }
  int getIODigInSize(void) const { return myIODigInSize; }
  int getIODigOutSize(void) const { return myIODigOutSize; }

  AREXPORT int getIOAnalog(int num) const;
  AREXPORT double getIOAnalogVoltage(int num) const;
  AREXPORT unsigned char getIODigIn(int num) const;
  AREXPORT unsigned char getIODigOut(int num) const;

  bool hasTableSensingIR(void) const { return myParams->haveTableSensingIR(); }
  AREXPORT bool isLeftTableSensingIRTriggered(void) const;
  AREXPORT bool isRightTableSensingIRTriggered(void) const;
  AREXPORT bool isLeftBreakBeamTriggered(void) const;
  AREXPORT bool isRightBreakBeamTriggered(void) const;
  ArTime getIOPacketTime(void) const { return myLastIOPacketReceivedTime; }

  AREXPORT bool getEstop(void) { return (myFlags & ArUtil::BIT5); }

  bool hasFrontBumpers(void) const { return myParams->haveFrontBumpers(); }
  unsigned int getNumFrontBumpers(void) const 
    { return myParams->numFrontBumpers(); }
  bool hasRearBumpers(void) const { return myParams->haveRearBumpers(); }
  unsigned int getNumRearBumpers(void) const 
    { return myParams->numRearBumpers(); }

  ArPose getEncoderPose(void) const { return myEncoderPose; }


  bool isTryingToMove(void) { return myTryingToMove; }

  void forceTryingToMove(void) { myTryingToMove = true; }
  

  AREXPORT int getMotorPacCount(void) const;
  AREXPORT int getSonarPacCount(void) const;

  AREXPORT int getSonarRange(int num) const;
  AREXPORT bool isSonarNew(int num) const;
  AREXPORT int getNumSonar(void) const { return myNumSonar; }
  AREXPORT ArSensorReading *getSonarReading(int num) const;
  AREXPORT int getClosestSonarRange(double startAngle, double endAngle) const;
  AREXPORT int getClosestSonarNumber(double startAngle, double endAngle) const;

  AREXPORT const char *getName(void) const;
  AREXPORT void setName(const char *name);

  AREXPORT void moveTo(ArPose pose, bool doCumulative = true);
  AREXPORT void moveTo(ArPose to, ArPose from, bool doCumulative = true);

  size_t getBatteryVoltageAverageOfNum(void) 
    { return myBatteryAverager.getNumToAverage(); }

  void setBatteryVoltageAverageOfNum(size_t numToAverage)
    { myBatteryAverager.setNumToAverage(numToAverage); }

  size_t getRealBatteryVoltageAverageOfNum(void) 
    { return myRealBatteryAverager.getNumToAverage(); }

  void setRealBatteryVoltageAverageOfNum(size_t numToAverage)
    { myRealBatteryAverager.setNumToAverage(numToAverage); }

  AREXPORT void requestEncoderPackets(void);

  AREXPORT void requestIOPackets(void);

  AREXPORT void stopEncoderPackets(void);

  AREXPORT void stopIOPackets(void);

  AREXPORT long int getLeftEncoder(void);

  AREXPORT long int getRightEncoder(void);

  AREXPORT void setEncoderTransform(ArPose deadReconPos,
                    ArPose globalPos);

  AREXPORT void setEncoderTransform(ArPose transformPos);

  AREXPORT ArTransform getEncoderTransform(void) const;

  AREXPORT ArTransform getToGlobalTransform(void) const;

  AREXPORT ArTransform getToLocalTransform(void) const;

  AREXPORT void applyTransform(ArTransform trans, bool doCumulative = true);

  AREXPORT void setDeadReconPose(ArPose pose);

  AREXPORT double getOdometerDistance(void) { return myOdometerDistance; }

  AREXPORT double getOdometerDegrees(void) { return myOdometerDegrees; }

  AREXPORT double getOdometerTime(void) { return myOdometerStart.secSince(); }

  AREXPORT void resetOdometer(void);

  AREXPORT void addRangeDevice(ArRangeDevice *device);
  AREXPORT void remRangeDevice(const char *name);
  AREXPORT void remRangeDevice(ArRangeDevice *device);

  AREXPORT const ArRangeDevice *findRangeDevice(const char *name) const;

  AREXPORT ArRangeDevice *findRangeDevice(const char *name);

  AREXPORT std::list<ArRangeDevice *> *getRangeDeviceList(void);

  AREXPORT bool hasRangeDevice(ArRangeDevice *device) const;

  AREXPORT double checkRangeDevicesCurrentPolar(
      double startAngle, double endAngle, double *angle = NULL,
      const ArRangeDevice **rangeDevice = NULL,
      bool useLocationDependentDevices = true) const;

  AREXPORT double checkRangeDevicesCumulativePolar(
      double startAngle, double endAngle, double *angle = NULL,
      const ArRangeDevice **rangeDevice = NULL,
      bool useLocationDependentDevices = true) const;


  // Goes through all the range devices and checks them
  AREXPORT double checkRangeDevicesCurrentBox(
      double x1, double y1, double x2, double y2,
      ArPose *readingPos = NULL,
      const ArRangeDevice **rangeDevice = NULL,
      bool useLocationDependentDevices = true) const;

  // Goes through all the range devices and checks them
  AREXPORT double checkRangeDevicesCumulativeBox(
      double x1, double y1, double x2, double y2,
      ArPose *readingPos = NULL, 
      const ArRangeDevice **rangeDevice = NULL,
      bool useLocationDependentDevices = true) const;

  void setPTZ(ArPTZ *ptz) { myPtz = ptz; }
  ArPTZ *getPTZ(void) { return myPtz; }

  AREXPORT void setStateReflectionRefreshTime(int msec);

  AREXPORT int getStateReflectionRefreshTime(void) const;

  AREXPORT void addPacketHandler(
      ArRetFunctor1<bool, ArRobotPacket *> *functor, 
      ArListPos::Pos position = ArListPos::LAST);
  
  AREXPORT void remPacketHandler(
      ArRetFunctor1<bool, ArRobotPacket *> *functor);

  AREXPORT void addConnectCB(ArFunctor *functor, 
                 ArListPos::Pos position = ArListPos::LAST);
  AREXPORT void remConnectCB(ArFunctor *functor);

  AREXPORT void addFailedConnectCB(ArFunctor *functor, 
                   ArListPos::Pos position = ArListPos::LAST);
  AREXPORT void remFailedConnectCB(ArFunctor *functor);

  AREXPORT void addDisconnectNormallyCB(ArFunctor *functor, 
                ArListPos::Pos position = ArListPos::LAST);
  AREXPORT void remDisconnectNormallyCB(ArFunctor *functor);
  
  AREXPORT void addDisconnectOnErrorCB(ArFunctor *functor, 
                   ArListPos::Pos position = ArListPos::LAST);
  AREXPORT void remDisconnectOnErrorCB(ArFunctor *functor);

  AREXPORT void addRunExitCB(ArFunctor *functor, 
                 ArListPos::Pos position = ArListPos::LAST);
  AREXPORT void remRunExitCB(ArFunctor *functor);

  AREXPORT WaitState waitForConnect(unsigned int msecs=0);
  AREXPORT WaitState waitForConnectOrConnFail(unsigned int msecs=0);
  AREXPORT WaitState waitForRunExit(unsigned int msecs=0);

  AREXPORT void wakeAllWaitingThreads();
  AREXPORT void wakeAllConnWaitingThreads();
  AREXPORT void wakeAllConnOrFailWaitingThreads();
  AREXPORT void wakeAllRunExitWaitingThreads();

  AREXPORT bool addUserTask(const char *name, int position, 
                   ArFunctor *functor,
                   ArTaskState::State *state = NULL);
  AREXPORT void remUserTask(const char *name);
  AREXPORT void remUserTask(ArFunctor *functor);

  AREXPORT ArSyncTask *findUserTask(const char *name);
  AREXPORT ArSyncTask *findUserTask(ArFunctor *functor);
  
  AREXPORT void logUserTasks(void) const;
  AREXPORT void logAllTasks(void) const;

  AREXPORT bool addSensorInterpTask(const char *name, int position, 
                       ArFunctor *functor,
                           ArTaskState::State *state = NULL);
  AREXPORT void remSensorInterpTask(const char *name);
  AREXPORT void remSensorInterpTask(ArFunctor *functor);

  AREXPORT ArSyncTask *findTask(const char *name);
  AREXPORT ArSyncTask *findTask(ArFunctor *functor);

  AREXPORT bool addAction(ArAction *action, int priority);
  AREXPORT bool remAction(ArAction *action);
  AREXPORT bool remAction(const char *actionName);
  AREXPORT ArAction *findAction(const char *actionName);
  AREXPORT ArResolver::ActionMap *getActionMap(void);
  AREXPORT void deactivateActions(void);

  AREXPORT void logActions(bool logDeactivated = false) const;

  AREXPORT ArResolver *getResolver(void);

  AREXPORT void setResolver(ArResolver *resolver);

  AREXPORT void setEncoderCorrectionCallback(
      ArRetFunctor1<double, ArPoseWithTime> *functor);
  AREXPORT ArRetFunctor1<double, ArPoseWithTime> *
          getEncoderCorrectionCallback(void) const;

  // set up some of the internals of how the ArRobot class works
  AREXPORT void setCycleTime(unsigned int ms);
  AREXPORT unsigned int getCycleTime(void) const;
  AREXPORT void setCycleWarningTime(unsigned int ms);
  AREXPORT unsigned int getCycleWarningTime(void) const;
  AREXPORT unsigned int getCycleWarningTime(void);
  AREXPORT void setConnectionCycleMultiplier(unsigned int multiplier);
  AREXPORT unsigned int getConnectionCycleMultiplier(void) const;
  
  void setCycleChained(bool cycleChained) { myCycleChained = cycleChained; }
  bool isCycleChained(void) const { return myCycleChained; }
  AREXPORT void setConnectionTimeoutTime(int mSecs);
  AREXPORT int getConnectionTimeoutTime(void) const;
  AREXPORT ArTime getLastPacketTime(void) const;

  AREXPORT void setPoseInterpNumReadings(size_t numReadings) 
    { myInterpolation.setNumberOfReadings(numReadings); }

  AREXPORT size_t getPoseInterpNumReadings(void) const
    { return myInterpolation.getNumberOfReadings(); }


  AREXPORT int getPoseInterpPosition(ArTime timeStamp, ArPose *position)
    { return myInterpolation.getPose(timeStamp, position); }

  AREXPORT void setEncoderPoseInterpNumReadings(size_t numReadings) 
    { myEncoderInterpolation.setNumberOfReadings(numReadings); }

  AREXPORT size_t getEncoderPoseInterpNumReadings(void) const
    { return myEncoderInterpolation.getNumberOfReadings(); }


  AREXPORT int getEncoderPoseInterpPosition(ArTime timeStamp, ArPose *position)
    { return myEncoderInterpolation.getPose(timeStamp, position); }

  AREXPORT unsigned int getCounter(void) const { return myCounter; }

  AREXPORT const ArRobotParams *getRobotParams(void) const;

  AREXPORT const ArRobotConfigPacketReader *getOrigRobotConfig(void) const;

  AREXPORT void setTransVelMax(double vel);
  AREXPORT void setTransAccel(double acc);
  AREXPORT void setTransDecel(double decel);
  AREXPORT void setRotVelMax(double vel);
  AREXPORT void setRotAccel(double acc);
  AREXPORT void setRotDecel(double decel);

  bool hasSettableVelMaxes(void) const 
    { return myParams->hasSettableVelMaxes(); }
  AREXPORT double getTransVelMax(void) const;
  AREXPORT double getRotVelMax(void) const;
  bool hasSettableAccsDecs(void)
      const { return myParams->hasSettableAccsDecs(); }
  AREXPORT double getTransAccel(void) const;
  AREXPORT double getTransDecel(void) const;
  AREXPORT double getRotAccel(void) const;
  AREXPORT double getRotDecel(void) const;

  AREXPORT bool loadParamFile(const char *file);

  AREXPORT void attachKeyHandler(ArKeyHandler *keyHandler,
                 bool exitOnEscape = true,
                 bool useExitNotShutdown = true);
  AREXPORT ArKeyHandler *getKeyHandler(void) const;

  AREXPORT int lock() {return(myMutex.lock());}
  AREXPORT int tryLock() {return(myMutex.tryLock());}
  AREXPORT int unlock() {return(myMutex.unlock());}

  AREXPORT bool isStabilizing(void) { return myIsStabilizing; }

  AREXPORT void setStabilizingTime(int mSecs);

  AREXPORT int getStabilizingTime(void) const;


  AREXPORT void addStabilizingCB(ArFunctor *functor, 
                 ArListPos::Pos position = ArListPos::LAST);
  AREXPORT void remStabilizingCB(ArFunctor *functor);
  
  AREXPORT ArSyncTask *getSyncTaskRoot(void);

  AREXPORT void loopOnce(void);
  
  AREXPORT void incCounter(void) { myCounter++; }

  AREXPORT void packetHandler(void);
  AREXPORT void actionHandler(void);
  AREXPORT void stateReflector(void);
  AREXPORT void robotLocker(void);
  AREXPORT void robotUnlocker(void);

  AREXPORT void keyHandlerExit(void);

  AREXPORT bool processMotorPacket(ArRobotPacket *packet);
  AREXPORT void processNewSonar(char number, int range, ArTime timeReceived);
  AREXPORT bool processEncoderPacket(ArRobotPacket *packet);
  AREXPORT bool processIOPacket(ArRobotPacket *packet);
  
  AREXPORT void init(void);

  AREXPORT void setUpSyncList(void);
  AREXPORT void setUpPacketHandlers(void);

  ArRetFunctor1C<bool, ArRobot, ArRobotPacket *> myMotorPacketCB;
  ArRetFunctor1C<bool, ArRobot, ArRobotPacket *> myEncoderPacketCB;
  ArRetFunctor1C<bool, ArRobot, ArRobotPacket *> myIOPacketCB;
  ArFunctorC<ArRobot> myPacketHandlerCB;
  ArFunctorC<ArRobot> myActionHandlerCB;
  ArFunctorC<ArRobot> myStateReflectorCB;
  ArFunctorC<ArRobot> myRobotLockerCB;
  ArFunctorC<ArRobot> myRobotUnlockerCB;
  ArFunctorC<ArRobot> myKeyHandlerExitCB;
  ArFunctorC<ArKeyHandler> *myKeyHandlerCB;

  // These four are internal... only people monkeying deeply should mess
  // with them, so they aren't documented... these process the cblists
  // and such
  AREXPORT int asyncConnectHandler(bool tryHarderToConnect);

  AREXPORT void dropConnection(void);
  AREXPORT void failedConnect(void);
  AREXPORT bool madeConnection(void);
  AREXPORT void startStabilization(void);
  AREXPORT void finishedConnection(void);
  AREXPORT void cancelConnection(void);

  AREXPORT bool getLogMovementSent(void) { return myLogMovementSent; }
  AREXPORT void setLogMovementSent(bool logMovementSent)
    { myLogMovementSent = logMovementSent; }

  AREXPORT bool getLogMovementReceived(void) { return myLogMovementReceived; }
  AREXPORT void setLogMovementReceived(bool logMovementReceived)
    { myLogMovementReceived = logMovementReceived; }

  AREXPORT bool getLogVelocitiesReceived(void) 
    { return myLogVelocitiesReceived; }
  AREXPORT void setLogVelocitiesReceived(bool logVelocitiesReceived) 
    { myLogVelocitiesReceived = logVelocitiesReceived; }

  AREXPORT bool getPacketsReceivedTracking(void)
    { return myPacketsReceivedTracking; }
  AREXPORT void setPacketsReceivedTracking(bool packetsReceivedTracking);

  AREXPORT bool getPacketsSentTracking(void)
    { return myPacketsSentTracking; }
  AREXPORT void setPacketsSentTracking(bool packetsSentTracking)
    { myPacketsSentTracking = packetsSentTracking; }

  AREXPORT bool getLogActions(void) { return myLogActions; }
  AREXPORT void setLogActions(bool logActions)
  { myLogActions = logActions; }


  AREXPORT bool handlePacket(ArRobotPacket *packet);
  AREXPORT std::list<ArFunctor *> * getRunExitListCopy();
  AREXPORT void processParamFile(void);

  AREXPORT ArPose getRawEncoderPose(void) const { return myRawEncoderPose; }

  AREXPORT bool getNoTimeWarningThisCycle(void)  
    { return myNoTimeWarningThisCycle; }
  AREXPORT void setNoTimeWarningThisCycle(bool noTimeWarningThisCycle)  
    { myNoTimeWarningThisCycle = noTimeWarningThisCycle; }
  // callbacks for warning time and if we should warn now to pass to sync tasks
  ArRetFunctorC<unsigned int, ArRobot> myGetCycleWarningTimeCB;
  ArRetFunctorC<bool, ArRobot> myGetNoTimeWarningThisCycleCB;
  AREXPORT void ariaExitCallback(void);
  void setConnectWithNoParams(bool connectWithNoParams) 
    { myConnectWithNoParams = connectWithNoParams; }
protected:
  enum RotDesired { 
    ROT_NONE,
    ROT_IGNORE,
    ROT_HEADING,
    ROT_VEL
  };
  enum TransDesired {
    TRANS_NONE,
    TRANS_IGNORE,
    TRANS_VEL,
    TRANS_VEL2,
    TRANS_DIST,
    TRANS_DIST_NEW
  };
  void reset(void);
 

  // the config the robot had at connection
  ArRobotConfigPacketReader *myOrigRobotConfig;
  // the values we'll maintain for the different motion parameters
  double myRotVelMax;
  double myRotAccel;
  double myRotDecel;
  double myTransVelMax;
  double myTransAccel;
  double myTransDecel;

  ArPTZ *myPtz;
  bool myNoTimeWarningThisCycle;

  long int myLeftEncoder;
  long int myRightEncoder;
  bool myFirstEncoderPose;
  ArPoseWithTime myRawEncoderPose;

  ArTransform myEncoderTransform;

  bool myLogMovementSent;
  bool myLogMovementReceived;
  bool myPacketsReceivedTracking;
  bool myLogActions;
  bool myLogVelocitiesReceived;
  double myLastVel;
  double myLastRotVel;
  double myLastHeading;
  double myLastCalculatedRotVel;

  bool myTryingToMove;

  long myPacketsReceivedTrackingCount;
  ArTime myPacketsReceivedTrackingStarted;
  bool myPacketsSentTracking;
  ArMutex myMutex;
  ArSyncTask *mySyncTaskRoot;
  std::list<ArRetFunctor1<bool, ArRobotPacket *> *> myPacketHandlerList;

  ArSyncLoop mySyncLoop;

  std::list<ArFunctor *> myStabilizingCBList;
  std::list<ArFunctor *> myConnectCBList;
  std::list<ArFunctor *> myFailedConnectCBList;
  std::list<ArFunctor *> myDisconnectNormallyCBList;
  std::list<ArFunctor *> myDisconnectOnErrorCBList;
  std::list<ArFunctor *> myRunExitCBList;

  ArRetFunctor1<double, ArPoseWithTime> *myEncoderCorrectionCB;
  std::list<ArRangeDevice *> myRangeDeviceList;

  ArCondition myConnectCond;
  ArCondition myConnOrFailCond;
  ArCondition myRunExitCond;

  ArResolver::ActionMap myActions;
  bool myOwnTheResolver;
  ArResolver *myResolver;

  std::map<int, ArSensorReading *> mySonars;
  int myNumSonar;
  
  unsigned int myCounter;
  bool myIsConnected;
  bool myIsStabilizing;

  bool myBlockingConnectRun;
  bool myAsyncConnectFlag;
  int myAsyncConnectState;
  int myAsyncConnectNoPacketCount;
  int myAsyncConnectTimesTried;
  ArTime myAsyncStartedConnection;
  int myAsyncConnectStartBaud;
  ArTime myAsyncConnectStartedChangeBaud;
  bool myAsyncConnectSentChangeBaud;
  ArTime myStartedStabilizing;

  int myStabilizingTime;

  bool mySentPulse;

  double myTransVal;
  double myTransVal2;
  int myLastTransVal;
  int myLastTransVal2;
  TransDesired myTransType;
  TransDesired myLastTransType;
  ArTime myTransSetTime;
  ArTime myLastTransSent;
  int myLastActionTransVal;
  bool myActionTransSet;
  ArPose myTransDistStart;
  double myMoveDoneDist;

  double myRotVal;
  int myLastRotVal;
  RotDesired myRotType;
  RotDesired myLastRotType;
  ArTime myRotSetTime;
  ArTime myLastRotSent;
  int myLastActionRotVal;
  bool myLastActionRotHeading;
  bool myLastActionRotStopped;
  bool myActionRotSet;
  double myHeadingDoneDiff;

  double myLastSentTransVelMax;
  double myLastSentTransAccel;
  double myLastSentTransDecel;
  double myLastSentRotVelMax;
  double myLastSentRotAccel;
  double myLastSentRotDecel;

  ArTime myLastPulseSent;

  int myDirectPrecedenceTime;
  
  int myStateReflectionRefreshTime;

  ArActionDesired myActionDesired;

  std::string myName;
  std::string myRobotName;
  std::string myRobotType;
  std::string myRobotSubType;

  double myAbsoluteMaxTransVel;
  double myAbsoluteMaxTransAccel;
  double myAbsoluteMaxTransDecel;
  double myAbsoluteMaxRotVel;
  double myAbsoluteMaxRotAccel;
  double myAbsoluteMaxRotDecel;

  ArDeviceConnection *myConn;

  ArRobotPacketSender mySender;
  ArRobotPacketReceiver myReceiver;

  ArRobotParams *myParams;
  double myRobotLengthFront;
  double myRobotLengthRear;

  ArInterpolation myInterpolation;
  ArInterpolation myEncoderInterpolation;

  ArKeyHandler *myKeyHandler;
  bool myKeyHandlerUseExitNotShutdown;

  bool myConnectWithNoParams;
  bool myWarnedAboutExtraSonar;

  // variables for tracking the data stream
  time_t myTimeLastMotorPacket;
  int myMotorPacCurrentCount;
  int myMotorPacCount;
  time_t myTimeLastSonarPacket;
  int mySonarPacCurrentCount;
  int mySonarPacCount;
  unsigned int myCycleTime;
  unsigned int myCycleWarningTime;
  unsigned int myConnectionCycleMultiplier;
  bool myCycleChained;
  ArTime myLastPacketReceivedTime;
  int myTimeoutTime;

  bool myRequestedIOPackets;
  bool myRequestedEncoderPackets;

  // all the state reflecing variables
  ArPoseWithTime myEncoderPose;
  ArTime myEncoderPoseTaken;
  ArPose myGlobalPose;
  // um, this myEncoderGlobalTrans doesn't do anything
  ArTransform myEncoderGlobalTrans;
  double myLeftVel;
  double myRightVel;
  double myBatteryVoltage;
  ArRunningAverage myBatteryAverager;
  double myRealBatteryVoltage;
  ArRunningAverage myRealBatteryAverager;
  int myStallValue;
  double myControl;
  int myFlags;
  int myFaultFlags;
  bool myHasFaultFlags;
  double myCompass;
  int myAnalogPortSelected;
  unsigned char myAnalog;
  unsigned char myDigIn;
  unsigned char myDigOut;
  int myIOAnalog[128];
  unsigned char myIODigIn[255];
  unsigned char myIODigOut[255];
  int myIOAnalogSize;
  int myIODigInSize;
  int myIODigOutSize;
  ArTime myLastIOPacketReceivedTime;
  double myVel;
  double myRotVel;
  int myLastX;
  int myLastY;
  int myLastTh;
  ChargeState myChargeState;

  double myOdometerDistance;
  double myOdometerDegrees;
  ArTime myOdometerStart;


  bool myAddedAriaExitCB;
  ArFunctorC<ArRobot> myAriaExitCB;
};


#endif // ARROBOT_H

---