ArModes.cpp

/*
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

*/

#include "ArExport.h"
#include "ariaOSDef.h"
#include "ArMode.h"
#include "ArModes.h"
#include "ArKeyHandler.h"
#include "ArSonyPTZ.h"
#include "ArVCC4.h"
#include "ArDPPTU.h"
#include "ArAMPTU.h"
#include "ArSick.h"
#include "ArAnalogGyro.h"
#include "ArRobotConfigPacketReader.h"
#include "ariaInternal.h"

ArModeTeleop::ArModeTeleop(ArRobot *robot, const char *name, char key, char key2): 
  ArMode(robot, name, key, key2),
  myGroup(robot)
{
  myGroup.deactivate();
}

ArModeTeleop::~ArModeTeleop()
{
  
}

void ArModeTeleop::activate(void)
{
  if (!baseActivate())
    return;
  myGroup.activateExclusive();
}

void ArModeTeleop::deactivate(void)
{
  if (!baseDeactivate())
    return;
  myGroup.deactivate();
}

void ArModeTeleop::help(void)
{
  ArLog::log(ArLog::Terse, 
           "Teleop mode will drive under your joystick or keyboard control.");
  ArLog::log(ArLog::Terse, 
             "It will not allow you to drive into obstacles it can see,");
  ArLog::log(ArLog::Terse, 
      "though if you are presistent you may be able to run into something.");
  ArLog::log(ArLog::Terse, "For joystick, hold in the trigger button and then move the joystick to drive.");
  ArLog::log(ArLog::Terse, "For keyboard control these are the keys and their actions:");
  ArLog::log(ArLog::Terse, "%13s:  speed up if forward or no motion, slow down if going backwards", "up arrow");
  ArLog::log(ArLog::Terse, "%13s:  slow down if going forwards, speed up if backward or no motion", "down arrow");
  ArLog::log(ArLog::Terse, "%13s:  turn left", "left arrow");
  ArLog::log(ArLog::Terse, "%13s:  turn right", "right arrow");
  ArLog::log(ArLog::Terse, "%13s:  stop", "space bar");
  printf("%10s %10s %10s %10s %10s %10s\n", "transVel", "rotVel", "x", "y", "th", "volts");
}

void ArModeTeleop::userTask(void)
{
  printf("\r%10.0f %10.0f %10.0f %10.0f %10.1f %10.1f", myRobot->getVel(), 
         myRobot->getRotVel(), myRobot->getX(), myRobot->getY(), 
         myRobot->getTh(), myRobot->getRealBatteryVoltage());
}

ArModeUnguardedTeleop::ArModeUnguardedTeleop(ArRobot *robot,
                                                      const char *name, 
                                                      char key, char key2): 
  ArMode(robot, name, key, key2),
  myGroup(robot)
{
  myGroup.deactivate();
}

ArModeUnguardedTeleop::~ArModeUnguardedTeleop()
{
  
}

void ArModeUnguardedTeleop::activate(void)
{
  if (!baseActivate())
    return;
  myGroup.activateExclusive();
}

void ArModeUnguardedTeleop::deactivate(void)
{
  if (!baseDeactivate())
    return;
  myGroup.deactivate();
}

void ArModeUnguardedTeleop::help(void)
{
  ArLog::log(ArLog::Terse, 
           "Unguarded teleop mode will drive under your joystick or keyboard control.");
  ArLog::log(ArLog::Terse, 
             "\n### THIS MODE IS UNGUARDED AND UNSAFE, BE CAREFUL DRIVING");
  ArLog::log(ArLog::Terse,
             "\nAs it will allow you to drive into things or down stairs.");
  ArLog::log(ArLog::Terse, "For joystick, hold in the trigger button and then move the joystick to drive.");
  ArLog::log(ArLog::Terse, "For keyboard control these are the keys and their actions:");
  ArLog::log(ArLog::Terse, "%13s:  speed up if forward or no motion, slow down if going backwards", "up arrow");
  ArLog::log(ArLog::Terse, "%13s:  slow down if going forwards, speed up if backward or no motion", "down arrow");
  ArLog::log(ArLog::Terse, "%13s:  turn left", "left arrow");
  ArLog::log(ArLog::Terse, "%13s:  turn right", "right arrow");
  ArLog::log(ArLog::Terse, "%13s:  stop", "space bar");
  printf("%10s %10s %10s %10s %10s %10s\n", "transVel", "rotVel", "x", "y", "th", "volts");
}

void ArModeUnguardedTeleop::userTask(void)
{
  printf("\r%10.0f %10.0f %10.0f %10.0f %10.1f %10.1f", myRobot->getVel(), 
         myRobot->getRotVel(), myRobot->getX(), myRobot->getY(), 
         myRobot->getTh(), myRobot->getRealBatteryVoltage());
}

ArModeWander::ArModeWander(ArRobot *robot, const char *name, char key, char key2): 
  ArMode(robot, name, key, key2),
  myGroup(robot)
{
  myGroup.deactivate();
}

ArModeWander::~ArModeWander()
{
  
}

void ArModeWander::activate(void)
{
  if (!baseActivate())
    return;
  myGroup.activateExclusive();
}

void ArModeWander::deactivate(void)
{
  if (!baseDeactivate())
    return;
  myGroup.deactivate(); 
}

void ArModeWander::help(void)
{
  ArLog::log(ArLog::Terse, "Wander mode will simply drive around forwards until it finds an obstacle,");
  ArLog::log(ArLog::Terse, "then it will turn until its clear, and continue.");
  printf("%10s %10s %10s %10s %10s %10s\n", "transVel", "rotVel", "x", "y", "th", "volts");
}

void ArModeWander::userTask(void)
{
  printf("\r%10.0f %10.0f %10.0f %10.0f %10.1f %10.1f", myRobot->getVel(), 
         myRobot->getRotVel(), myRobot->getX(), myRobot->getY(), 
         myRobot->getTh(), myRobot->getRealBatteryVoltage());
}

ArModeGripper::ArModeGripper(ArRobot *robot, const char *name, 
                                      char key, char key2): 
  ArMode(robot, name, key, key2),
  myGripper(robot),
  myOpenCB(this, &ArModeGripper::open),
  myCloseCB(this, &ArModeGripper::close),
  myUpCB(this, &ArModeGripper::up),
  myDownCB(this, &ArModeGripper::down),
  myStopCB(this, &ArModeGripper::stop),
  myExerciseCB(this, &ArModeGripper::exercise)
{
  myExercising = false;
}

ArModeGripper::~ArModeGripper()
{
  
}

void ArModeGripper::activate(void)
{
  if (!baseActivate())
    return;

  addKeyHandler(ArKeyHandler::UP, &myUpCB);
  addKeyHandler(ArKeyHandler::DOWN, &myDownCB);
  addKeyHandler(ArKeyHandler::RIGHT, &myOpenCB);
  addKeyHandler(ArKeyHandler::LEFT, &myCloseCB);  
  addKeyHandler(ArKeyHandler::SPACE, &myStopCB);
  addKeyHandler('e', &myExerciseCB);
  addKeyHandler('E', &myExerciseCB);
}

void ArModeGripper::deactivate(void)
{
  if (!baseDeactivate())
    return;

  remKeyHandler(&myUpCB);
  remKeyHandler(&myDownCB);
  remKeyHandler(&myOpenCB);
  remKeyHandler(&myCloseCB);
  remKeyHandler(&myStopCB);
  remKeyHandler(&myExerciseCB);
}

void ArModeGripper::userTask(void)
{
  int val;
  printf("\r");
  if (myGripper.getBreakBeamState() & 2) // outer 
    printf("%13s", "blocked");
  else 
    printf("%13s", "clear");
  if (myGripper.getBreakBeamState() & 1) // inner
    printf("%13s", "blocked");
  else 
    printf("%13s", "clear");
  val = myGripper.getGripState(); // gripper portion
  if (val == 0)
    printf("%13s", "between");
  else if (val == 1)
    printf("%13s", "open");
  else if (val == 2)
    printf("%13s", "closed");
  if (myGripper.isLiftMaxed()) // lift
    printf("%13s", "maxed");
  else
    printf("%13s", "clear");
  val = myGripper.getPaddleState(); // paddle section
  if (val & 1) // left paddle
    printf("%13s", "triggered");
  else
    printf("%13s", "clear");
  if (val & 2) // right paddle
    printf("%13s", "triggered");
  else
    printf("%13s", "clear");
  // exercise the thing
  if (myExercising)
  {
    switch (myExerState) {
    case UP_OPEN:
      if ((myLastExer.mSecSince() > 3000 && myGripper.isLiftMaxed()) ||
          myLastExer.mSecSince() > 30000)
      {
        myGripper.gripClose();
        myExerState = UP_CLOSE;
        myLastExer.setToNow();
        if (myLastExer.mSecSince() > 30000)
          ArLog::log(ArLog::Terse, "\nLift took more than thirty seconds to raise, there is probably a problem with it.\n");
      }
      break;
    case UP_CLOSE:
      if (myGripper.getGripState() == 2 || myLastExer.mSecSince() > 10000)
      {
        myGripper.liftDown();
        myExerState = DOWN_CLOSE;
        myLastExer.setToNow();
        if (myLastExer.mSecSince() > 10000)
          ArLog::log(ArLog::Terse, "\nGripper took more than 10 seconds to close, there is probably a problem with it.\n");
      }
      break;
    case DOWN_CLOSE:
      if ((myLastExer.mSecSince() > 3000 && myGripper.isLiftMaxed()) ||
          myLastExer.mSecSince() > 30000)
      {
        myGripper.gripOpen();
        myExerState = DOWN_OPEN;
        myLastExer.setToNow();
        if (myLastExer.mSecSince() > 30000)
          ArLog::log(ArLog::Terse, "\nLift took more than thirty seconds to raise, there is probably a problem with it.\n");
      }
      break;
    case DOWN_OPEN:
      if (myGripper.getGripState() == 1 || myLastExer.mSecSince() > 10000)
      {
        myGripper.liftUp();
        myExerState = UP_OPEN;
        myLastExer.setToNow();
        if (myLastExer.mSecSince() > 10000)
          ArLog::log(ArLog::Terse, "\nGripper took more than 10 seconds to open, there is probably a problem with it.\n");
      }
      break;
    }      
    
  }
}

void ArModeGripper::open(void)
{
  if (myExercising == true)
  {
    myExercising = false;
    myGripper.gripperHalt();
  }
  myGripper.gripOpen();
}

void ArModeGripper::close(void)
{
  if (myExercising == true)
  {
    myExercising = false;
    myGripper.gripperHalt();
  }
  myGripper.gripClose();
}

void ArModeGripper::up(void)
{
  if (myExercising == true)
  {
    myExercising = false;
    myGripper.gripperHalt();
  }
  myGripper.liftUp();
}

void ArModeGripper::down(void)
{
  if (myExercising == true)
  {
    myExercising = false;
    myGripper.gripperHalt();
  }
  myGripper.liftDown();
}

void ArModeGripper::stop(void)
{
  if (myExercising == true)
  {
    myExercising = false;
    myGripper.gripperHalt();
  }
  myGripper.gripperHalt();
}

void ArModeGripper::exercise(void)
{
  if (myExercising == false)
  {
    ArLog::log(ArLog::Terse, 
       "\nGripper will now be exercised until another command is given.");
    myExercising = true;
    myExerState = UP_OPEN;
    myGripper.liftUp();
    myGripper.gripOpen();
    myLastExer.setToNow();
  }
}

void ArModeGripper::help(void)
{
  ArLog::log(ArLog::Terse, 
             "Gripper mode will let you control or exercise the gripper.");
  ArLog::log(ArLog::Terse, 
      "If you start exercising the gripper it will stop your other commands.");
  ArLog::log(ArLog::Terse, 
             "If you use other commands it will interrupt the exercising.");
  ArLog::log(ArLog::Terse, "%13s:  raise lift", "up arrow");
  ArLog::log(ArLog::Terse, "%13s:  lower lift", "down arrow");
  ArLog::log(ArLog::Terse, "%13s:  close gripper paddles", "left arrow");
  ArLog::log(ArLog::Terse, "%13s:  open gripper paddles", "right arrow");
  ArLog::log(ArLog::Terse, "%13s:  stop gripper paddles and lift", 
             "space bar");
  ArLog::log(ArLog::Terse, "%13s:  exercise the gripper", "'e' or 'E'");
  ArLog::log(ArLog::Terse, "\nGripper status:");
  ArLog::log(ArLog::Terse, "%13s%13s%13s%13s%13s%13s", "BB outer", "BB inner",
             "Paddles", "Lift", "LeftPaddle", "RightPaddle");
  
}



ArModeCamera::ArModeCamera(ArRobot *robot, const char *name, 
                                      char key, char key2): 
  ArMode(robot, name, key, key2),
  myUpCB(this, &ArModeCamera::up),
  myDownCB(this, &ArModeCamera::down),
  myLeftCB(this, &ArModeCamera::left),
  myRightCB(this, &ArModeCamera::right),
  myCenterCB(this, &ArModeCamera::center),
  myZoomInCB(this, &ArModeCamera::zoomIn),  
  myZoomOutCB(this, &ArModeCamera::zoomOut),
  myExerciseCB(this, &ArModeCamera::exercise),
  mySonyCB(this, &ArModeCamera::sony),
  myCanonCB(this, &ArModeCamera::canon),
  myDpptuCB(this, &ArModeCamera::dpptu),
  myAmptuCB(this, &ArModeCamera::amptu),
  myCanonInvertedCB(this, &ArModeCamera::canonInverted),
  mySonySerialCB(this, &ArModeCamera::sonySerial),
  myCanonSerialCB(this, &ArModeCamera::canonSerial),
  myDpptuSerialCB(this, &ArModeCamera::dpptuSerial),
  myAmptuSerialCB(this, &ArModeCamera::amptuSerial),
  myCanonInvertedSerialCB(this, &ArModeCamera::canonInvertedSerial),
  myCom1CB(this, &ArModeCamera::com1),
  myCom2CB(this, &ArModeCamera::com2),
  myCom3CB(this, &ArModeCamera::com3),
  myCom4CB(this, &ArModeCamera::com4),
  myAux1CB(this, &ArModeCamera::aux1),
  myAux2CB(this, &ArModeCamera::aux2)
{
  myState = STATE_CAMERA;
  myExercising = false;
}

ArModeCamera::~ArModeCamera()
{
  
}

void ArModeCamera::activate(void)
{
  ArKeyHandler *keyHandler;
  if (!baseActivate())
    return;
  // see if there is already a keyhandler, if not something is wrong
  // (since constructor should make one if there isn't one yet
  if ((keyHandler = Aria::getKeyHandler()) == NULL)
  {
    ArLog::log(ArLog::Terse,"ArModeCamera::activate: There should already be a key handler, but there isn't... mode won't work");
    return;
  }
  if (myState == STATE_CAMERA)
    takeCameraKeys();
  else if (myState == STATE_PORT)
    takePortKeys();
  else if (myState == STATE_MOVEMENT)
    takeMovementKeys();
  else
    ArLog::log(ArLog::Terse,"ArModeCamera in bad state.");
  
}

void ArModeCamera::deactivate(void)
{
  if (!baseDeactivate())
    return;
  if (myState == STATE_CAMERA)
    giveUpCameraKeys();
  else if (myState == STATE_PORT)
    giveUpPortKeys();
  else if (myState == STATE_MOVEMENT)
    giveUpMovementKeys();
  else
    ArLog::log(ArLog::Terse,"ArModeCamera in bad state.");
}

void ArModeCamera::userTask(void)
{
  if (myExercising && myCam != NULL && myLastExer.mSecSince() > 10000)
  {
    switch (myExerState) {
    case CENTER:
      myCam->panTilt(myCam->getMaxNegPan(), myCam->getMaxPosTilt());
      myExerState = UP_LEFT;
      myLastExer.setToNow();
      break;
    case UP_LEFT:
      myCam->panTilt(myCam->getMaxPosPan(), myCam->getMaxPosTilt());
      myExerState = UP_RIGHT;
      myLastExer.setToNow();
      break;
    case UP_RIGHT:
      myCam->panTilt(myCam->getMaxPosPan(), myCam->getMaxNegTilt());
      myExerState = DOWN_RIGHT;
      myLastExer.setToNow();
      break;
    case DOWN_RIGHT:
      myCam->panTilt(myCam->getMaxNegPan(), myCam->getMaxNegTilt());
      myExerState = DOWN_LEFT;
      myLastExer.setToNow();
      break;
    case DOWN_LEFT:
      myCam->panTilt(0, 0);
      myExerState = CENTER;
      myLastExer.setToNow();
      break;
    }      
  }
  if (myExercising && myCam != NULL && myCam->canZoom() && 
      myLastExerZoomed.mSecSince() > 35000)
  {
    if (myExerZoomedIn)
      myCam->zoom(myCam->getMinZoom());
    else
      myCam->zoom(myCam->getMaxZoom());
    myLastExerZoomed.setToNow();
  }
}

void ArModeCamera::left(void)
{
  if (myExercising == true)
    myExercising = false;
  myCam->panRel(-5);
}

void ArModeCamera::right(void)
{
  if (myExercising == true)
    myExercising = false;
  myCam->panRel(5);
}

void ArModeCamera::up(void)
{
  if (myExercising == true)
    myExercising = false;
  myCam->tiltRel(3);
}

void ArModeCamera::down(void)
{  
  if (myExercising == true)
    myExercising = false;
  myCam->tiltRel(-3);
}

void ArModeCamera::center(void)
{
  if (myExercising == true)
    myExercising = false;
  myCam->panTilt(0, 0);
  myCam->zoom(myCam->getMinZoom());
}

void ArModeCamera::exercise(void)
{
  if (myExercising == false)
  {
    ArLog::log(ArLog::Terse, 
       "Camera will now be exercised until another command is given.");
    myExercising = true;
    myExerState = UP_LEFT;
    myLastExer.setToNow();
    myCam->panTilt(myCam->getMaxNegPan(), myCam->getMaxPosTilt());
    myLastExerZoomed.setToNow();
    myExerZoomedIn = true;
    if (myCam->canZoom())
      myCam->zoom(myCam->getMaxZoom());
  }
}

void ArModeCamera::help(void)
{
  ArLog::log(ArLog::Terse, 
             "Camera mode will let you control or exercise the camera.");
  ArLog::log(ArLog::Terse, 
      "If you start exercising the camera it will stop your other commands.");
  if (myState == STATE_CAMERA)
    helpCameraKeys();
  else if (myState == STATE_PORT)
    helpPortKeys();
  else if (myState == STATE_MOVEMENT)
    helpMovementKeys();
  else
    ArLog::log(ArLog::Terse, "Something is horribly wrong and mode camera is in no state.");
}

void ArModeCamera::zoomIn(void)
{
  if (myCam->canZoom())
  {
    myCam->zoom(myCam->getZoom() + 
         ArMath::roundInt((myCam->getMaxZoom() - myCam->getMinZoom()) * .01));
  }
}

void ArModeCamera::zoomOut(void)
{
  if (myCam->canZoom())
  {
    myCam->zoom(myCam->getZoom() - 
        ArMath::roundInt((myCam->getMaxZoom() - myCam->getMinZoom()) * .01));
  }
}

void ArModeCamera::sony(void)
{
  myCam = new ArSonyPTZ(myRobot);
  ArLog::log(ArLog::Terse, "\nSony selected, now need to select the aux port.");
  cameraToAux();
}

void ArModeCamera::canon(void)
{
  myCam = new ArVCC4(myRobot);
  ArLog::log(ArLog::Terse, "\nCanon selected, now need to select the aux port.");
  cameraToAux();
}

void ArModeCamera::dpptu(void)
{
  myCam = new ArDPPTU(myRobot);
  ArLog::log(ArLog::Terse, "\nDPPTU selected, now need to select the aux port.");
  cameraToAux();
}

void ArModeCamera::amptu(void)
{
  myCam = new ArAMPTU(myRobot);
  ArLog::log(ArLog::Terse, 
             "\nActivMedia Pan Tilt Unit selected, now need to select the aux port.");
  cameraToAux();
}

void ArModeCamera::canonInverted(void)
{
  myCam = new ArVCC4(myRobot, true);
  ArLog::log(ArLog::Terse, "\nInverted Canon selected, now need to select the aux port.");
  cameraToAux();
}

void ArModeCamera::sonySerial(void)
{
  myCam = new ArSonyPTZ(myRobot);
  ArLog::log(ArLog::Terse, "\nSony selected, now need to select serial port.");
  cameraToPort();
}

void ArModeCamera::canonSerial(void)
{
  myCam = new ArVCC4(myRobot);
  ArLog::log(ArLog::Terse, 
             "\nCanon VCC4 selected, now need to select serial port.");
  cameraToPort();
}

void ArModeCamera::dpptuSerial(void)
{
  myCam = new ArDPPTU(myRobot);
  ArLog::log(ArLog::Terse, "\nDPPTU selected, now need to select serial port.");
  cameraToPort();
}

void ArModeCamera::amptuSerial(void)
{
  myCam = new ArAMPTU(myRobot);
  ArLog::log(ArLog::Terse, "\nAMPTU selected, now need to select serial port.");
  cameraToPort();
}

void ArModeCamera::canonInvertedSerial(void)
{
  myCam = new ArVCC4(myRobot, true);
  ArLog::log(ArLog::Terse, 
             "\nInverted Canon VCC4 selected, now need to select serial port.");
  cameraToPort();
}

void ArModeCamera::com1(void)
{
  myConn.setPort(ArUtil::COM1);
  portToMovement();
}

void ArModeCamera::com2(void)
{
  myConn.setPort(ArUtil::COM2);
  portToMovement();
}

void ArModeCamera::com3(void)
{
  myConn.setPort(ArUtil::COM3);
  portToMovement();
}

void ArModeCamera::com4(void)
{
  myConn.setPort(ArUtil::COM4);
  portToMovement();
}

void ArModeCamera::aux1(void)
{
  myCam->setAuxPort(1);
  auxToMovement();
}
void ArModeCamera::aux2(void)
{
  myCam->setAuxPort(2);
  auxToMovement();
}

void ArModeCamera::cameraToMovement(void)
{
  myState = STATE_MOVEMENT;
  myCam->init();
  myRobot->setPTZ(myCam);
  giveUpCameraKeys();
  takeMovementKeys();
  helpMovementKeys();
}

void ArModeCamera::cameraToPort(void)
{
  myState = STATE_PORT;
  giveUpCameraKeys();
  takePortKeys();
  helpPortKeys();
}

void ArModeCamera::cameraToAux(void)
{
  giveUpCameraKeys();
  takeAuxKeys();
  helpAuxKeys();
}

void ArModeCamera::portToMovement(void)
{
  if (!myConn.openSimple())
  {
    ArLog::log(ArLog::Terse, 
               "\n\nCould not open camera on that port, try another port.\n");
    helpPortKeys();
    return;
  }
  myCam->setDeviceConnection(&myConn);
  myCam->init();
  myRobot->setPTZ(myCam);
  myState = STATE_MOVEMENT;
  giveUpPortKeys();
  takeMovementKeys();
  helpMovementKeys();
}

void ArModeCamera::auxToMovement(void)
{
  myCam->init();
  myRobot->setPTZ(myCam);
  myState = STATE_MOVEMENT;
  giveUpAuxKeys();
  takeMovementKeys();
  helpMovementKeys();
}

void ArModeCamera::takeCameraKeys(void)
{
  addKeyHandler('1', &mySonyCB);
  addKeyHandler('2', &myCanonCB);
  addKeyHandler('3', &myDpptuCB);
  addKeyHandler('4', &myAmptuCB);
  addKeyHandler('5', &myCanonInvertedCB);
  addKeyHandler('!', &mySonySerialCB);
  addKeyHandler('@', &myCanonSerialCB);
  addKeyHandler('#', &myDpptuSerialCB);
  addKeyHandler('$', &myAmptuSerialCB);
  addKeyHandler('%', &myCanonInvertedSerialCB);
}

void ArModeCamera::giveUpCameraKeys(void)
{
  remKeyHandler(&myCanonCB);
  remKeyHandler(&mySonyCB);
  remKeyHandler(&myDpptuCB);
  remKeyHandler(&myAmptuCB);
  remKeyHandler(&myCanonInvertedCB);
  remKeyHandler(&mySonySerialCB);
  remKeyHandler(&myCanonSerialCB);
  remKeyHandler(&myDpptuSerialCB);
  remKeyHandler(&myAmptuSerialCB);
  remKeyHandler(&myCanonInvertedSerialCB);
}

void ArModeCamera::helpCameraKeys(void)
{
  ArLog::log(ArLog::Terse, 
             "You now need to select what type of camera you have.");
  ArLog::log(ArLog::Terse, 
             "%13s: select a SONY PTZ camera attached to the robot", "'1'");
  ArLog::log(ArLog::Terse, 
             "%13s: select a Canon VCC4 camera attached to the robot", "'2'");
  ArLog::log(ArLog::Terse, 
             "%13s: select a DPPTU camera attached to the robot", "'3'");
  ArLog::log(ArLog::Terse, 
             "%13s: select an AMPTU camera attached to the robot", "'4'");
  ArLog::log(ArLog::Terse, 
             "%13s: select an inverted Canon VCC4 camera attached to the robot", "'5'");

  ArLog::log(ArLog::Terse, 
             "%13s: select a SONY PTZ camera attached to a serial port", 
             "'!'");
  ArLog::log(ArLog::Terse, 
             "%13s: select a Canon VCC4 camera attached to a serial port", 
             "'@'");
  ArLog::log(ArLog::Terse, 
             "%13s: select a DPPTU camera attached to a serial port",
             "'#'");
  ArLog::log(ArLog::Terse, 
             "%13s: select an AMPTU camera attached to a serial port", 
             "'$'");
  ArLog::log(ArLog::Terse, 
             "%13s: select an inverted Canon VCC4 camera attached to a serial port", 
             "'%'");
}

void ArModeCamera::takePortKeys(void)
{
  addKeyHandler('1', &myCom1CB);
  addKeyHandler('2', &myCom2CB);
  addKeyHandler('3', &myCom3CB);
  addKeyHandler('4', &myCom4CB);
}

void ArModeCamera::giveUpPortKeys(void)
{
  remKeyHandler(&myCom1CB);
  remKeyHandler(&myCom2CB);
  remKeyHandler(&myCom3CB);
  remKeyHandler(&myCom4CB);
}

void ArModeCamera::helpPortKeys(void)
{
  ArLog::log(ArLog::Terse, 
             "You now need to select what port your camera is on.");
  ArLog::log(ArLog::Terse, "%13s:  select COM1 or /dev/ttyS0", "'1'");
  ArLog::log(ArLog::Terse, "%13s:  select COM2 or /dev/ttyS1", "'2'");
  ArLog::log(ArLog::Terse, "%13s:  select COM3 or /dev/ttyS2", "'3'");
  ArLog::log(ArLog::Terse, "%13s:  select COM4 or /dev/ttyS3", "'4'");
}

void ArModeCamera::takeAuxKeys(void)
{
  addKeyHandler('1', &myAux1CB);
  addKeyHandler('2', &myAux2CB);
}

void ArModeCamera::giveUpAuxKeys(void)
{
  remKeyHandler(&myAux1CB);
  remKeyHandler(&myAux2CB);
}

void ArModeCamera::helpAuxKeys(void)
{
  ArLog::log(ArLog::Terse,
             "You now need to select what aux port your camera is on.");
  ArLog::log(ArLog::Terse, "%13s:  select AUX1", "'1'");
  ArLog::log(ArLog::Terse, "%13s:  select AUX2", "'2'");
}

void ArModeCamera::takeMovementKeys(void)
{
  addKeyHandler(ArKeyHandler::UP, &myUpCB);
  addKeyHandler(ArKeyHandler::DOWN, &myDownCB);
  addKeyHandler(ArKeyHandler::LEFT, &myLeftCB);
  addKeyHandler(ArKeyHandler::RIGHT, &myRightCB);
  addKeyHandler(ArKeyHandler::SPACE, &myCenterCB);
  addKeyHandler('e', &myExerciseCB);
  addKeyHandler('E', &myExerciseCB);
  if (myCam->canZoom())
  {
    addKeyHandler('z', &myZoomInCB);
    addKeyHandler('Z', &myZoomInCB);
    addKeyHandler('x', &myZoomOutCB);
    addKeyHandler('X', &myZoomOutCB);
  }
}

void ArModeCamera::giveUpMovementKeys(void)
{
  remKeyHandler(&myUpCB);
  remKeyHandler(&myDownCB);
  remKeyHandler(&myLeftCB);
  remKeyHandler(&myRightCB);
  remKeyHandler(&myCenterCB);
  remKeyHandler(&myExerciseCB);
  if (myCam->canZoom())
  {
    remKeyHandler(&myZoomInCB);
    remKeyHandler(&myZoomOutCB);
  }
}

void ArModeCamera::helpMovementKeys(void)
{
  ArLog::log(ArLog::Terse, 
             "Camera mode will now let you move the camera.");
  ArLog::log(ArLog::Terse, "%13s:  tilt camera up", "up arrow");
  ArLog::log(ArLog::Terse, "%13s:  tilt camera down", "down arrow");
  ArLog::log(ArLog::Terse, "%13s:  pan camera left", "left arrow");
  ArLog::log(ArLog::Terse, "%13s:  pan camera right", "right arrow");
  ArLog::log(ArLog::Terse, "%13s:  center camera and zoom out", 
             "space bar");
  ArLog::log(ArLog::Terse, "%13s:  exercise the camera", "'e' or 'E'");
  if (myCam->canZoom())
  {
    ArLog::log(ArLog::Terse, "%13s:  zoom in", "'z' or 'Z'");
    ArLog::log(ArLog::Terse, "%13s:  zoom out", "'x' or 'X'");
  }
}

ArModeSonar::ArModeSonar(ArRobot *robot, const char *name, char key,
                                  char key2) :
  ArMode(robot, name, key, key2),
  myAllSonarCB(this, &ArModeSonar::allSonar),
  myFirstSonarCB(this, &ArModeSonar::firstSonar),
  mySecondSonarCB(this, &ArModeSonar::secondSonar),
  myThirdSonarCB(this, &ArModeSonar::thirdSonar),
  myFourthSonarCB(this, &ArModeSonar::fourthSonar)
{
  myState = STATE_FIRST;
}

ArModeSonar::~ArModeSonar()
{

}

void ArModeSonar::activate(void)
{
  if (!baseActivate())
    return;
  addKeyHandler('1', &myAllSonarCB);
  addKeyHandler('2', &myFirstSonarCB);
  addKeyHandler('3', &mySecondSonarCB);
  addKeyHandler('4', &myThirdSonarCB);
  addKeyHandler('5', &myFourthSonarCB);
}

void ArModeSonar::deactivate(void)
{
  if (!baseDeactivate())
    return;
  remKeyHandler(&myAllSonarCB);
  remKeyHandler(&myFirstSonarCB);
  remKeyHandler(&mySecondSonarCB);
  remKeyHandler(&myThirdSonarCB);
  remKeyHandler(&myFourthSonarCB);
}

void ArModeSonar::help(void)
{
  int i;
  ArLog::log(ArLog::Terse, "This mode displays different segments of sonar.");
  ArLog::log(ArLog::Terse, 
             "You can use these keys to switch what is displayed:");
  ArLog::log(ArLog::Terse, "%13s: display all sonar", "'1'");
  ArLog::log(ArLog::Terse, "%13s: display sonar 0 - 7", "'2'");
  ArLog::log(ArLog::Terse, "%13s: display sonar 8 - 15", "'3'");
  ArLog::log(ArLog::Terse, "%13s: display sonar 16 - 23", "'4'");
  ArLog::log(ArLog::Terse, "%13s: display sonar 24 - 31", "'5'");
  ArLog::log(ArLog::Terse, "Sonar readings:");
  if (myState == STATE_ALL)
  {
    ArLog::log(ArLog::Terse, "Displaying all sonar.");
    for (i = 0; i < myRobot->getNumSonar(); ++i)
      printf("%6d", i); 
  }
  else if (myState == STATE_FIRST)
  {
    ArLog::log(ArLog::Terse, "Displaying 0-7 sonar.");
    for (i = 0; i < myRobot->getNumSonar() && i <= 7; ++i)
      printf("%6d", i); 
  }
  else if (myState == STATE_SECOND)
  {
    ArLog::log(ArLog::Terse, "Displaying 8-15 sonar.");
    for (i = 8; i < myRobot->getNumSonar() && i <= 15; ++i)
      printf("%6d", i); 
  }
  else if (myState == STATE_THIRD)
  {
    ArLog::log(ArLog::Terse, "Displaying 16-23 sonar.");
    for (i = 16; i < myRobot->getNumSonar() && i <= 23; ++i)
      printf("%6d", i); 
  }
  else if (myState == STATE_FOURTH)
  {
    ArLog::log(ArLog::Terse, "Displaying 24-31 sonar.");
    for (i = 24; i < myRobot->getNumSonar() && i <= 31; ++i)
      printf("%6d", i); 
  }
  printf("\n");
}

void ArModeSonar::userTask(void)
{
  int i;
  printf("\r");
  if (myState == STATE_ALL)
  {
    for (i = 0; i < myRobot->getNumSonar(); ++i)
      printf("%6d", myRobot->getSonarRange(i)); 
  }
  else if (myState == STATE_FIRST)
  {
     for (i = 0; i < myRobot->getNumSonar() && i <= 7; ++i)
      printf("%6d", myRobot->getSonarRange(i));
  }
  else if (myState == STATE_SECOND)
  {
     for (i = 8; i < myRobot->getNumSonar() && i <= 15; ++i)
      printf("%6d", myRobot->getSonarRange(i));
  }
  else if (myState == STATE_THIRD)
  {
    for (i = 16; i < myRobot->getNumSonar() && i <= 23; ++i)
      printf("%6d", myRobot->getSonarRange(i)); 
  }
  else if (myState == STATE_FOURTH)
  {
    for (i = 24; i < myRobot->getNumSonar() && i <= 31; ++i)
      printf("%6d", myRobot->getSonarRange(i)); 
  }
}

void ArModeSonar::allSonar(void)
{
  myState = STATE_ALL;
  printf("\n");
  help();
}

void ArModeSonar::firstSonar(void)
{
  myState = STATE_FIRST;
  printf("\n");
  help();
}

void ArModeSonar::secondSonar(void)
{
  myState = STATE_SECOND;
  printf("\n");
  help();
}

void ArModeSonar::thirdSonar(void)
{
  myState = STATE_THIRD;
  printf("\n");
  help();
}

void ArModeSonar::fourthSonar(void)
{
  myState = STATE_FOURTH;
  printf("\n");
  help();
}

ArModeBumps::ArModeBumps(ArRobot *robot, const char *name, char key, char key2): 
  ArMode(robot, name, key, key2)
{
}

ArModeBumps::~ArModeBumps()
{
  
}

void ArModeBumps::activate(void)
{
  if (!baseActivate())
    return;
}

void ArModeBumps::deactivate(void)
{
  if (!baseDeactivate())
    return;
}

void ArModeBumps::help(void)
{
  unsigned int i;
  ArLog::log(ArLog::Terse, "Bumps mode will display whether bumpers are triggered or not...");
  ArLog::log(ArLog::Terse, "keep in mind it is assuming you have a full bump ring... so you should");
  ArLog::log(ArLog::Terse, "ignore readings for where there aren't bumpers.");
  ArLog::log(ArLog::Terse, "Bumper readings:");
  for (i = 0; i < myRobot->getNumFrontBumpers(); i++)
  {
    printf("%6d", i + 1);
  }
  printf(" |");
  for (i = 0; i < myRobot->getNumRearBumpers(); i++)
  {
    printf("%6d", i + 1);
  }
  printf("\n");
}

void ArModeBumps::userTask(void)
{
  unsigned int i;
  int val;
  int bit;
  if (myRobot == NULL)
    return;
  printf("\r");
  val = ((myRobot->getStallValue() & 0xff00) >> 8);
  for (i = 0, bit = 2; i < myRobot->getNumFrontBumpers(); i++, bit *= 2)
  {
    if (val & bit)
      printf("%6s", "trig");
    else
      printf("%6s", "clear");
  }
  printf(" |");
  val = ((myRobot->getStallValue() & 0xff));
  for (i = 0, bit = 2; i < myRobot->getNumRearBumpers(); i++, bit *= 2)
  {
    if (val & bit)
      printf("%6s", "trig");
    else
      printf("%6s", "clear");
  }

}

ArModePosition::ArModePosition(ArRobot *robot, const char *name, char key, char key2, ArAnalogGyro *gyro): 
  ArMode(robot, name, key, key2),
  myUpCB(this, &ArModePosition::up),
  myDownCB(this, &ArModePosition::down),
  myLeftCB(this, &ArModePosition::left),
  myRightCB(this, &ArModePosition::right),
  myStopCB(this, &ArModePosition::stop),
  myResetCB(this, &ArModePosition::reset),
  myModeCB(this, &ArModePosition::mode),
  myGyroCB(this, &ArModePosition::gyro),
  myIncDistCB(this, &ArModePosition::incDistance),
  myDecDistCB(this, &ArModePosition::decDistance)
{
  myGyro = gyro;
  myMode = MODE_BOTH;
  myModeString = "both";
  myInHeadingMode = false;
  myDistance = 1000;

  if (myGyro != NULL)
    myGyroZero = myGyro->getHeading();
  myRobotZero = myRobot->getRawEncoderPose().getTh();
  myInHeadingMode = true;
  myHeading = myRobot->getTh();
}

ArModePosition::~ArModePosition()
{
  
}

void ArModePosition::activate(void)
{
  if (!baseActivate())
    return;

  addKeyHandler(ArKeyHandler::UP, &myUpCB);
  addKeyHandler(ArKeyHandler::DOWN, &myDownCB);
  addKeyHandler(ArKeyHandler::LEFT, &myLeftCB);  
  addKeyHandler(ArKeyHandler::RIGHT, &myRightCB);
  addKeyHandler(ArKeyHandler::SPACE, &myStopCB);
  addKeyHandler(ArKeyHandler::PAGEUP, &myIncDistCB);
  addKeyHandler(ArKeyHandler::PAGEDOWN, &myDecDistCB);
  addKeyHandler('r', &myResetCB);
  addKeyHandler('R', &myResetCB);
  addKeyHandler('x', &myModeCB);
  addKeyHandler('X', &myModeCB);
  addKeyHandler('z', &myGyroCB);
  addKeyHandler('Z', &myGyroCB);
}

void ArModePosition::deactivate(void)
{
  if (!baseDeactivate())
    return;

  remKeyHandler(&myUpCB);
  remKeyHandler(&myDownCB);
  remKeyHandler(&myLeftCB);
  remKeyHandler(&myRightCB);
  remKeyHandler(&myStopCB);
  remKeyHandler(&myResetCB);
  remKeyHandler(&myModeCB);
  remKeyHandler(&myGyroCB);
  remKeyHandler(&myIncDistCB);
  remKeyHandler(&myDecDistCB);
}

void ArModePosition::up(void)
{
  myRobot->move(myDistance);
  if (myInHeadingMode)
  {
    myInHeadingMode = false;
    myHeading = myRobot->getTh();
  }
}

void ArModePosition::down(void)
{
  myRobot->move(-myDistance);
  if (myInHeadingMode)
  {
    myInHeadingMode = false;
    myHeading = myRobot->getTh();
  }
}

void ArModePosition::incDistance(void)
{
  myDistance += 500;
}

void ArModePosition::decDistance(void)
{
  myDistance -= 500;
  if(myDistance < 500) myDistance = 500;
}

void ArModePosition::left(void)
{
  myRobot->setDeltaHeading(90);
  myInHeadingMode = true;
}

void ArModePosition::right(void)
{
  myRobot->setDeltaHeading(-90);
  myInHeadingMode = true;
}

void ArModePosition::stop(void)
{
  myRobot->stop();
  myInHeadingMode = true;
}

void ArModePosition::reset(void)
{
  myRobot->stop();
  myRobot->moveTo(ArPose(0, 0, 0));
  if (myGyro != NULL)
    myGyroZero = myGyro->getHeading();
  myRobotZero = myRobot->getRawEncoderPose().getTh();
  myInHeadingMode = true;
  myHeading = myRobot->getTh();
}

void ArModePosition::mode(void)
{
  if (myMode == MODE_BOTH)
  {
    myMode = MODE_EITHER;
    myModeString = "either";
    myInHeadingMode = true;
    myRobot->stop();
  }
  else if (myMode == MODE_EITHER)
  {
    myMode = MODE_BOTH;
    myModeString = "both";
  }
}

void ArModePosition::gyro(void)
{
  if (myGyro == NULL || !myGyro->haveGottenData())
    return;

  if (myGyro != NULL && myGyro->isActive())
    myGyro->deactivate();
  else if (myGyro != NULL && !myGyro->isActive())
    myGyro->activate();

  help();
}

void ArModePosition::help(void)
{
  ArLog::log(ArLog::Terse, "Mode is one of two values:");
  ArLog::log(ArLog::Terse, "%13s: heading and move can happen simultaneously", 
             "both");
  ArLog::log(ArLog::Terse, "%13s: only heading or move is active (move holds heading)", "either");
  ArLog::log(ArLog::Terse, "");
  ArLog::log(ArLog::Terse, "%13s:  forward %.1f meter(s)", "up arrow", myDistance/1000.0);
  ArLog::log(ArLog::Terse, "%13s:  backward %.1f meter(s)", "down arrow", myDistance/1000.0);
  ArLog::log(ArLog::Terse, "%13s:  increase distance by 1/2 meter", "page up");
  ArLog::log(ArLog::Terse, "%13s:  decrease distance by 1/2 meter", "page down");
  ArLog::log(ArLog::Terse, "%13s:  turn left 90 degrees", "left arrow");
  ArLog::log(ArLog::Terse, "%13s:  turn right 90 degrees", "right arrow");
  ArLog::log(ArLog::Terse, "%13s:  stop", "space bar");
  ArLog::log(ArLog::Terse, "%13s:  reset position to (0, 0, 0)", "'r' or 'R'");
  ArLog::log(ArLog::Terse, "%13s:  switch heading/velocity mode","'x' or 'X'");
  if (myGyro != NULL && myGyro->haveGottenData())
    ArLog::log(ArLog::Terse, "%13s:  turn gyro on or off (stays this way in other modes)","'z' or 'Z'");

  ArLog::log(ArLog::Terse, "");
  ArLog::log(ArLog::Terse, "Position mode shows the position stats on a robot.");
  if (myGyro != NULL && myGyro->haveGottenData())
    ArLog::log(ArLog::Terse, "%7s%7s%9s%7s%8s%7s%8s%6s%10s%10s", "x", "y", "th", "comp", "volts", "mpacs", "mode", "gyro", "gyro_th", "robot_th");
  else
    ArLog::log(ArLog::Terse, "%7s%7s%9s%7s%8s%7s%8s", "x", "y", "th", "comp", "volts", "mpacs", "mode");
  
}

void ArModePosition::userTask(void)
{
  if (myRobot == NULL)
    return;
  // if we're in either mode and not in the heading mode try to keep the
  // same heading (in heading mode its controlled by those commands)
  if (myMode == MODE_EITHER && !myInHeadingMode)
  {
    myRobot->setHeading(myHeading);
  }
  double voltage;
  if (myRobot->getRealBatteryVoltage() > 0)
    voltage = myRobot->getRealBatteryVoltage();
  else
    voltage = myRobot->getBatteryVoltage();
  if (myGyro != NULL && myGyro->haveGottenData())
    printf("\r%7.0f%7.0f%9.2f%7.0f%8.2f%7d%8s%6s%10.2f%10.2f", 
           myRobot->getX(),  myRobot->getY(), myRobot->getTh(), 
           myRobot->getCompass(), voltage,
           myRobot->getMotorPacCount(),
           myMode == MODE_BOTH ? "both" : "either", 
           myGyro->isActive() ? "on" : "off", 
           ArMath::subAngle(myGyro->getHeading(), myGyroZero), 
           ArMath::subAngle(myRobot->getRawEncoderPose().getTh(),myRobotZero));
  else
    printf("\r%7.0f%7.0f%9.2f%7.0f%8.2f%7d%8s", myRobot->getX(), 
           myRobot->getY(), myRobot->getTh(), myRobot->getCompass(), 
           voltage, myRobot->getMotorPacCount(), 
           myMode == MODE_BOTH ? "both" : "either");
}

ArModeIO::ArModeIO(ArRobot *robot, const char *name, char key, char key2): 
  ArMode(robot, name, key, key2)
{
}

ArModeIO::~ArModeIO()
{
  
}

void ArModeIO::activate(void)
{
  if (!baseActivate())
    return;
  if (myRobot == NULL)
    return;
  myRobot->comInt(ArCommands::IOREQUEST, 2);
  myOutput[0] = '\0';
  myLastPacketTime = myRobot->getIOPacketTime();
}

void ArModeIO::deactivate(void)
{
  if (!baseDeactivate())
    return;
  if (myRobot == NULL)
    return;
  myRobot->comInt(ArCommands::IOREQUEST, 0);
}

void ArModeIO::help(void)
{
  ArLog::log(ArLog::Terse, 
             "IO mode shows the IO (digin, digout, a/d) from the robot.");
  myExplanationReady = false;
  myExplained = false;
}

void ArModeIO::userTask(void)
{
  int num;
  int i, j;
  unsigned int value;
  int bit;
  char label[256];
  myOutput[0] = '\0';

  if (myLastPacketTime.mSecSince(myRobot->getIOPacketTime()) == 0)
    return;

  if (!myExplanationReady)
    myExplanation[0] = '\0';

  value = myRobot->getFlags();
  if (!myExplanationReady)
  {
    sprintf(label, "flags");
    sprintf(myExplanation, "%s%17s  ", myExplanation, label);
  }
  for (j = 0, bit = 1; j < 16; ++j, bit *= 2)
  {
    if (j == 8)
      sprintf(myOutput, "%s ", myOutput);
    if (value & bit)
      sprintf(myOutput, "%s%d", myOutput, 1);
    else
      sprintf(myOutput, "%s%d", myOutput, 0);
  }
  sprintf(myOutput, "%s  ", myOutput);

  num = myRobot->getIODigInSize();
  for (i = 0; i < num; ++i)
  {
    value = myRobot->getIODigIn(i);
    if (!myExplanationReady)
    {
      sprintf(label, "digin%d", i);
      sprintf(myExplanation, "%s%8s  ", myExplanation, label);
    }
    for (j = 0, bit = 1; j < 8; ++j, bit *= 2)
    {
      if (value & bit)
        sprintf(myOutput, "%s%d", myOutput, 1);
      else
        sprintf(myOutput, "%s%d", myOutput, 0);
    }
    sprintf(myOutput, "%s  ", myOutput);
  }

  num = myRobot->getIODigOutSize();
  for (i = 0; i < num; ++i)
  {
    value = myRobot->getIODigOut(i);
    if (!myExplanationReady)
    {
      sprintf(label, "digout%d", i);
      sprintf(myExplanation, "%s%8s  ", myExplanation, label);
    }
    for (j = 0, bit = 1; j < 8; ++j, bit *= 2)
    {
      if (value & bit)
        sprintf(myOutput, "%s%d", myOutput, 1);
      else
        sprintf(myOutput, "%s%d", myOutput, 0);
    }
    sprintf(myOutput, "%s  ", myOutput);
  }

  num = myRobot->getIOAnalogSize();
  for (i = 0; i < num; ++i)
  {
    if (!myExplanationReady)
    {
      sprintf(label, "a/d%d", i);
      sprintf(myExplanation, "%s%6s", myExplanation, label);
    }
    
    /*
      int ad = myRobot->getIOAnalog(i);
      double adVal;
    ad &= 0xfff;
    adVal = ad * .0048828;
    sprintf(myOutput, "%s%6.2f", myOutput,adVal);
    */
    sprintf(myOutput, "%s%6.2f", myOutput, myRobot->getIOAnalogVoltage(i));
    
  }

  if (!myExplained)
  {
    printf("\n%s\n", myExplanation);
    myExplained = true;
  }

  printf("\r%s", myOutput);
}

ArModeLaser::ArModeLaser(ArRobot *robot, const char *name, 
                                  char key, char key2, ArSick *sick) :
  ArMode(robot, name, key, key2),
  myFailedConnectCB(this, &ArModeLaser::failedConnect)
{
  myState = STATE_UNINITED;
  mySick = sick;
  myConnectingLaser = false;
  myInitializedLaser = false;
}

ArModeLaser::~ArModeLaser()
{
}

void ArModeLaser::activate(void)
{
  if (!baseActivate())
    return;

  if (myRobot == NULL || mySick == NULL)
  {
    ArLog::log(ArLog::Terse, "Laser mode activated but either no robot or laser given to it.");
    return;
  }
  if (myState == STATE_UNINITED)
  {
    mySick->lockDevice();
    if (mySick->isConnected())
    {
      ArLog::log(ArLog::Verbose, 
                 "\nArModeLaser using already existing and connected laser.");
      myState = STATE_CONNECTED;
    }
    else if (myConnectingLaser && !mySick->tryingToConnect())
    {
      ArLog::log(ArLog::Terse, 
                 "\nArModeLaser trying to connect to a laser its already started once.");
      mySick->asyncConnect();
      myState = STATE_CONNECTING;
    } 
    else if (myConnectingLaser)
    {
      ArLog::log(ArLog::Terse, "\nArModeLaser already connecting to laser.");
    }
    else
    {
      ArLog::log(ArLog::Terse,
                 "\nArModeLaser is connecting to the laser.");
      myConnectingLaser = true;
      myInitializedLaser = true;
      myRobot->remRangeDevice(mySick);
      myRobot->addRangeDevice(mySick);
      mySick->remFailedConnectCB(&myFailedConnectCB);
      mySick->addFailedConnectCB(&myFailedConnectCB, ArListPos::LAST);
      // run the laser in its own thread if it isn't already
      if (!mySick->getRunning())
        mySick->runAsync();
      // connect the thing
      if (!mySick->tryingToConnect())
        mySick->asyncConnect();
      myState = STATE_CONNECTING;
    }
    mySick->unlockDevice();
  } 
}

void ArModeLaser::deactivate(void)
{
  if (!baseDeactivate())
    return;
}

void ArModeLaser::help(void)
{
  ArLog::log(ArLog::Terse, 
             "Laser mode connects to a laser, or uses a previously established connection.");
  ArLog::log(ArLog::Terse,
             "Laser mode then displays the closest and furthest reading from the laser.");

}


void ArModeLaser::userTask(void)
{
  double dist, angle;
  const std::list<ArPoseWithTime *> *readings;
  std::list<ArPoseWithTime *>::const_iterator it;
  double farDist, farAngle;
  bool found;
  int i = 0;

  if (myRobot == NULL || mySick == NULL)
    return;
  if (myState == STATE_CONNECTED)
  {
    mySick->lockDevice();
    if (!mySick->isConnected())
    {
      ArLog::log(ArLog::Terse, "\n\nLaser mode lost connection to the laser.");
      ArLog::log(ArLog::Terse, "Switch out of this mode and back if you want to try reconnecting to the laser.\n");
      myState = STATE_UNINITED;
    }
    dist = mySick->currentReadingPolar(-90, 90, &angle);
    if (dist < mySick->getMaxRange())
      printf("\rClose: %10.2fmm %5.2f deg\t", dist, angle);
    else
      printf("\rNo close reading.\t\t");
    readings = mySick->getCurrentBuffer();
    for (it = readings->begin(), found = false; it != readings->end(); it++)
    {
      i++;
      dist = (*it)->findDistanceTo(ArPose(0, 0));
      angle = (*it)->findAngleTo(ArPose(0, 0));
      if (!found || dist > farDist)
      {
        found = true;
        farDist = dist;
        farAngle = angle;
      }
    }
    if (found)
      printf("Far: %10.2fmm %5.2f deg\t", 
             farDist, farAngle);
    else
      printf("No far reading found\t");
    printf("%d readings", readings->size());
    mySick->unlockDevice();
  }
  else if (myState == STATE_CONNECTING)
  {
    mySick->lockDevice();
    if (mySick->isConnected())
    {
      ArLog::log(ArLog::Terse, "\nLaser mode has connected to the laser.\n");
      myState = STATE_CONNECTED;
    }
    mySick->unlockDevice();
  }
}

void ArModeLaser::failedConnect(void)
{
  ArLog::log(ArLog::Terse, "\nLaser mode failed to connect to the laser.\n");
  ArLog::log(ArLog::Terse, 
             "Switch out of this mode and back to try reconnecting.\n");
  myState = STATE_UNINITED;
}

ArModeActs::ArModeActs(ArRobot *robot, const char *name, char key, 
                                char key2, ArACTS_1_2 *acts): 
  ArMode(robot, name, key, key2),
  myChannel1CB(this, &ArModeActs::channel1),
  myChannel2CB(this, &ArModeActs::channel2),
  myChannel3CB(this, &ArModeActs::channel3),
  myChannel4CB(this, &ArModeActs::channel4),
  myChannel5CB(this, &ArModeActs::channel5),
  myChannel6CB(this, &ArModeActs::channel6),
  myChannel7CB(this, &ArModeActs::channel7),
  myChannel8CB(this, &ArModeActs::channel8),
  myStopCB(this, &ArModeActs::stop),
  myStartCB(this, &ArModeActs::start),
  myToggleAcquireCB(this, &ArModeActs::toggleAcquire)
{
  if (acts != NULL)
    myActs = acts;
  else
    myActs = new ArACTS_1_2;
  myRobot = robot;
  myActs->openPort(myRobot);
  myGroup = new ArActionGroupColorFollow(myRobot, myActs, camera);
  myGroup->deactivate();
}

// Destructor
ArModeActs::~ArModeActs()
{
  
}

// Activate the mode
void ArModeActs::activate(void)
{
  // Activate the group
  if (!baseActivate())
    return;
  myGroup->activateExclusive();
  
  // Add key handlers for keyboard input
  addKeyHandler(ArKeyHandler::SPACE, &myStopCB);
  addKeyHandler('z', &myStartCB);
  addKeyHandler('Z', &myStartCB);
  addKeyHandler('x', &myToggleAcquireCB);
  addKeyHandler('X', &myToggleAcquireCB);
  addKeyHandler('1', &myChannel1CB);
  addKeyHandler('2', &myChannel2CB);
  addKeyHandler('3', &myChannel3CB);
  addKeyHandler('4', &myChannel4CB);
  addKeyHandler('5', &myChannel5CB);
  addKeyHandler('6', &myChannel6CB);
  addKeyHandler('7', &myChannel7CB);
  addKeyHandler('8', &myChannel8CB);

  // Set the camera
  camera = myRobot->getPTZ();
  
  // Tell us whether we are connected to ACTS or not
  if(myActs->isConnected())
    { 
      printf("\nConnected to ACTS.\n");
    }
  else printf("\nNot connected to ACTS.\n");

  // Tell us whether a camera is defined or not
  if(camera != NULL)
    {
      printf("\nCamera defined.\n\n");
      myGroup->setCamera(camera);
    }
  else
    {  
      printf("\nNo camera defined.\n");
      printf("The robot will not tilt its camera up or down until\n");
      printf("a camera has been defined in camera mode ('c' or 'C').\n\n");
    }
}

// Deactivate the group
void ArModeActs::deactivate(void)
{
  if (!baseDeactivate())
    return;

  // Remove the key handlers
  remKeyHandler(&myStopCB);
  remKeyHandler(&myStartCB);
  remKeyHandler(&myToggleAcquireCB);
  remKeyHandler(&myChannel1CB);
  remKeyHandler(&myChannel2CB);
  remKeyHandler(&myChannel3CB);
  remKeyHandler(&myChannel4CB);
  remKeyHandler(&myChannel5CB);
  remKeyHandler(&myChannel6CB);
  remKeyHandler(&myChannel7CB);
  remKeyHandler(&myChannel8CB);

  myGroup->deactivate();
}

// Display the available commands
void ArModeActs::help(void)
{
  ArLog::log(ArLog::Terse, 
           "ACTS mode will drive the robot in an attempt to follow a color blob.\n");

  ArLog::log(ArLog::Terse, "%20s:  Pick a channel",     "1 - 8    ");
  ArLog::log(ArLog::Terse, "%20s:  toggle acquire mode", "'x' or 'X'");
  ArLog::log(ArLog::Terse, "%20s:  start movement",     "'z' or 'Z'");
  ArLog::log(ArLog::Terse, "%20s:  stop movement",      "space bar");
  ArLog::log(ArLog::Terse, "");
  
}

// Display data about this mode
void ArModeActs::userTask(void)
{
  int myChannel;

  char *acquire;
  char *move;
  char *blob;

  myChannel = myGroup->getChannel();
  if(myGroup->getAcquire()) acquire = "actively acquiring";
  else acquire = "passively acquiring";
  
  if(myGroup->getMovement()) move = "movement on";
  else move = "movement off";

  if(myGroup->getBlob()) blob = "blob in sight";
  else blob = "no blob in sight";

  printf("\r Channel: %d  %15s %25s %20s", myChannel, move, acquire, blob);
}

// The channels
void ArModeActs::channel1(void)
{
  myGroup->setChannel(1);
}

void ArModeActs::channel2(void)
{
  myGroup->setChannel(2);
}

void ArModeActs::channel3(void)
{
  myGroup->setChannel(3);
}

void ArModeActs::channel4(void)
{
  myGroup->setChannel(4);
}

void ArModeActs::channel5(void)
{
  myGroup->setChannel(5);
}

void ArModeActs::channel6(void)
{
  myGroup->setChannel(6);
}

void ArModeActs::channel7(void)
{
  myGroup->setChannel(7);
}

void ArModeActs::channel8(void)
{
  myGroup->setChannel(8);
}

// Stop the robot from moving
void ArModeActs::stop(void)
{
  myGroup->stopMovement();
}

// Allow the robot to move
void ArModeActs::start(void)
{
  myGroup->startMovement();
}

// Toggle whether or not the robot is allowed
// to aquire anything
void ArModeActs::toggleAcquire()
{
  if(myGroup->getAcquire())
    myGroup->setAcquire(false);
  else myGroup->setAcquire(true);

}

ArModeCommand::ArModeCommand(ArRobot *robot, const char *name, char key, char key2): 
  ArMode(robot, name, key, key2),
  my0CB(this, &ArModeCommand::addChar, '0'),
  my1CB(this, &ArModeCommand::addChar, '1'),
  my2CB(this, &ArModeCommand::addChar, '2'),
  my3CB(this, &ArModeCommand::addChar, '3'),
  my4CB(this, &ArModeCommand::addChar, '4'),
  my5CB(this, &ArModeCommand::addChar, '5'),
  my6CB(this, &ArModeCommand::addChar, '6'),
  my7CB(this, &ArModeCommand::addChar, '7'),
  my8CB(this, &ArModeCommand::addChar, '8'),
  my9CB(this, &ArModeCommand::addChar, '9'),
  myMinusCB(this, &ArModeCommand::addChar, '-'),
  myBackspaceCB(this, &ArModeCommand::addChar, ArKeyHandler::BACKSPACE),
  mySpaceCB(this, &ArModeCommand::addChar, ArKeyHandler::SPACE),
  myEnterCB(this, &ArModeCommand::finishParsing)

{
  reset(false);
}

ArModeCommand::~ArModeCommand()
{
  
}

void ArModeCommand::activate(void)
{
  reset(false);
  if (!baseActivate())
    return;
  myRobot->stopStateReflection();
  takeKeys();
  reset(true);
}

void ArModeCommand::deactivate(void)
{
  if (!baseDeactivate())
    return;
  giveUpKeys();
}

void ArModeCommand::help(void)
{
  
  ArLog::log(ArLog::Terse, "Command mode has three ways to send commands");
  ArLog::log(ArLog::Terse, "%-30s: Sends com(<command>)", "<command>");
  ArLog::log(ArLog::Terse, "%-30s: Sends comInt(<command>, <integer>)", "<command> <integer>");
  ArLog::log(ArLog::Terse, "%-30s: Sends com2Bytes(<command>, <byte1>, <byte2>)", "<command> <byte1> <byte2>");
}

void ArModeCommand::addChar(int ch)
{
  if (ch < '0' && ch > '9' && ch != '-' && ch != ArKeyHandler::BACKSPACE && 
      ch != ArKeyHandler::SPACE)
  {
    ArLog::log(ArLog::Terse, "Something horribly wrong in command mode since number is < 0 || > 9 (it is the value %d)", ch);
    return;
  }

  size_t size = sizeof(myCommandString);
  size_t len = strlen(myCommandString);

  if (ch == ArKeyHandler::BACKSPACE)
  {
    // don't overrun backwards
    if (len < 1)
      return;
    myCommandString[len-1] = '\0';
    printf("\r> %s  \r> %s", myCommandString, myCommandString);
    return;
  }
  if (ch == ArKeyHandler::SPACE)
  {
    // if we're at the start or have a space or - just return
    if (len < 1 || myCommandString[len-1] == ' ' || 
        myCommandString[len-1] == '-')
      return;
    myCommandString[len] = ' ';
    myCommandString[len+1] = '\0';
    printf(" ");
    return;
  }
  if (ch == '-')
  {
    // make sure it isn't the command trying to be negated or that its the start of the byte
    if (len < 1 || myCommandString[len-1] != ' ')
      return;
    printf("%c", '-');
    myCommandString[len] = '-';
    myCommandString[len+1] = '\0';
    return;
  }
  if (len + 1 >= size)
  {
    printf("\n");
    ArLog::log(ArLog::Terse, "Command is too long, abandoning command");
    reset();
    return;
  }
  else
  {
    printf("%c", ch);
    myCommandString[len] = ch;
    myCommandString[len+1] = '\0';
    return;
  }
}

void ArModeCommand::finishParsing(void)
{
  
  ArArgumentBuilder builder;
  builder.add(myCommandString);
  int command;
  int int1;
  int int2;

  if (myCommandString[0] == '\0')
    return;

  printf("\n");
  if (builder.getArgc() == 0)
  {
    ArLog::log(ArLog::Terse, "Syntax error, no arguments.");
  }
  if (builder.getArgc() == 1)
  {
    command = builder.getArgInt(0);
    if (command < 0 || command > 255 || !builder.isArgInt(0))
    {
      ArLog::log(ArLog::Terse, 
                 "Invalid command, must be an integer between 0 and 255");
      reset();
      return;
    }
    else
    {
      ArLog::log(ArLog::Terse, "com(%d)", command);
      myRobot->com(command);
      reset();
      return;
    }
  }
  else if (builder.getArgc() == 2)
  {
    command = builder.getArgInt(0);
    int1 = builder.getArgInt(1);
    if (command < 0 || command > 255 || !builder.isArgInt(0))
    {
      ArLog::log(ArLog::Terse, 
                 "Invalid command, must be an integer between 0 and 255");
      reset();
      return;
    }
    else if (int1 < -32767 || int1 > 32767 || !builder.isArgInt(1))
    {
      ArLog::log(ArLog::Terse, 
         "Invalid integer, must be an integer between -32767 and 32767");
      reset();
      return;
    }
    else
    {
      ArLog::log(ArLog::Terse, "comInt(%d, %d)", command,
                 int1);
      myRobot->comInt(command, int1);
      reset();
      return;
    }
  }
  else if (builder.getArgc() == 3)
  {
    command = builder.getArgInt(0);
    int1 = builder.getArgInt(1);
    int2 = builder.getArgInt(2);
    if (command < 0 || command > 255 || !builder.isArgInt(0))
    {
      ArLog::log(ArLog::Terse, 
                 "Invalid command, must be between 0 and 255");
      reset();
      return;
    }
    else if (int1 < -128 || int1 > 255 || !builder.isArgInt(1))
    {
      ArLog::log(ArLog::Terse, 
         "Invalid byte1, must be an integer between -128 and 127, or between 0 and 255");
      reset();
      return;
    }
    else if (int2 < -128 || int2 > 255 || !builder.isArgInt(2))
    {
      ArLog::log(ArLog::Terse, 
         "Invalid byte2, must be an integer between -128 and 127, or between 0 and 255");
      reset();
      return;
    }
    else
    {
      ArLog::log(ArLog::Terse, 
                 "com2Bytes(%d, %d, %d)", 
                 command, int1, int2);
      myRobot->com2Bytes(command, int1, int2);
      reset();
      return;
    }
  }
  else
  {
    ArLog::log(ArLog::Terse, "Syntax error, too many arguments");
    reset();
    return;
  }
}

void ArModeCommand::reset(bool print)
{
  myCommandString[0] = '\0';
  if (print)
  {
    ArLog::log(ArLog::Terse, "");
    printf("> ");
  }
}

void ArModeCommand::takeKeys(void)
{
  addKeyHandler('0', &my0CB);
  addKeyHandler('1', &my1CB);
  addKeyHandler('2', &my2CB);
  addKeyHandler('3', &my3CB);
  addKeyHandler('4', &my4CB);
  addKeyHandler('5', &my5CB);
  addKeyHandler('6', &my6CB);
  addKeyHandler('7', &my7CB);
  addKeyHandler('8', &my8CB);
  addKeyHandler('9', &my9CB);
  addKeyHandler('-', &myMinusCB);
  addKeyHandler(ArKeyHandler::BACKSPACE, &myBackspaceCB);
  addKeyHandler(ArKeyHandler::ENTER, &myEnterCB);
  addKeyHandler(ArKeyHandler::SPACE, &mySpaceCB);
}

void ArModeCommand::giveUpKeys(void)
{
  remKeyHandler(&my0CB);
  remKeyHandler(&my1CB);
  remKeyHandler(&my2CB);
  remKeyHandler(&my3CB);
  remKeyHandler(&my4CB);
  remKeyHandler(&my5CB);
  remKeyHandler(&my6CB);
  remKeyHandler(&my7CB);
  remKeyHandler(&my8CB);
  remKeyHandler(&my9CB);
  remKeyHandler(&myBackspaceCB);
  remKeyHandler(&myMinusCB);
  remKeyHandler(&myEnterCB);
  remKeyHandler(&mySpaceCB);
}

ArModeTCM2::ArModeTCM2(ArRobot *robot, const char *name, char key, char key2, ArTCM2 *tcm2): 
  ArMode(robot, name, key, key2)
{
  if (tcm2 != NULL)
    myTCM2 = tcm2;
  else
    myTCM2 = new ArTCM2(robot);
  myOffCB = new ArFunctorC<ArTCM2>(myTCM2, &ArTCM2::commandOff);
  myCompassCB = new ArFunctorC<ArTCM2>(myTCM2, &ArTCM2::commandJustCompass);
  myOnePacketCB = new ArFunctorC<ArTCM2>(myTCM2, &ArTCM2::commandOnePacket);
  myContinuousPacketsCB = new ArFunctorC<ArTCM2>(
          myTCM2, &ArTCM2::commandContinuousPackets);
  myUserCalibrationCB = new ArFunctorC<ArTCM2>(
          myTCM2, &ArTCM2::commandUserCalibration);
  myAutoCalibrationCB = new ArFunctorC<ArTCM2>(
          myTCM2, &ArTCM2::commandAutoCalibration);
  myStopCalibrationCB = new ArFunctorC<ArTCM2>(
          myTCM2, &ArTCM2::commandStopCalibration);
  myResetCB = new ArFunctorC<ArTCM2>(
          myTCM2, &ArTCM2::commandSoftReset);

}

ArModeTCM2::~ArModeTCM2()
{
  
}

void ArModeTCM2::activate(void)
{
  if (!baseActivate())
    return;
  myTCM2->commandContinuousPackets();
  addKeyHandler('0', myOffCB);
  addKeyHandler('1', myCompassCB);
  addKeyHandler('2', myOnePacketCB);
  addKeyHandler('3', myContinuousPacketsCB);
  addKeyHandler('4', myUserCalibrationCB);
  addKeyHandler('5', myAutoCalibrationCB);
  addKeyHandler('6', myStopCalibrationCB);
  addKeyHandler('7', myResetCB);
}

void ArModeTCM2::deactivate(void)
{
  if (!baseDeactivate())
    return;
  myTCM2->commandJustCompass();
  remKeyHandler(myOffCB);
  remKeyHandler(myCompassCB);
  remKeyHandler(myOnePacketCB);
  remKeyHandler(myContinuousPacketsCB);
  remKeyHandler(myUserCalibrationCB);
  remKeyHandler(myAutoCalibrationCB);
  remKeyHandler(myStopCalibrationCB);
  remKeyHandler(myResetCB);
}

void ArModeTCM2::help(void)
{
  ArLog::log(ArLog::Terse, 
             "TCM2 mode shows the data from the TCM2 compass and lets you send the TCM2 commands");
  ArLog::log(ArLog::Terse, "%20s:  turn TCM2 off", "'0'");  
  ArLog::log(ArLog::Terse, "%20s:  just get compass readings", "'1'");  
  ArLog::log(ArLog::Terse, "%20s:  get a single set of TCM2 data", "'2'");  
  ArLog::log(ArLog::Terse, "%20s:  get continuous TCM2 data", "'3'");  
  ArLog::log(ArLog::Terse, "%20s:  start user calibration", "'4'");  
  ArLog::log(ArLog::Terse, "%20s:  start auto calibration", "'5'");  
  ArLog::log(ArLog::Terse, "%20s:  stop calibration and get a single set of data", "'6'");  
  ArLog::log(ArLog::Terse, "%20s:  soft reset of compass", "'7'");  

  printf("%6s %5s %5s %6s %6s %6s %6s %10s %4s %4s %6s %3s\n", 
         "comp", "pitch", "roll", "magX", "magY", "magZ", "temp", "error",
         "calH", "calV", "calM", "cnt");
}

void ArModeTCM2::userTask(void)
{
  printf("\r%6.1f %5.1f %5.1f %6.2f %6.2f %6.2f %6.1f 0x%08x %4.0f %4.0f %6.2f %3d", 
         myTCM2->getCompass(), myTCM2->getPitch(), myTCM2->getRoll(), 
         myTCM2->getXMagnetic(), myTCM2->getYMagnetic(), 
         myTCM2->getZMagnetic(), 
         myTCM2->getTemperature(), myTCM2->getError(), 
         myTCM2->getCalibrationH(), myTCM2->getCalibrationV(), 
         myTCM2->getCalibrationM(), myTCM2->getPacCount());

}

---