FormControl.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
//
// $Id$
//
// Copyright 2008, 2009, 2010, 2011  Antonio Franchi and Antonio Franchi    
//
// This file is part of MIP.
//
// MIP 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 3 of the License, or
// (at your option) any later version.
//
// MIP 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 MIP. If not, see <http://www.gnu.org/licenses/>.
//
// Contact info: antonio.franchi@tuebingen.mpg.de stegagno@diag.uniroma1.it
//
// ----------------------------------------------------------------------------





#ifndef __FORM_CONTROL_TASK_H_
#define __FORM_CONTROL_TASK_H_

#include <Task.h>
#include <Resource.h>
#include <Uav.h>
#include <UavControl.h>
#include <ProtrusionExtr.h>
#include <SharedMem.h>
#include <CommonOptions.h>

#include <CommModule.h>

#ifndef MIP_PLATFORM_ARM
 #include <DraWin.h>
#endif


namespace MipTasks{
 
 /* @{ */
 
 enum FormControlTaskStates{
  FORM_CONTROL_FIRST_RUN,
  FORM_CONTROL_NORMAL_RUN,
   FORM_CONTROL_STATES_NUM
 };
 
 static const char* FormControlTaskStatesNames[FORM_CONTROL_STATES_NUM] = {
  "FirstRun",
  "NormalRun"
 };
 
 
 class FormControlTaskOptions : public Options {
  public:
   DecimalOption  *commPeriod;
   
   FormControlTaskOptions();

   string getObjectName() const {
    return "FormControlTaskOptions";
   }
 };
 
 
 class FormControlTaskSharedMem : public SharedMem{
  public:
   void initSharedMem();
 };
 
 
 class FormControlTask : public Task{
  private:
   
   static const TaskPlate _plate = FCT_TASK;
   
   static const long int  _maxDurSec  = 0,      _maxDurUsec = 100000;
   static const long int  _maxSamplPeriodSec = 0, _maxSamplPeriodUsec = 100000;
   static const long int  _minSamplPeriodSec = 0, _minSamplPeriodUsec = 5000;
   
   UavControlPar* _controlPar;
   UavControl* _uavControl;
   
   Uav*  _uav;
   
   CommModule* _commModule;
   
 #ifndef MIP_PLATFORM_ARM
   DraWin*      _draWin;   
   unsigned int _drawList; 
   bool         dwOn;      
 #endif
 
   FormControlTaskSharedMem* _sharedMem;
   
   FormControlTaskOptions _options;
   
   // Define here the local variables of your task.
   UavVar* _uavState;
   FormControlTaskStates _state;
   int _myId;
   
   stringstream _ssdebug;
   
   Timer _commSendTimer;
   Time _commSendPeriod;
   Timer _commRecTimer;
   Time _commRecPeriod;
   Time _maxPacketLifeTime;
   
   // associative array indexed by the robots id and containing the poses and
   // the ang vel of all the robot in communication range
   // maps are necesary because of multiple packets can be received  
   map<int,Position3D>      _receivedPositions;  
   map<int,Position3D>::iterator _itP; 
   map<int,Position3D>      _receivedVelocities; 
   map<int,Position3D>::iterator _itA; 
   map<int,Time>      _receivedTimeStamps;
   map<int,Time>::iterator _itT; 
   
   vector<Position3D> _otherRobotPosi; 
   vector<Position3D>  _otherRobotVel;  
   vector<int>     _otherRobotIds;  
   
   // Define here the private methods of your task.
   void _firstRun();
   void _normalRun();
   
   void _sendCommPackets();
   void _receiveCommPackets();
   bool _getOtherRobotPosi();
   
   
  public:
   FormControlTask(ResourcePointers resources,int argc, const char* argv[]);
   
   ~FormControlTask();
   
   TaskPlate getPlate() const {
    return _plate;
   }
   Time getMaxDuration(){
    return Time(_maxDurSec,_maxDurUsec);
   }
   Time getMaxSamplPeriod(){
    return Time(_maxSamplPeriodSec,_maxSamplPeriodUsec);
   }
   Time getMinSamplPeriod(){
    return Time(_minSamplPeriodSec,_minSamplPeriodUsec);
   }
   
   TaskOutputs run();
 };
 
 /* @} */
 
};// end namespace MipTasks

#endif