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"

#ifndef NO_PROCESS_FRAMEWORK
#include "Platform/ProcessFramework.h"
#endif

#ifdef NO_PROCESS_FRAMEWORK
#define GT_GLOBAL 
#endif

/**
* The default robot configuration is a hack for RobotControl.
*/
RobotConfiguration defaultConfiguration;

GT_GLOBAL RobotConfiguration* theRobotConfiguration = &defaultConfiguration;

void setRobotConfiguration(RobotConfiguration* configuration)
{
  theRobotConfiguration = configuration;
}

RobotConfiguration& getRobotConfiguration()
{
  return *theRobotConfiguration;
}

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;
      OUTPUT(idText, text, "new body offsets: " << robotCalibration.bodyTiltOffset
        <<", "<<robotCalibration.bodyRollOffset
        <<", "<<robotCalibration.headTiltOffset
        <<", "<<robotCalibration.headRollOffset
        <<", "<<robotCalibration.tiltFactor
        <<", "<<robotCalibration.panFactor
        <<", "<<robotCalibration.tilt2Factor);
      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;
}

---