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

# Modules/BehaviorControl/GT2005BehaviorControl/GT2005BasicBehaviors/GT2005WalkCalibrationBasicBehaviors.h

Go to the documentation of this file.

    00001 #ifndef __GT2005WalkCalibrationBasicBehaviors_h_
    00002 #define __GT2005WalkCalibrationBasicBehaviors_h_
    00003
    00004 #include "../../BehaviorControl.h"
    00005 #include "Tools/Xabsl2/Xabsl2Engine/Xabsl2Engine.h"
    00006 #include "Tools/Math/PIDsmoothedValue.h"
    00007
    00008 class GT2005WalkCalibrationMainBehavior : public Xabsl2BasicBehavior, public BehaviorControlInterfaces
    00009 {
    00010 private:
    00011
    00012   Pose2D calculateStartPose(const Pose2D& base);
    00013   static double minDistanceToBorder(Vector2<double>& t);
    00014
    00015   double initializeParameter;
    00016   double rotationParameter;
    00017
    00018   Pose2D startPose;
    00019   double startPoseAngle;
    00020   bool startPoseCalculated;
    00021   int startPoseBadCount;
    00022   double initializationTime;
    00023   int measureCount;
    00024   char filename[20];
    00025
    00026   int qCount;
    00027   double qSum;
    00028
    00029   double result;
    00030   enum ResultCodes {
    00031     resultUnready = 0,
    00032     resultReady = 1,
    00033     resultMeasure = 2,
    00034     resultReposition = 3,
    00035     resultInteraction = 4
    00036   };
    00037
    00038   void Initialize();
    00039   void Measure();
    00040
    00041
    00042   double measureStartTime, measureStopTime, tempTime;
    00043   double startOffset, measureDuration, stopOffset;
    00044   Vector2<double> translationStartPoint, translationStopPoint, test;
    00045   double rotationStartAngle, rotationStopAngle, rotationTemp;
    00046
    00047   double measureStartTime0, measureStopTime0;
    00048   Vector2<double> translationStartPoint0, translationStopPoint0;
    00049   double rotationStartAngle0, rotationStopAngle0;
    00050
    00051   double walkingDirection;
    00052   int currentTable, currentPoint, currentRotation;
    00053   int robotColor, robotNumber;
    00054
    00055   typedef struct
    00056   {
    00057     Pose2D controlled;
    00058     Pose2D measured;
    00059   } OdometryPoint;
    00060
    00061   static const int measureSize = 195; //195 for normal plane callibartion 21 for rotation only callibration
    00062
    00063   void removeExtreme(bool *pBadIndex, bool removeMaximum);
    00064
    00065
    00066   OdometryPoint measurePoints[measureSize];
    00067   int tempPoseCount;
    00068   Pose2D tempPose[50];
    00069   double tempTimeStart, tempTimeStop;
    00070
    00071   Pose2D camRobotPose;
    00072   double camTimestamp;
    00073   int robotNotDetectedCount;
    00074
    00075
    00076   /** center of the camera */
    00077   Vector2<double> center;
    00078
    00079
    00080   enum State
    00081   {
    00082     stateNotInitialized,
    00083     stateMeasureStart,
    00084     stateMeasure,
    00085     stateMeasureFinished,
    00086     stateCalibrationFinished
    00087   };
    00088
    00089   State state;
    00090
    00091   enum MeasureStatus
    00092   {
    00093     before,
    00094     measuring,
    00095     end
    00096   };
    00097
    00098   MeasureStatus measureStatus;
    00099
    00100
    00101
    00102
    00103
    00104 public:
    00105
    00106   GT2005WalkCalibrationMainBehavior(BehaviorControlInterfaces& interfaces, Xabsl2ErrorHandler& errorHandler);
    00107
    00108   void registerSymbols(Xabsl2Engine& engine);
    00109
    00110
    00111   /**  Executes the basic behavior. */
    00112   virtual void execute();
    00113
    00114 };
    00115
    00116
    00117
    00118 class GT2005WalkCalibrationBasicBehaviors : public BehaviorControlInterfaces
    00119 {
    00120 public:
    00121 /**
    00122 * Constructor
    00123   */
    00124
    00125   GT2005WalkCalibrationBasicBehaviors(BehaviorControlInterfaces& interfaces,
    00126     Xabsl2ErrorHandler& errorHandler)
    00127     : BehaviorControlInterfaces(interfaces),
    00128     errorHandler(errorHandler),
    00129     gt2005WalkCalibrationMainBehavior(interfaces, errorHandler)
    00130    {}
    00131
    00132   /** Registers basic behaviors at the engine */
    00133   void registerBasicBehaviors(Xabsl2Engine& engine);
    00134
    00135 private:
    00136   /** Is invoked when errors occurs */
    00137   Xabsl2ErrorHandler& errorHandler;
    00138
    00139   GT2005WalkCalibrationMainBehavior gt2005WalkCalibrationMainBehavior;
    00140
    00141   //!@}
    00142 };
    00143
    00144
    00145
    00146 #endif // __GT2005BasicWalkCalibrationBasicBehaviors_h_
    00147