Tools/RobotConfiguration.cpp

Go to the documentation of this file.

/**
* @file RobotConfiguration.cpp
*
* Implementation of class RobotConfiguration.
* 
* @author Martin Lötzsch
*/

#include "RobotConfiguration.h"
#include "Platform/SystemCall.h"
#include "Tools/Debugging/Debugging.h"
#include "Tools/Debugging/GenericDebugData.h"
#include "Tools/Streams/InStreams.h"
#include "Platform/ProcessFramework.h"

static RobotConfiguration theRobotConfiguration[ROBOT_MAX];

RobotConfiguration& getRobotConfiguration()
{
  return theRobotConfiguration[PlatformProcess::getRobotIndex()];
}

RobotConfiguration::RobotConfiguration()
: robotDimensions(RobotDimensionsERS210())
{
}

void RobotConfiguration::load()
{
  robotDesign = SystemCall::getRobotDesign();
  if(robotDesign == ERS210)
  {
    RobotDimensionsERS210 ers210;
    memcpy(&robotDimensions, &ers210, sizeof(RobotDimensions));
  }
  else
  {
    RobotDimensionsERS7 ers7;
    memcpy(&robotDimensions, &ers7, sizeof(RobotDimensions));
  }
  unsigned char address[6];
  SystemCall::getMacAddress(address);
  sprintf(macAddressString, "%02X%02X%02X%02X%02X%02X", int(address[0]), int(address[1]), 
          int(address[2]), int(address[3]), int(address[4]), int(address[5]));
  InConfigFile file("robot.cfg",macAddressString);
  if(file.exists() && !file.eof())
  {
    char ignore[50];
    file >> ignore >> robotCalibration.bodyTiltOffset
         >> ignore >> robotCalibration.bodyRollOffset
         >> ignore >> robotCalibration.headTiltOffset
         >> ignore >> robotCalibration.headRollOffset
         >> ignore >> robotCalibration.tiltFactor
         >> ignore >> robotCalibration.panFactor
         >> ignore >> robotCalibration.tilt2Factor;
    if(!robotCalibration.tiltFactor)
      robotCalibration.tiltFactor = 1;
    if(!robotCalibration.panFactor)
      robotCalibration.panFactor = 1;
    if(!robotCalibration.tilt2Factor)
      robotCalibration.tilt2Factor = 1;
  }
}

bool RobotConfiguration::handleMessage(InMessage& message)
{
  switch(message.getMessageID())
  {
    case idBodyOffsets:
    {
      GenericDebugData d;
      message.bin >> d;
      robotCalibration.bodyTiltOffset = d.data[0];
      robotCalibration.bodyRollOffset = d.data[1];
      robotCalibration.headTiltOffset = d.data[2];
      robotCalibration.headRollOffset = d.data[3];
      robotCalibration.tiltFactor = d.data[4];
      robotCalibration.panFactor = d.data[5];
      robotCalibration.tilt2Factor = d.data[6];
      if(!robotCalibration.tiltFactor)
        robotCalibration.tiltFactor = 1;
      if(!robotCalibration.panFactor)
        robotCalibration.panFactor = 1;
      if(!robotCalibration.tilt2Factor)
        robotCalibration.tilt2Factor = 1;
      return true;
    }
  }
  return false;
}

In& operator>>(In& stream,RobotConfiguration& robotConfiguration)
{
  stream.read(&robotConfiguration,sizeof(RobotConfiguration));
  return stream;
}
 
Out& operator<<(Out& stream, const RobotConfiguration& robotConfiguration)
{
  stream.write(&robotConfiguration,sizeof(RobotConfiguration));
  return stream;
}


/*
* Change Log:
*
* $Log: RobotConfiguration.cpp,v $
* Revision 1.1.1.1  2004/05/22 17:35:55  cvsadm
* created new repository GT2004_WM
*
* Revision 1.10  2004/03/12 21:59:49  roefer
* Better default values for unspecified values in generic debug request
*
* Revision 1.9  2004/03/08 02:32:20  roefer
* Calibration parameters changed
*
* Revision 1.8  2004/02/29 14:56:03  roefer
* Additional calibration parameters
*
* Revision 1.7  2004/02/24 19:01:06  roefer
* Additional calibration parameters added
*
* Revision 1.6  2004/01/23 00:13:24  roefer
* ERS-7 simulation, first draft
*
* Revision 1.5  2004/01/15 23:20:17  roefer
* RobotDesign detection in simulator
*
* Revision 1.4  2004/01/05 14:47:02  roefer
* Type problem corrected
*
* Revision 1.3  2004/01/01 10:58:52  roefer
* RobotDimensions are in a class now
*
* Revision 1.2  2003/12/31 12:21:25  roefer
* getRobotDesign added
*
* Revision 1.1  2003/10/07 10:13:21  cvsadm
* Created GT2004 (M.J.)
*
* Revision 1.3  2003/09/26 11:40:40  juengel
* - sorted tools
* - clean-up in DataTypes
*
* Revision 1.2  2003/07/05 09:49:05  roefer
* Generic debug message for bodyOffsets improved
*
* Revision 1.1.1.1  2003/07/02 09:40:28  cvsadm
* created new repository for the competitions in Padova from the 
* tamara CVS (Tuesday 2:00 pm)
*
* removed unused solutions
*
* Revision 1.3  2003/05/08 14:55:03  juengel
* Added getMacAddressString().
*
* Revision 1.2  2003/05/05 08:52:17  roefer
* OUTPUT removed
*
* Revision 1.1  2003/05/03 16:20:01  roefer
* robot.cfg added
*
*
*/

---