mobRob2DPIDController.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 __MOB_ROB_2D_CTRL
#define __MOB_ROB_2D_CTRL

#include <Spaces.h>
#include <MobileRob.h>
#include <math.h>

namespace MipAlgorithms{
// ///\class mobRob2DPIDControllerPars
// ///\brief Contains the parameters for the Controller.
// ///\author Roberto Capobianco, Chiara Picardi, Lorenzo Saccares
class mobRob2DPIDControllerPars {
 public:
  Decimal b;
  unsigned int k1;
  unsigned int k2; 
  unsigned int k3;
  Pose* objPose;
  Decimal objLv;
  Decimal objAv;
  mobRob2DPIDControllerPars();
  mobRob2DPIDControllerPars(const mobRob2DPIDControllerPars&);
  ~mobRob2DPIDControllerPars();
  string print();
};

// ///\class mobRob2DPIDController
// ///\brief Contains the parameters and the methods needed to compute a I/O Linearization Controller for a MobileRob Khepera.
// ///\author Roberto Capobianco, Chiara Picardi, Lorenzo Saccares
class mobRob2DPIDController {
 private:
  mobRob2DPIDControllerPars* _pars;   
  Decimal _integral_x;      
  Decimal _integral_y;      
  Decimal _previousTimeStamp;     
  
  bool firstStep;
  void extractFromState(Decimal&, Pose&, Decimal&, Decimal&, MotionModuleTState*);
  void extractFromPose(Decimal&, Decimal&, Decimal&, const Pose*);
  void computeCTRLPointCoords(Decimal&, Decimal&, const Pose*);
  void computeCTRLPointVel(Decimal&, Decimal&, const Decimal, const Decimal, const Decimal);
  void computeRobotVelocities(Decimal&, Decimal&, const Decimal, const Decimal, const Decimal);
 public:
  mobRob2DPIDController(const mobRob2DPIDControllerPars*);
  ~mobRob2DPIDController();
  bool step(Decimal&, Decimal&, MotionModuleTState*);
  
};
}

#endif