Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Tools/RobotConfiguration.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file RobotConfiguration.cpp
    00003 *
    00004 * Implementation of class RobotConfiguration.
    00005 *
    00006 * @author Martin Lötzsch
    00007 */
    00008
    00009 #include "RobotConfiguration.h"
    00010 #include "Platform/SystemCall.h"
    00011 #include "Tools/Debugging/Debugging.h"
    00012 #include "Tools/Debugging/GenericDebugData.h"
    00013 #include "Tools/Streams/InStreams.h"
    00014
    00015 #ifndef NO_PROCESS_FRAMEWORK
    00016 #include "Platform/ProcessFramework.h"
    00017 #endif
    00018
    00019 #ifdef NO_PROCESS_FRAMEWORK
    00020 #define GT_GLOBAL
    00021 #endif
    00022
    00023 /**
    00024 * The default robot configuration is a hack for RobotControl.
    00025 */
    00026 RobotConfiguration defaultConfiguration;
    00027
    00028 GT_GLOBAL RobotConfiguration* theRobotConfiguration = &defaultConfiguration;
    00029
    00030 void setRobotConfiguration(RobotConfiguration* configuration)
    00031 {
    00032   theRobotConfiguration = configuration;
    00033 }
    00034
    00035 RobotConfiguration& getRobotConfiguration()
    00036 {
    00037   return *theRobotConfiguration;
    00038 }
    00039
    00040 RobotConfiguration::RobotConfiguration()
    00041 : robotDimensions(RobotDimensionsERS210())
    00042 {
    00043 }
    00044
    00045 void RobotConfiguration::load()
    00046 {
    00047   robotDesign = SystemCall::getRobotDesign();
    00048   if(robotDesign == ERS210)
    00049   {
    00050     RobotDimensionsERS210 ers210;
    00051     memcpy(&robotDimensions, &ers210, sizeof(RobotDimensions));
    00052   }
    00053   else
    00054   {
    00055     RobotDimensionsERS7 ers7;
    00056     memcpy(&robotDimensions, &ers7, sizeof(RobotDimensions));
    00057   }
    00058   unsigned char address[6];
    00059   SystemCall::getMacAddress(address);
    00060   sprintf(macAddressString, "%02X%02X%02X%02X%02X%02X", int(address[0]), int(address[1]),
    00061           int(address[2]), int(address[3]), int(address[4]), int(address[5]));
    00062   InConfigFile file("robot.cfg",macAddressString);
    00063   if(file.exists() && !file.eof())
    00064   {
    00065     char ignore[50];
    00066     file >> ignore >> robotCalibration.bodyTiltOffset
    00067          >> ignore >> robotCalibration.bodyRollOffset
    00068          >> ignore >> robotCalibration.headTiltOffset
    00069          >> ignore >> robotCalibration.headRollOffset
    00070          >> ignore >> robotCalibration.tiltFactor
    00071          >> ignore >> robotCalibration.panFactor
    00072          >> ignore >> robotCalibration.tilt2Factor;
    00073     if(!robotCalibration.tiltFactor)
    00074       robotCalibration.tiltFactor = 1;
    00075     if(!robotCalibration.panFactor)
    00076       robotCalibration.panFactor = 1;
    00077     if(!robotCalibration.tilt2Factor)
    00078       robotCalibration.tilt2Factor = 1;
    00079   }
    00080 }
    00081
    00082 bool RobotConfiguration::handleMessage(InMessage& message)
    00083 {
    00084   switch(message.getMessageID())
    00085   {
    00086     case idBodyOffsets:
    00087     {
    00088       GenericDebugData d;
    00089       message.bin >> d;
    00090       robotCalibration.bodyTiltOffset = d.data[0];
    00091       robotCalibration.bodyRollOffset = d.data[1];
    00092       robotCalibration.headTiltOffset = d.data[2];
    00093       robotCalibration.headRollOffset = d.data[3];
    00094       robotCalibration.tiltFactor = d.data[4];
    00095       robotCalibration.panFactor = d.data[5];
    00096       robotCalibration.tilt2Factor = d.data[6];
    00097       if(!robotCalibration.tiltFactor)
    00098         robotCalibration.tiltFactor = 1;
    00099       if(!robotCalibration.panFactor)
    00100         robotCalibration.panFactor = 1;
    00101       if(!robotCalibration.tilt2Factor)
    00102         robotCalibration.tilt2Factor = 1;
    00103       OUTPUT(idText, text, "new body offsets: " << robotCalibration.bodyTiltOffset
    00104         <<", "<<robotCalibration.bodyRollOffset
    00105         <<", "<<robotCalibration.headTiltOffset
    00106         <<", "<<robotCalibration.headRollOffset
    00107         <<", "<<robotCalibration.tiltFactor
    00108         <<", "<<robotCalibration.panFactor
    00109         <<", "<<robotCalibration.tilt2Factor);
    00110       return true;
    00111     }
    00112   }
    00113   return false;
    00114 }
    00115
    00116 In& operator>>(In& stream,RobotConfiguration& robotConfiguration)
    00117 {
    00118   stream.read(&robotConfiguration,sizeof(RobotConfiguration));
    00119   return stream;
    00120 }
    00121
    00122 Out& operator<<(Out& stream, const RobotConfiguration& robotConfiguration)
    00123 {
    00124   stream.write(&robotConfiguration,sizeof(RobotConfiguration));
    00125   return stream;
    00126 }
    00127