ArSonyPTZ.cpp

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

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

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

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

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

#include "ArExport.h"
#include "ariaOSDef.h"
#include "ArSonyPTZ.h"
#include "ArRobot.h"
#include "ArCommands.h"

AREXPORT ArSonyPacket::ArSonyPacket(ArTypes::UByte2 bufferSize) :
  ArBasePacket(bufferSize)
{
  
}

AREXPORT ArSonyPacket::~ArSonyPacket()
{

}

AREXPORT void ArSonyPacket::uByteToBuf(ArTypes::UByte val)
{
  if (myLength + 1 > myMaxLength)
  {
    ArLog::log(ArLog::Terse, "ArSonyPacket::uByteToBuf: Trying to add beyond length of buffer.");
    return;
  }
  myBuf[myLength] = val;
  ++myLength;
}

AREXPORT void ArSonyPacket::byte2ToBuf(ArTypes::Byte2 val)
{
  if ((myLength + 4) > myMaxLength)
  {
    ArLog::log(ArLog::Terse, "ArSonyPacket::Byte2ToBuf: Trying to add beyond length of buffer.");
    return;
  }
  myBuf[myLength] = (val & 0xf000) >> 12;
  ++myLength;
  myBuf[myLength] = (val & 0x0f00) >> 8;
  ++myLength;
  myBuf[myLength] = (val & 0x00f0) >> 4;
  ++myLength;
  myBuf[myLength] = (val & 0x000f) >> 0;
  ++myLength;
}

AREXPORT void ArSonyPacket::byte2ToBufAtPos(ArTypes::Byte2 val,
                        ArTypes::UByte2 pose)
{
  ArTypes::Byte2 prevLength = myLength;

  if ((pose + 4) > myMaxLength)
  {
    ArLog::log(ArLog::Terse, "ArSonyPacket::Byte2ToBuf: Trying to add beyond length of buffer.");
    return;
  }
  myLength = pose;
  byte2ToBuf(val);
  myLength = prevLength;
}


AREXPORT ArSonyPTZ::ArSonyPTZ(ArRobot *robot) :
  ArPTZ(robot),
  myPacket(255), 
  myZoomPacket(9)
{
  myRobot = robot;
  initializePackets();
  
  myDegToTilt = 0x12c / ((double) MAX_TILT);
  myDegToPan = 0x370 / ((double) MAX_PAN);
}

AREXPORT ArSonyPTZ::~ArSonyPTZ()
{
}

void ArSonyPTZ::initializePackets(void)
{
  myZoomPacket.empty();
  myZoomPacket.uByteToBuf(0x81);
  myZoomPacket.uByteToBuf(0x01);
  myZoomPacket.uByteToBuf(0x04);
  myZoomPacket.uByteToBuf(0x47);
  myZoomPacket.uByteToBuf(0x00);
  myZoomPacket.uByteToBuf(0x00);
  myZoomPacket.uByteToBuf(0x00);
  myZoomPacket.uByteToBuf(0x00);
  myZoomPacket.uByteToBuf(0xff);

  myPanTiltPacket.empty();
  myPanTiltPacket.uByteToBuf(0x81);
  myPanTiltPacket.uByteToBuf(0x01);
  myPanTiltPacket.uByteToBuf(0x06);
  myPanTiltPacket.uByteToBuf(0x02);
  myPanTiltPacket.uByteToBuf(0x18);
  myPanTiltPacket.uByteToBuf(0x14);
  myPanTiltPacket.uByteToBuf(0x00);
  myPanTiltPacket.uByteToBuf(0x00);
  myPanTiltPacket.uByteToBuf(0x00);
  myPanTiltPacket.uByteToBuf(0x00);
  myPanTiltPacket.uByteToBuf(0x00);
  myPanTiltPacket.uByteToBuf(0x00);
  myPanTiltPacket.uByteToBuf(0x00);
  myPanTiltPacket.uByteToBuf(0x00);
  myPanTiltPacket.uByteToBuf(0xff);
}


AREXPORT bool ArSonyPTZ::init(void)
{
  myPacket.empty();
  myPacket.uByteToBuf(0x88);
  myPacket.uByteToBuf(0x01);
  myPacket.uByteToBuf(0x00);
  myPacket.uByteToBuf(0x01);
  myPacket.uByteToBuf(0xff);
  myPacket.uByteToBuf(0x88);
  myPacket.uByteToBuf(0x30);
  myPacket.uByteToBuf(0x01);
  myPacket.uByteToBuf(0xff);

  if (!sendPacket(&myPacket))
    return false;
  if (!panTilt(0, 0))
    return false;
  if (!zoom(0))
    return false;
  return true;
}

AREXPORT bool ArSonyPTZ::backLightingOn(void)
{
  myPacket.empty();
  myPacket.uByteToBuf(0x81);
  myPacket.uByteToBuf(0x01);
  myPacket.uByteToBuf(0x04);
  myPacket.uByteToBuf(0x33);
  myPacket.uByteToBuf(0x02);
  myPacket.uByteToBuf(0xff);

  return sendPacket(&myPacket);
}

AREXPORT bool ArSonyPTZ::backLightingOff(void)
{
  myPacket.empty();
  myPacket.uByteToBuf(0x81);
  myPacket.uByteToBuf(0x01);
  myPacket.uByteToBuf(0x04);
  myPacket.uByteToBuf(0x33);
  myPacket.uByteToBuf(0x03);
  myPacket.uByteToBuf(0xff);

  return sendPacket(&myPacket);
}

AREXPORT bool ArSonyPTZ::panTilt(double degreesPan, double degreesTilt)
{
  if (degreesPan > MAX_PAN)
    degreesPan = MAX_PAN;
  if (degreesPan < -MAX_PAN)
    degreesPan = -MAX_PAN;
  myPan = degreesPan;

  if (degreesTilt > MAX_TILT)
    degreesTilt = MAX_TILT;
  if (degreesTilt < -MAX_TILT)
    degreesTilt = -MAX_TILT;
  myTilt = degreesTilt;

  myPanTiltPacket.byte2ToBufAtPos(ArMath::roundInt(myPan * myDegToPan), 6);
  myPanTiltPacket.byte2ToBufAtPos(ArMath::roundInt(myTilt * myDegToTilt), 10);
  return sendPacket(&myPanTiltPacket);
}

AREXPORT bool ArSonyPTZ::panTiltRel(double degreesPan, double degreesTilt)
{
  return panTilt(myPan + degreesPan, myTilt + degreesTilt);
}

AREXPORT bool ArSonyPTZ::pan(double degrees)
{
  return panTilt(degrees, myTilt);
}

AREXPORT bool ArSonyPTZ::panRel(double degrees)
{
  return panTiltRel(degrees, 0);
}

AREXPORT bool ArSonyPTZ::tilt(double degrees)
{
  return panTilt(myPan, degrees);
}

AREXPORT bool ArSonyPTZ::tiltRel(double degrees)
{
  return panTiltRel(0, degrees);
}

AREXPORT bool ArSonyPTZ::zoom(int zoomValue)
{
  if (zoomValue > MAX_ZOOM)
    zoomValue = MAX_ZOOM;
  if (zoomValue < MIN_ZOOM)
    zoomValue = MIN_ZOOM;
  myZoom = zoomValue;
    
  myZoomPacket.byte2ToBufAtPos(ArMath::roundInt(myZoom), 4);
  return sendPacket(&myZoomPacket);
}

AREXPORT bool ArSonyPTZ::zoomRel(int zoomValue)
{
  return zoom(myZoom + zoomValue);
}


/*
AREXPORT bool ArSonyPTZ::packetHandler(ArRobotPacket *packet)
{
  if (packet->getID() != 0xE0)
    return false;

  return true;
}
*/

---