ArVCC4.h

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

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

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

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

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

#ifndef ARVCC4_H
#define ARVCC4_H

#include "ariaTypedefs.h"
#include "ArBasePacket.h"
#include "ArPTZ.h"
#include "ariaUtil.h"
#include "ArCommands.h"
#include "ArSerialConnection.h"

// maximum number of bytes expected for a response from the camera
#define MAX_RESPONSE_BYTES 14

// the state timeout when using bidirectional communication
// This is big because it may have to wait for a power on or
// power off command to complete, which take ~4 seconds.
#define BIDIRECTIONAL_TIMEOUT 5000

// The number of ms to wait for a timeout for unidirectional communication.
// This is how long the usertask will wait before assuming that the camera
// has processed the last command.
#define UNIDIRECTIONAL_TIMEOUT 300

// how often to request position information from the camera if using
// bidirectional communication (in ms)
#define AUTO_UPDATE_TIME 2000

// accuracy of camera movements.  This sets how different the current 
// position and the desired position must be in order for a command to be
// sent to the camera.
#define TOLERANCE .1


class ArVCC4Commands
{
public:
  enum Command {
    DELIM = 0x00, 
    DEVICEID = 0x30, 
    PANSLEW = 0x50, 
    TILTSLEW = 0x51, 
    STOP = 0x53, 
    INIT = 0x58, 
    SLEWREQ = 0x59, 
    ANGLEREQ = 0x5c, 
    PANTILT = 0x62, 
    SETRANGE = 0x64, 
    PANTILTREQ = 0x63, 
    INFRARED = 0x76, 
    PRODUCTNAME = 0x87, 
    LEDCONTROL = 0x8E, 
    CONTROL = 0x90, 
    POWER = 0xA0, 
    AUTOFOCUS = 0xA1, 
    ZOOMSTOP = 0xA2, 
    GAIN = 0xA5, 
    FOCUS = 0xB0, 
    ZOOM = 0xB3, 
    ZOOMREQ = 0xB4, 
    IRCUTFILTER = 0xB5, 
    DIGITALZOOM = 0xB7, 
    FOOTER = 0xEF, 
    RESPONSE = 0xFE, 
    HEADER = 0xFF 
  };

};


class ArVCC4Packet: public ArBasePacket
{
public:
  AREXPORT ArVCC4Packet(ArTypes::UByte2 bufferSize = 30);
  AREXPORT virtual ~ArVCC4Packet();

  AREXPORT virtual void byte2ToBuf(ArTypes::Byte4 val);

  AREXPORT virtual void finalizePacket(void);

protected:
};

class ArVCC4 : public ArPTZ
{
public:
  // the states for communication
  enum CommState {
    COMM_UNKNOWN,
    COMM_BIDIRECTIONAL,
    COMM_UNIDIRECTIONAL
  };

  enum CameraType {
    CAMERA_VCC4,
    CAMERA_C50I
  };

  AREXPORT ArVCC4(ArRobot *robot, bool inverted = false, CommState commDirection = COMM_UNKNOWN, bool autoUpdate = true, bool disableLED = false, CameraType cameraType = CAMERA_VCC4);
  AREXPORT virtual ~ArVCC4();

  AREXPORT virtual bool power(bool state) { myPowerStateDesired = state; return true; }
  AREXPORT bool getPower(void) { return myPowerState; }
  AREXPORT virtual bool init(void) { myInitRequested = true; return true; }
  AREXPORT virtual void reset(void) { ArPTZ::reset(); init(); }

  AREXPORT bool isInitted(void) { return myCameraIsInitted; }
  AREXPORT virtual void connectHandler(void);
  AREXPORT virtual bool packetHandler(ArBasePacket *packet);

  AREXPORT virtual bool pan(double deg) { return panTilt(deg, myTiltDesired); }
  AREXPORT virtual bool panRel(double deg) { return panTilt(myPanDesired + deg, myTiltDesired); }
  AREXPORT virtual bool tilt(double deg) { return panTilt(myPanDesired, deg); }
  AREXPORT virtual bool tiltRel(double deg) { return panTilt(myPanDesired, myTiltDesired + deg); }
  AREXPORT virtual bool panTiltRel(double pdeg, double tdeg) { return panTilt(myPanDesired + pdeg, myTiltDesired + tdeg); }

  AREXPORT virtual double getMaxPosPan(void) const 
    { if (myInverted) return invert(MIN_PAN); else return MAX_PAN; }
  AREXPORT virtual double getMaxNegPan(void) const 
    { if (myInverted) return invert(MAX_PAN); else return MIN_PAN; }
  AREXPORT virtual double getMaxPosTilt(void) const 
    { if (myInverted) return invert(MIN_TILT); else return MAX_TILT; }
  AREXPORT virtual double getMaxNegTilt(void) const
    { if (myInverted) return invert(MAX_TILT); else return MIN_TILT; }

  AREXPORT void getRealPanTilt(void) { myRealPanTiltRequested = true; }

  AREXPORT void getRealZoomPos(void) { myRealZoomRequested = true; }

  AREXPORT virtual bool canZoom(void) const { return true; }

  AREXPORT virtual bool panTilt(double pdeg, double tdeg);
  AREXPORT virtual bool zoom(int deg);
  AREXPORT bool digitalZoom(int deg);

  AREXPORT void addErrorCB(ArFunctor *functor, ArListPos::Pos position);

  AREXPORT void remErrorCB(ArFunctor *functor);

  AREXPORT bool haltPanTilt(void) { myHaltPanTiltRequested = true; return true; }
  AREXPORT bool haltZoom(void) { myHaltZoomRequested = true; return true; }

  AREXPORT bool panSlew(double deg) { myPanSlewDesired = deg; return true; }
  AREXPORT bool tiltSlew(double deg) { myTiltSlewDesired = deg; return true; }

  AREXPORT void preparePacket(ArVCC4Packet *packet);

  AREXPORT virtual double getPan(void) const { return myPanDesired; }
  AREXPORT virtual double getTilt(void) const { return myTiltDesired; }
  AREXPORT virtual int getZoom(void) const { return myZoomDesired; }
  AREXPORT double getDigitalZoom(void) const { return myDigitalZoomDesired; }

  AREXPORT virtual bool canGetRealPanTilt(void) const { return true; }
  AREXPORT virtual bool canGetRealZoom(void) const { return true; }
  AREXPORT virtual bool canSetFocus(void) const { return false; }
  AREXPORT virtual bool autoFocus(void) { myFocusModeDesired = 0; return true;}
  AREXPORT virtual bool focusNear(void) { myFocusModeDesired = 2; return true;}
  AREXPORT virtual bool focusFar(void) { myFocusModeDesired = 3; return true; }

  AREXPORT double getPanSlew(void) { return myPanSlewDesired; }
  AREXPORT double getMaxPanSlew(void) { return MAX_PAN_SLEW; }
  AREXPORT double getMinPanSlew(void) { return MIN_PAN_SLEW; }

  AREXPORT double getTiltSlew(void) { return myTiltSlewDesired; }
  AREXPORT double getMaxTiltSlew(void) { return MAX_TILT_SLEW; }
  AREXPORT double getMinTiltSlew(void) { return MIN_TILT_SLEW; }

  AREXPORT virtual int getMaxZoom(void) const;
  AREXPORT virtual int getMinZoom(void) const { return MIN_ZOOM; }

  AREXPORT virtual bool canGetFOV(void) { return true; }
  AREXPORT virtual double getFOVAtMaxZoom(void) { return myFOVAtMaxZoom; }
  AREXPORT virtual double getFOVAtMinZoom(void) { return myFOVAtMinZoom; }


  AREXPORT bool wasError(void) { return myWasError; }

  AREXPORT void enableAutoUpdate(void) { myAutoUpdate = true; }
  AREXPORT void disableAutoUpdate(void) { myAutoUpdate = false; }
  AREXPORT bool getAutoUpdate(void) { return myAutoUpdate; }

  AREXPORT void setLEDControlMode(int controlMode) { myDesiredLEDControlMode = controlMode; }
  AREXPORT void enableIRLEDs(void) { myDesiredIRLEDsMode = true; }
  AREXPORT void disableIRLEDs(void) { myDesiredIRLEDsMode = false; }
  AREXPORT bool getIRLEDsEnabled(void) { return myIRLEDsEnabled; }
  AREXPORT void enableIRFilterMode(void) { myDesiredIRFilterMode = true; }
  AREXPORT void disableIRFilterMode(void) { myDesiredIRFilterMode = false; }
  AREXPORT bool getIRFilterModeEnabled (void) { return myIRFilterModeEnabled; }
protected:

  // preset limits on movements.  Based on empirical data
  enum Param {
    MAX_PAN = 98,       // 875 units is max pan assignment
    MIN_PAN = -98,      // -875 units is min pan assignment
    MAX_TILT = 88,      // 790 units is max tilt assignment
    MIN_TILT = -30,     // -267 units is min tilt assignment
    MAX_PAN_SLEW = 90,      // 800 positions per sec (PPS)
    MIN_PAN_SLEW = 1,       // 8 positions per sec (PPS)
    MAX_TILT_SLEW = 69,     // 662 positions per sec (PPS)
    MIN_TILT_SLEW = 1,      // 8 positions per sec (PPS)
    MAX_ZOOM_OPTIC = 1960,
    MIN_ZOOM = 0
  };

  // the various error states that the camera can return
  enum Error {
    CAM_ERROR_NONE = 0x30, 
    CAM_ERROR_BUSY = 0x31, 
    CAM_ERROR_PARAM = 0x35, 
    CAM_ERROR_MODE = 0x39,  
    CAM_ERROR_UNKNOWN = 0xFF 
 };

  // the states of the FSM
  enum State {
    UNINITIALIZED,
    STATE_UNKNOWN,
    INITIALIZING,
    SETTING_CONTROL_MODE,
    SETTING_INIT_TILT_RATE,
    SETTING_INIT_PAN_RATE,
    SETTING_INIT_RANGE,
    POWERING_ON,
    POWERING_OFF,
    POWERED_OFF,
    POWERED_ON,
    AWAITING_INITIAL_POWERON,
    AWAITING_INITIAL_INIT,
    AWAITING_ZOOM_RESPONSE,
    AWAITING_PAN_TILT_RESPONSE,
    AWAITING_STOP_PAN_TILT_RESPONSE,
    AWAITING_STOP_ZOOM_RESPONSE,
    AWAITING_PAN_SLEW_RESPONSE,
    AWAITING_TILT_SLEW_RESPONSE,
    AWAITING_POS_REQUEST,
    AWAITING_ZOOM_REQUEST,
    AWAITING_LED_CONTROL_RESPONSE,
    AWAITING_IRLEDS_RESPONSE,
    AWAITING_IRFILTER_RESPONSE,
    AWAITING_PRODUCTNAME_REQUEST,
    AWAITING_DIGITAL_ZOOM_RESPONSE,
    AWAITING_FOCUS_RESPONSE,
    STATE_DELAYED_SWITCH,
    STATE_ERROR
  };

  // flips the sign if needed
  double invert(double before) const
    { if (myInverted) return -before; else return before; }
  bool myInverted;

  // true if there was an error during the last cycle
  bool myWasError;

  // the camera name.  "C50i" for C50i, and "VC-C" for VC-C4
  std::string myProductName;

  ArRobot *myRobot;
  ArDeviceConnection *myConn;
  ArBasePacket *newPacket;
  ArVCC4Packet myPacket;

  // timers for watching for timeouts
  ArTime myStateTime;
  ArTime myPacketTime;
  ArTime myIdleTime;

  // gets set to true if using an aux port vs computer serial port
  bool myUsingAuxPort;

  // delay variable, if delaying before switching to the next state
  int myStateDelayTime;

  // what type of communication the camera is using
  CommState myCommType;

  // used to read data if the camera is attached directly to a computer
  virtual ArBasePacket* readPacket(void);

  // the functor to add as a usertask
  ArFunctorC<ArVCC4> myTaskCB;

  // the actual task to be added as a usertask
  void camTask(void);

  // true when a response has been received from the camera, but has
  // not yet been acted on by the state machine
  bool myResponseReceived;

  bool myWaitingOnStop;
  bool myWaitingOnPacket;

  // the state of the state machine
  State myState;
  State myPreviousState;
  State myNextState;

  // used to switch between states in the state machine
  void switchState(State state, int delayTime = 0);

  // the max time before a state times out, and the time for a packet response
  // to timeout.  The difference being that a packet reponse can be received
  // immediately, but it could say that the camera is busy, meaning the state
  // has not yet completed
  int myStateTimeout;
  int myPacketTimeout;

  // request a packet from the microcontroller of size num bytes.
  // most camera responses are 6 bytes, so just use the default
  void requestBytes(int num = 6);

  // the buffer to store the incoming packet data in
  unsigned char myPacketBuf[50];
  int myPacketBufLen;

  // how many bytes we're still expecting to receive from the controller
  int myBytesLeft;

  // these all send commands to the camera.
  bool sendPanTilt(void);
  bool sendZoom(void);
  bool sendPanSlew(void);
  bool sendTiltSlew(void);
  bool sendPower(void);
  bool sendHaltPanTilt(void);
  bool sendHaltZoom(void);
  bool sendRealPanTiltRequest(void);
  bool sendRealZoomRequest(void);
  bool sendDigitalZoom(void);
  bool sendFocus(void);
  
  // this is currently not used because it doesn't work right
  bool sendProductNameRequest(void);

  // the camera type is used to specify VC-C4 vs. C50i
  CameraType myCameraType;
  bool myRequestProductName;

  bool sendLEDControlMode(void);
  bool sendCameraNameRequest(void);
  int myDesiredLEDControlMode;

  bool sendIRFilterControl(void);
  bool sendIRLEDControl(void);
  bool myIRLEDsEnabled;
  bool myDesiredIRLEDsMode;
  bool myIRFilterModeEnabled;
  bool myDesiredIRFilterMode;

  // These should only be used by the state machine to initialize the 
  // camera for the first time
  bool setDefaultRange(void);
  bool setControlMode(void);
  bool sendInit(void);

  // process the packet data for a camera response that has accurate
  // pan/tilt positional information in it, and the product name
  void processGetPanTiltResponse(void);
  void processGetZoomResponse(void);
  void processGetProductNameResponse(void);

  // true if autoupdating of camera's position should be used  
  bool myAutoUpdate;

  // cycle for stepping through various autoupdate resquests from the camera
  int myAutoUpdateCycle;

  // returns true if there is no reponse to a packet within the timeout
  // or also if the state times out.  The argument will overrid the default
  // timeout periods
  bool timeout(int mSec = 0);

  // internal reperesenstation of pan, tilt, and zoom positions
  double myPan;
  double myTilt;
  int myZoom;
  int myDigitalZoom;
  int myFocusMode;

  // used to store the returned positional values when requesting the true
  // position from the camera
  double myPanResponse;
  double myTiltResponse;
  int myZoomResponse;

  // the returned product name
  char myProductNameResponse[4];

  // the positions that were last sent to the camera.  These are needed
  // because the desired positions can change between time a command is
  // sent and before it succeeds.
  double myPanSent;
  double myTiltSent;
  int myZoomSent;
  double myPanSlewSent;
  double myTiltSlewSent;

  // internal representation of pan and tilt slew
  double myPanSlew;
  double myTiltSlew;

  // where the user has requested the camera move to
  double myPanDesired;
  double myTiltDesired;
  int myZoomDesired;
  int myDigitalZoomDesired;
  int myFocusModeDesired;

  // the pan an tilt slew that the user requested
  double myPanSlewDesired;
  double myTiltSlewDesired;

  // internal mirror of camera power state, and whether it's be initted
  bool myPowerState;
  bool myCameraIsInitted;

  // whether the user wants the camera on or off, or initialized
  bool myPowerStateDesired;
  bool myInitRequested;

  // whether the user has requested to halt movement
  bool myHaltZoomRequested;
  bool myHaltPanTiltRequested;

  // whether the camera has been initialized since instance inception
  bool myCameraHasBeenInitted;

  // true if the user has requested to update the camera's postion
  // from the data returned from the camera
  bool myRealPanTiltRequested;
  bool myRealZoomRequested;

  // the error state from the last packet received
  unsigned int myError;

  // our FOV numbers (these should change if we use the digital zoom)
  double myFOVAtMaxZoom;
  double myFOVAtMinZoom;

  // run through the list or error callbacks
  void throwError();

  // the list of error callbacks to step through when a error occurs
  std::list<ArFunctor *> myErrorCBList;
};

#endif // ARVCC4_H

---