ArRobotConfigPacketReader.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 "ariaOSDef.h"
#include "ArExport.h"
#include "ArRobotConfigPacketReader.h"
#include "ArRobot.h"
#include "ArRobotPacket.h"
#include "ArCommands.h"

AREXPORT ArRobotConfigPacketReader::ArRobotConfigPacketReader(
    ArRobot *robot, bool onlyOneRequest, ArFunctor *packetArrivedCB) : 
  myPacketHandlerCB(this, &ArRobotConfigPacketReader::packetHandler),
  myConnectedCB(this, &ArRobotConfigPacketReader::connected)
{
  myRobot = robot;
  myPacketHandlerCB.setName("ArRobotConfigPacketReader");
  myRobot->addPacketHandler(&myPacketHandlerCB);
  myRobot->addConnectCB(&myConnectedCB);
  myOnlyOneRequest = onlyOneRequest;
  myPacketRequested = false;
  myPacketArrived = false;
  myPacketArrivedCB = packetArrivedCB;
}

AREXPORT ArRobotConfigPacketReader::~ArRobotConfigPacketReader(void)
{
  myRobot->remPacketHandler(&myPacketHandlerCB);
  myRobot->remConnectCB(&myConnectedCB);
}

AREXPORT bool ArRobotConfigPacketReader::requestPacket(void)
{
  // make sure we haven't already gotten one
  if (myOnlyOneRequest && myPacketArrived)
    return false;

  if (myPacketRequested && myLastPacketRequest.mSecSince() < 200)
    return true;

  myPacketArrived = false;
  myPacketRequested = true;
  myLastPacketRequest.setToNow();
  myRobot->comInt(ArCommands::CONFIG, 1);
  return true;
}

AREXPORT void ArRobotConfigPacketReader::connected(void)
{
  if (myPacketRequested)
    myRobot->comInt(ArCommands::CONFIG, 1);
}

AREXPORT bool ArRobotConfigPacketReader::packetHandler(ArRobotPacket *packet)
{
  char buf[256];

  // if this isn't the right packet ignore it
  if (packet->getID() != 0x20)
    return false;
  // if we've already gotten our one request ignore it
  if (myPacketArrived)
    return false;
  // if we haven't requested a packet ignore it
  if (!myPacketRequested)
    return false;

  myPacketRequested = false;
  myPacketArrived = true;
  // read in all the data
  packet->bufToStr(buf, sizeof(buf));
  myType = buf;
  packet->bufToStr(buf, sizeof(buf));
  mySubType = buf;
  packet->bufToStr(buf, sizeof(buf));
  mySerialNumber = buf;
  packet->bufToUByte();
  myRotVelTop = packet->bufToUByte2();
  myTransVelTop = packet->bufToUByte2();
  myRotAccelTop = packet->bufToUByte2();
  myTransAccelTop = packet->bufToUByte2();
  myPwmMax = packet->bufToUByte2();
  packet->bufToStr(buf, sizeof(buf));
  myName = buf;
  mySipCycleTime = packet->bufToUByte();
  myHostBaud = packet->bufToUByte();
  myAux1Baud = packet->bufToUByte();
  myHasGripper = (bool)packet->bufToUByte2();
  myFrontSonar = (bool) packet->bufToUByte2();
  myRearSonar = (bool) packet->bufToUByte();
  myLowBattery = packet->bufToUByte2();
  myRevCount = packet->bufToUByte2();
  myWatchdog = packet->bufToUByte2();
  myNormalMPacs = (bool) packet->bufToUByte();
  myStallVal = packet->bufToUByte2();
  myStallCount = packet->bufToUByte2();
  myJoyVel = packet->bufToUByte2();
  myJoyRotVel = packet->bufToUByte2();
  myRotVelMax = packet->bufToUByte2();
  myTransVelMax = packet->bufToUByte2();
  myRotAccel = packet->bufToUByte2();
  myRotDecel = packet->bufToUByte2();
  myRotKP = packet->bufToUByte2();
  myRotKV = packet->bufToUByte2();
  myRotKI = packet->bufToUByte2();
  myTransAccel = packet->bufToUByte2();
  myTransDecel = packet->bufToUByte2();
  myTransKP = packet->bufToUByte2();
  myTransKV = packet->bufToUByte2();
  myTransKI = packet->bufToUByte2();
  myFrontBumps = packet->bufToUByte();
  myRearBumps = packet->bufToUByte();
  myHasCharger = packet->bufToUByte();
  mySonarCycle = packet->bufToUByte();
  if (packet->bufToUByte() == 2)
    myResetBaud = true;
  else
    myResetBaud = false;
  myGyroType = packet->bufToUByte();
  if (myGyroType == 1)
    myHasGyro = true;
  else
    myHasGyro = false;
  myDriftFactor = packet->bufToUByte2();
  myAux2Baud = packet->bufToUByte();
  myAux3Baud = packet->bufToUByte();
  myTicksMM = packet->bufToUByte2();
  myShutdownVoltage = packet->bufToUByte2();

  if (myPacketArrivedCB != NULL)
  {
    myPacketArrivedCB->invoke();
  }
  return true;
}
  
AREXPORT void ArRobotConfigPacketReader::log(void) const
{
  std::string str;
  str = buildString();
  ArLog::log(ArLog::Terse, str.c_str());
}

AREXPORT std::string ArRobotConfigPacketReader::buildString(void) const
{
  std::string ret;

  char line[32000];
  sprintf(line, "General information:\n");
  ret += line;
  sprintf(line, "Robot is type '%s' subtype '%s'\n",
         getType(), getSubType());
  ret += line;
  sprintf(line, "serial number '%s' name '%s'\n", 
         getSerialNumber(), getName());
  ret += line;
  sprintf(line, "Intrinsic properties and unsettable maxes:\n");
  ret += line;
  sprintf(line, "RotVelTop %d RotAccelTop %d\n", 
         getRotVelTop(), getRotAccelTop());
  ret += line;
  sprintf(line, "TransVelTop %d TransAccelTop %d\n", 
         getTransVelTop(), getTransAccelTop());
  ret += line;
  sprintf(line, "PWMMax %d ResetBaud %s\n", getPwmMax(),
         ArUtil::convertBool(getResetBaud()));
  ret += line;
  sprintf(line, "Current values:\n");
  ret += line;
  sprintf(line, "RotVelMax %d RotAccel %d RotDecel %d\n", 
         getRotVelMax(), getRotAccel(), getRotDecel());
  ret += line;
  sprintf(line, "TransVelMax %d TransAccel %d TransDecel %d\n", 
         getTransVelMax(), getTransAccel(), getTransDecel());
  ret += line;  
  sprintf(line, "Accessories:\n");
  ret += line;  
  sprintf(line, 
         "Gripper %s FrontSonar %s RearSonar %s Charger %d Gyro %s\n", 
         ArUtil::convertBool(getHasGripper()), 
         ArUtil::convertBool(getFrontSonar()), 
         ArUtil::convertBool(getRearSonar()), 
         getHasCharger(),
         ArUtil::convertBool(getHasGyro()));
  ret += line;  
  sprintf(line, "FrontBumps %d RearBumps %d\n", 
         getFrontBumps(), getRearBumps());
  ret += line;  
  sprintf(line, "Settings:\n");
  ret += line;  
  sprintf(line, "SipCycle %d SonarCycle %d HostBaud %d Aux1Baud %d\n", getSipCycleTime(), getSonarCycle(), getHostBaud(), getAux1Baud());
  ret += line;  
  sprintf(line, "StallVal %d StallCount %d RevCount %d Watchdog %d\n",
         getStallVal(), getStallCount(), getRevCount(), getWatchdog());
  ret += line;  
  sprintf(line, "JoyVel %d JoyRVel %d NormalMotorPackets %s\n", getJoyVel(), getJoyRotVel(), ArUtil::convertBool(getNormalMPacs()));
  ret += line;  
  sprintf(line, "PID Settings:\n");
  ret += line;  
  sprintf(line, "Rot kp %d kv %d ki %d\n", getRotKP(), getRotKV(),
         getRotKI());
  ret += line;  
  sprintf(line, "Trans kp %d kv %d ki %d\n", getTransKP(), 
         getTransKV(), getTransKI());
  ret += line;  
  sprintf(line, "DriftFactor %d\n", getDriftFactor());
  ret += line;  
  sprintf(line, "Aux2Baud setting %d, Aux3Baud setting %d\n", getAux2Baud(), getAux3Baud());
  ret += line;  
  sprintf(line, "TicksMM: %d\n", getTicksMM());
  ret += line;  
  sprintf(line, "Shutdown Voltage: %d\n", getShutdownVoltage());
  ret += line;  

  return ret;
}

---