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

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

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

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;
}

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

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;
  if (packet->bufToUByte() == 1)
    myHasGyro = true;
  else
    myHasGyro = false;
  myDriftFactor = packet->bufToUByte2();
  myAux2Baud = packet->bufToUByte();
  myAux3Baud = packet->bufToUByte();
  myTicksMM = packet->bufToUByte2();
  myShutdownVoltage = packet->bufToUByte2();
  return true;
}
  
void ArRobotConfigPacketReader::log(void) const
{
  ArLog::log(ArLog::Terse, "General information:");
  ArLog::log(ArLog::Terse, "Robot is type '%s' subtype '%s'",
             getType(), getSubType());
  ArLog::log(ArLog::Terse, "serial number '%s' name '%s'", 
             getSerialNumber(), getName());
  ArLog::log(ArLog::Terse, "Intrinsic properties and unsettable maxes:");
  ArLog::log(ArLog::Terse, "RotVelTop %d RotAccelTop %d", 
             getRotVelTop(), getRotAccelTop());
  ArLog::log(ArLog::Terse, "TransVelTop %d TransAccelTop %d", 
             getTransVelTop(), getTransAccelTop());
  ArLog::log(ArLog::Terse, "PWMMax %d ResetBaud %s", getPwmMax(),
             ArUtil::convertBool(getResetBaud()));
  ArLog::log(ArLog::Terse, "Current values:");
  ArLog::log(ArLog::Terse, "RotVelMax %d RotAccel %d RotDecel %d", 
             getRotVelMax(), getRotAccel(), getRotDecel());
  ArLog::log(ArLog::Terse, "TransVelMax %d TransAccel %d TransDecel %d", 
             getTransVelMax(), getTransAccel(), getTransDecel());
  
  ArLog::log(ArLog::Terse, "Accessories:");
  ArLog::log(ArLog::Terse, 
             "Gripper %s FrontSonar %s RearSonar %s Charger %d Gyro %s", 
             ArUtil::convertBool(getHasGripper()), 
             ArUtil::convertBool(getFrontSonar()), 
             ArUtil::convertBool(getRearSonar()), 
             getHasCharger(),
             ArUtil::convertBool(getHasGyro()));
  ArLog::log(ArLog::Terse, "FrontBumps %d RearBumps %d", 
             getFrontBumps(), getRearBumps());
  ArLog::log(ArLog::Terse, "Settings:");
  ArLog::log(ArLog::Terse, "SipCycle %d SonarCycle %d HostBaud %d Aux1Baud %d ", getSipCycleTime(), getSonarCycle(), getHostBaud(), getAux1Baud());
  ArLog::log(ArLog::Terse, "StallVal %d StallCount %d RevCount %d Watchdog %d ",
             getStallVal(), getStallCount(), getRevCount(), getWatchdog());
  
  ArLog::log(ArLog::Terse, "JoyVel %d JoyRVel %d NormalMotorPackets %s ", getJoyVel(), getJoyRotVel(), ArUtil::convertBool(getNormalMPacs()));
  ArLog::log(ArLog::Terse, "PID Settings:");
  ArLog::log(ArLog::Terse, "Rot kp %d kv %d ki %d", getRotKP(), getRotKV(),
             getRotKI());
  ArLog::log(ArLog::Terse, "Trans kp %d kv %d ki %d", getTransKP(), 
             getTransKV(), getTransKI());
  ArLog::log(ArLog::Terse, "DriftFactor %d", getDriftFactor());
  ArLog::log(ArLog::Terse, "Aux2Baud setting %d, Aux3Baud setting %d", getAux2Baud(), getAux3Baud());
  ArLog::log(ArLog::Terse, "TicksMM: %d", getTicksMM());
  ArLog::log(ArLog::Terse, "Shutdown Voltage: %d", getShutdownVoltage());
}

---