ArRobot.h

/*
ActivMedia Robotics Interface for Applications (ARIA)
Copyright (C) 2004,2005 ActivMedia Robotics, LLC


     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 
ActivMedia Robotics for information about a commercial version of ARIA at 
robots@activmedia.com or 
ActivMedia Robotics, 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
  };
  ArRobot(const char * name = NULL, bool ignored = true,
                   bool doSigHandle=true, 
                   bool normalInit = true, bool addAriaExitCallback = true);
                   
  ~ArRobot();

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

  bool isRunning(void) const;

  void stopRunning(bool doDisconnect=true); 

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

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

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

  void stopStateReflection(void);
  

  void enableMotors();
  void disableMotors();

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


  void setDirectMotionPrecedenceTime(int mSec);

  unsigned int getDirectMotionPrecedenceTime(void) const;

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

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

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

  double getAbsoluteMaxRotVel(void) const 
    { return myAbsoluteMaxRotVel; }
  bool setAbsoluteMaxRotVel(double maxVel);
  
  // 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 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; }
  bool areMotorsEnabled(void) const { return (myFlags & ArUtil::BIT0); }
  bool areSonarsEnabled(void) const { return (myFlags & ArUtil::BIT1); }
  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; }
  ChargeState getChargeState(void) const;

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

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

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

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

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

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

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

  void moveTo(ArPose pose, bool doCumulative = true);
  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); }

  void requestEncoderPackets(void);

  void requestIOPackets(void);

  void stopEncoderPackets(void);

  void stopIOPackets(void);

  long int getLeftEncoder(void);

  long int getRightEncoder(void);

  void setEncoderTransform(ArPose deadReconPos,
                                    ArPose globalPos);

  void setEncoderTransform(ArPose transformPos);

  ArTransform getEncoderTransform(void) const;

  ArTransform getToGlobalTransform(void) const;

  ArTransform getToLocalTransform(void) const;

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

  void setDeadReconPose(ArPose pose);
  
  void addRangeDevice(ArRangeDevice *device);
  void remRangeDevice(const char *name);
  void remRangeDevice(ArRangeDevice *device);

#ifndef SWIG

  const ArRangeDevice *findRangeDevice(const char *name) const;
#endif

  ArRangeDevice *findRangeDevice(const char *name);

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

  bool hasRangeDevice(ArRangeDevice *device) const;

  double checkRangeDevicesCurrentPolar(double startAngle,
                                                double endAngle, 
                                                double *angle = NULL) const;

  double checkRangeDevicesCumulativePolar(double startAngle,
                                                double endAngle, 
                                                double *angle = NULL) const;


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

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

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

  void setStateReflectionRefreshTime(int msec);

  int getStateReflectionRefreshTime(void) const;

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

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

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

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

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

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

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

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

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

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

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

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

  void logActions(void) const;

  ArResolver *getResolver(void);

  void setResolver(ArResolver *resolver);

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

  // set up some of the internals of how the ArRobot class works
  void setCycleTime(unsigned int ms);
  unsigned int getCycleTime(void) const;
  void setCycleWarningTime(unsigned int ms);
  unsigned int getCycleWarningTime(void) const;
#ifndef SWIG

  unsigned int getCycleWarningTime(void);
#endif

  void setConnectionCycleMultiplier(unsigned int multiplier);
  unsigned int getConnectionCycleMultiplier(void) const;
  
  void setCycleChained(bool cycleChained) { myCycleChained = cycleChained; }
  bool isCycleChained(void) const { return myCycleChained; }
  void setConnectionTimeoutTime(int mSecs);
  int getConnectionTimeoutTime(void) const;
  ArTime getLastPacketTime(void) const;

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

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


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

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

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


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

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

  const ArRobotParams *getRobotParams(void) const;

  const ArRobotConfigPacketReader *getOrigRobotConfig(void) const;

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

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

  bool loadParamFile(const char *file);

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

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

  bool isStabilizing(void) { return myIsStabilizing; }

  void setStabilizingTime(int mSecs);

  int getStabilizingTime(void) const;


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

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

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

  void keyHandlerExit(void);

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

  void setUpSyncList(void);
  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
  int asyncConnectHandler(bool tryHarderToConnect);

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

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

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

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

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

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


  bool handlePacket(ArRobotPacket *packet);
  std::list<ArFunctor *> * getRunExitListCopy();
  void processParamFile(void);
  ArPose getRawEncoderPose(void) const { return myRawEncoderPose; }
  bool getNoTimeWarningThisCycle(void)  
    { return myNoTimeWarningThisCycle; }
  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;
  void ariaExitCallback(void);
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 myLogVelocitiesReceived;
  bool myPacketsReceivedTracking;
  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 myAbsoluteMaxRotVel;

  ArDeviceConnection *myConn;

  ArRobotPacketSender mySender;
  ArRobotPacketReceiver myReceiver;

  ArRobotParams *myParams;

  ArInterpolation myInterpolation;
  ArInterpolation myEncoderInterpolation;

  ArKeyHandler *myKeyHandler;
  bool myKeyHandlerUseExitNotShutdown;

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

  bool myAddedAriaExitCB;
  ArFunctorC<ArRobot> myAriaExitCB;
};


#endif // ARROBOT_H

---