LineFollowing.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
//
// $Id$
//
// Copyright 2008, 2009, 2010, 2011, 2012  Antonio Franchi and Paolo Stegagno    
//
// 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 __LINE_FOLLOWING_H_
#define __LINE_FOLLOWING_H_


#ifdef MIP_HOST_APPLE
#include <applePatch.h>
#endif

#include <Task.h>
#include <MobileRob.h>
#include <MobileRob3D.h>
// #include <PositionDetector3D.h>
#include <Keyboard.h>
#ifndef MIP_PLATFORM_ARM
#include <DraWin.h>
#endif
#include <string.h>
#include <math.h>
#include <CommonOptions.h>
#include <SharedMem.h>
#include <FirFilter.h>

using namespace MipResources;

// #define LINVEL_STEP 0.02
// #define ANGVEL_STEP 0.05
namespace MipTasks{
 
 /* @{ */
 
 enum lineFollowingControls{
  LINE_FOLLOWING_CTRL_LINEAR,
  LINE_FOLLOWING_CTRL_ADAPTIVE,
  LINE_FOLLOWING_CTRL_NUM
 };

 static const char * lineFollowingControlNames[LINE_FOLLOWING_CTRL_NUM] ={
 "linear",
 "adaptive"
 };
 
 
 class LineFollowingOptions : public Options {
  static const Decimal khIIIblackLineValue;
  static const Decimal khIIIbwSharpBorderSlope; // line slope for Graduated Line
 //  static const Decimal khIIIbwSharpBorderSlope = 666666.666666; //line slope for black line
  
  public:
   IntOption* cmdType;
   DecimalOption* lineSlope;
   DecimalOption* colorValue;
   DecimalOption* gain;
   DecimalOption* cruiseSpeed;
   DecimalOption* cruiseControlPower;
   DecimalOption* timeFilter;
   DecimalOption* markUpGain;
   IntOption* fileFlag;
   IntOption* controlType;
   StringOption *fileName;
   DecimalOption* maxPolSpeed;
   DecimalOption* minCurvature;  
   DecimalOption* maxCurvature;
   DecimalOption* initSpeed;
   DecimalOption* minSpeed;
   DecimalOption* initTime;
   DecimalOption* irToAxisDistance;
   DecimalOption* maxEstimatedDistance;

   string getObjectName() const {
    return "LineFollowingOptions";
   }
 
   LineFollowingOptions(){
    cmdType       = createIntOption("lineFollowingCmdType",  "se 0 comanda veloGoal, se 1 comanda velo", 0);
    lineSlope      = createDecimalOption("lineFollowingLineSlope",  "Characteristic slope of the borders of the line to follow.", khIIIbwSharpBorderSlope);
    colorValue     = createDecimalOption("lineFollowingColorValue", "Value returned by the sensor when it is put on the line.", khIIIblackLineValue);
    gain         = createDecimalOption("lineFollowingGain", "Controller gain only for Linear Control.", 200.0);
    cruiseSpeed     = createDecimalOption("lineFollowingCruiseSpeed", "Cruise Speed.", 0.1);
    cruiseControlPower = createDecimalOption("lineFollowingControlPower", "ControlPower.", 4.0);
    timeFilter      = createDecimalOption("lineFollowingTimeFilter", "Time Filter.", 1.0);
    fileFlag       = createIntOption("lineFollowingFileFlag", "File Flag.", 0);
    markUpGain      = createDecimalOption("lineFollowingMarkUpGain", "Mark-Up Gain.", 30);
    controlType     = createIntOption("lineFollowingControlType", "Control Type.", 0);
    fileName      = createStringOption("lineFollowingFileName","","lineFollLog.dat");
    maxPolSpeed     = createDecimalOption("lineFollowingMaxPolSpeed","MaxPolSpeed",0.06);
    minCurvature    = createDecimalOption("lineFollowingMinCurvature","MinCurvature",2);
    maxCurvature    = createDecimalOption("lineFollowingMaxCurvature","MinCurvature",16);
    initSpeed      = createDecimalOption("lineFollowingInitSpeed","InitSpeed",0.02);
    minSpeed      = createDecimalOption("lineFollowingMinSpeed","MinSpeed",0.01);
    initTime      = createDecimalOption("lineFollowingInitTime","InitTime",6);
    irToAxisDistance  = createDecimalOption("lineFollowingIrToAxisDistance","irToAxisDistance",0.05784);
    maxEstimatedDistance= createDecimalOption("lineFollowingMaxEstimatedDistance","maxEstimatedDistance",0.006);
    
    updateValues();
   }
   
 };
 
 
 class LineFollowingSharedMem : public SharedMem{
  public:
   void initSharedMem(){
   }
   
   void togglePrintPose(){
    _printPose = !_printPose;
   }
   void togglePrintScan(){
    _printScan = !_printScan;
   }
   void togglePrintFeat(){
    _printFeat = !_printFeat;
   }
   void togglePrintFormation(){
    _printFormation = !_printFormation;
   }
   void togglePrintFormationDetails(){
    _printFormationDetails = !_printFormationDetails;
   }
   void togglePrintTarget(){
    _printTarget = !_printTarget;
   }
   void toggleDoMloc(){
    _doMloc = !_doMloc;
   }
   void toggleDoAutoNavigation(){
    _doAutoNavigation = !_doAutoNavigation;
   }
   
   
   
   bool printPose(){
    return _printPose;
   }
   bool printScan(){
    return _printScan;
   }
   bool doMloc(){
    return _doMloc;
   }
   bool doAutoNavigation(){
    return _doAutoNavigation;
   }
   
 };
 
 
 enum LineFollowingPhases{
  LINEFOLLOW_INIT,
  LINEFOLLOW_CONTROL,
  LINEFOLLOW_EXIT,
  LINEFOLLOW_NUM
  
 };
 
 
 class LFDistanceEstimator {
  private:
   Decimal _lineValue;
   Decimal _lineSlope;
  public:
   LFDistanceEstimator(Decimal lineValue=0.0, Decimal lineSlope=1.0){
    _lineValue =  lineValue;
    _lineSlope = lineSlope;
   }
 
   
   Decimal estimate(Decimal _leftReading,Decimal _rightReading){
    Decimal leftDistance = (_leftReading - _lineValue) / _lineSlope;
    Decimal rightDistance = -(_rightReading - _lineValue) / _lineSlope;
    return leftDistance + rightDistance;
   }
 
 
 };
 
 
 class LineFollowing : public Task{
 
  private:
   static const TaskPlate _plate = LNF_TASK;
   static const long int  _maxDurSec  = 0,    _maxDurUsec   = 300000;
   static const long int  _maxSamplPeriodSec = 0, _maxSamplPeriodUsec = 300000;
   static const long int  _minSamplPeriodSec = 0, _minSamplPeriodUsec = 10000;
  
 
   Keyboard*  _keyboard; 
   MobileRob*   _mobileRob; 
   MobileRob3D*   _mobileRob3D; 
   //PositionDetector3D*  _positionDetector3D; ///<
   MotionModule*  _motionModule; 
   LineFollowingSharedMem*   _sharedMem;
 
   LineFollowingPhases _status;
   LineFollowingOptions _options;
   
   LFDistanceEstimator* distanceEstimator;
   string _String;
   
   int _fileFlag;
   int _controlType;
   Decimal _gain;
   Decimal _cruiseSpeed;
   Decimal _maxPolSpeed;
   Decimal _lineSlope;
   Decimal _cruiseControlPower;
   Decimal _timeFilter;
   Decimal _markUpGain;
   Decimal _leftReading;
   Decimal _rightReading;
   vector<int> _infraredMeasures;
   ofstream _myfile;
   Pose _pose;
   Decimal _linVel;
   Decimal _angVel;
   Decimal _velOpt;
   Decimal _curvature;
   Decimal _curvatureFiltered;
   Decimal _alpha;
   Decimal _drive;
   Decimal _turnrate;
   Decimal _minCurvature;
   Decimal _maxCurvature;
   Decimal _initSpeed;
   Decimal _minSpeed;
   Decimal _initTime;
   Decimal _irToAxisDistance;
   Decimal _maxEstimatedDistance;
 
   
   Timer _timer;
   bool _curvatureFlag;
   bool _doControl;
   bool _useMobileRob;
   bool _useMotionModule;
   bool _useMobileRob3D;
   
   lineFollowingControls  _controlType1;
 //   bool usePositionDetector3D;
 
 #ifndef MIP_PLATFORM_ARM
   DraWin* _draWin;
   unsigned int _drawList; 
   bool         dwOn;
 #endif
 
   bool rangeSensOn;
   bool rangeSensPrint;
   
   FirFilter *curvatureFilter;
   
   void _lineFollowingInit();
   void _lineFollowingControl();
   void _recovery();
   
  public:
   LineFollowing(ResourcePointers resources,int argc, const char* argv[]);
   
   ~LineFollowing(){
    cout << "destr LineFollowingTask" << endl;
   }
   
   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