ArRobotPacketSender.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 "ArDeviceConnection.h"
#include "ArRobotPacketSender.h"

AREXPORT ArRobotPacketSender::ArRobotPacketSender(unsigned char sync1,
                          unsigned char sync2) :
  myPacket(sync1, sync2)
{
  myDeviceConn = NULL;
}

AREXPORT ArRobotPacketSender::ArRobotPacketSender(
    ArDeviceConnection *deviceConnection, unsigned char sync1,
    unsigned char sync2) :
  myPacket(sync1, sync2)
{
  myDeviceConn = deviceConnection;
}

AREXPORT ArRobotPacketSender::~ArRobotPacketSender()
{

}

AREXPORT void ArRobotPacketSender::setDeviceConnection(
    ArDeviceConnection *deviceConnection)
{
  myDeviceConn = deviceConnection;
}

AREXPORT ArDeviceConnection *ArRobotPacketSender::getDeviceConnection(void)
{
  return myDeviceConn;
}

bool ArRobotPacketSender::connValid(void)
{
  return (myDeviceConn != NULL && 
      myDeviceConn->getStatus() == ArDeviceConnection::STATUS_OPEN);
}

AREXPORT bool ArRobotPacketSender::com(unsigned char number)
{
  if (!connValid())
    return false;

  myPacket.empty();
  myPacket.setID(number);

  myPacket.finalizePacket();
  
  return myDeviceConn->write(myPacket.getBuf(), myPacket.getLength());
}

AREXPORT bool ArRobotPacketSender::comInt(unsigned char command, 
                      short int argument)
{

  if (!connValid())
    return false;

  myPacket.empty();
  myPacket.setID(command);
  if (argument >= 0) 
  {
    myPacket.uByteToBuf(INTARG);
  }
  else 
  {
    myPacket.uByteToBuf(NINTARG);
    argument = -argument;
  }
  myPacket.uByte2ToBuf(argument);

  myPacket.finalizePacket();

  if (myDeviceConn->write(myPacket.getBuf(), myPacket.getLength()) >= 0)
    return true;

  return false;

}

AREXPORT bool ArRobotPacketSender::com2Bytes(unsigned char command, char high,
                         char low)
{
  return comInt(command, ((high & 0xff)<<8) + (low & 0xff));
}

AREXPORT bool ArRobotPacketSender::comStr(unsigned char command, 
                      const char *argument)
{
  size_t size;
  if (!connValid())
    return false;
  size = strlen(argument);
  if (size > 199) // 200 - 1 byte for length
    return false;

  myPacket.empty();
  
  myPacket.setID(command);
  myPacket.uByteToBuf(STRARG);
  myPacket.uByteToBuf(size);
  myPacket.strToBuf(argument);
  
  myPacket.finalizePacket();

  if (myDeviceConn->write(myPacket.getBuf(), myPacket.getLength()) >= 0)
    return true;

  return false;
  
}

AREXPORT bool ArRobotPacketSender::comStrN(unsigned char command, 
                       const char *str, int size)
{
  if (!connValid())
    return false;

  if(size > 199) return false;   // 200 - 1 byte for length

  myPacket.empty();
  
  myPacket.setID(command);
  myPacket.uByteToBuf(STRARG);

  myPacket.uByteToBuf(size);
  myPacket.strNToBuf(str, size);
  
  myPacket.finalizePacket();

  if (myDeviceConn->write(myPacket.getBuf(), myPacket.getLength()) >= 0)
    return true;

  return false;
  
}

AREXPORT bool ArRobotPacketSender::comDataN(unsigned char command, const char* data, int size)
{
  if(!connValid()) return false;
  if(size > 200) return false;
  myPacket.empty();
  myPacket.setID(command);
  myPacket.uByteToBuf(STRARG);
  myPacket.strNToBuf(data, size);
  myPacket.finalizePacket();
  if(myDeviceConn->write(myPacket.getBuf(), myPacket.getLength()) >= 0)
      return true;
  return false;
}

---