TrajControl.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 __TRAJ_CONTORL_H
#define __TRAJ_CONTORL_H

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

#include "Trajectory.h"

using namespace MipBaselib;
class VelVec{
 private:
  Decimal _linVel;
  Decimal _angVel;
  Time _time;
 public:
  VelVec();
  
  VelVec(Decimal linVel, Decimal angVel, Time time);
  
  VelVec(const VelVec& vel);
  
  VelVec& operator=(const VelVec& vel);
  
  /***  SET FUNCTIONS  ******************/
  void setLinVel(Decimal value);
  
  void setAngVel(Decimal value);
  
  void setTime(Time value);
  
  /***  GET FUNCTIONS  ******************/
  Decimal linVel();
  
  Decimal angVel();
  
  Time time();
};


class OpenLoopIn{
 protected:
  Position _desiredPosition;
  Position _desiredDPosition;
  
 public:
  OpenLoopIn();
  
  OpenLoopIn(Position& desP, Position& desDP);
  
  OpenLoopIn(OpenLoopIn& other);
  
  OpenLoopIn operator=(OpenLoopIn& other);
  
  /*** Set functions *************************/
  void setDesiredPosition(Position& p);
  
  void setDesiredDPosition(Position& p);
  
  /*** Get functions *************************/
  Position desiredPosition();
  
  Position desiredDPosition();
};


class NonlinearIn : public OpenLoopIn{
 protected:
  Pose _robotPose;
  
 public:
  NonlinearIn();
  
  NonlinearIn(Pose& rP, Position& desP, Position& desDP);
  
  NonlinearIn(NonlinearIn& other);
  
  NonlinearIn operator=(NonlinearIn& other);
  
  /*** Set functions *************************/
  void setRobotPose(Pose& p);
  
  /*** Get functions *************************/
  Pose robotPose();
};



class DFLIn: public NonlinearIn{
  Pose _robotDPose;
  Position _desiredDDPosition;
  Decimal _timeOfSampling;
  
 public:
  DFLIn();
  
  DFLIn(Pose& rP, Pose& rDP, Position& desP, Position& desDP, Position& desDDP, Decimal tos = 0.0);
  
  DFLIn(DFLIn& other);
  
  DFLIn operator=(DFLIn& other);
  
  /*** Set functions *************************/
  void setRobotDPose(Pose& p);
  
  void setDesiredDDPosition(Position& p);
  
  void setTimeOfSampling(Decimal tos);
  
  /*** Get functions *************************/
  Pose robotDPose();
  
  Position desiredDDPosition();
  
  Decimal timeOfSampling();
};


class TrajectoryState{
 
 private:
  Pose _statusPose;
  Pose _statusPosep;
  Position _dxy;
  Decimal _PSpace;
  VelVec _statusVel;
  Position _wheelVel;
  Position _dtWheel;
  
 public:
  TrajectoryState();
  
  TrajectoryState(const TrajectoryState& status);
  
  TrajectoryState& operator=(const TrajectoryState& status);
  
  /***  SET FUNCTIONS  ******************/
  void setStatusPose(Pose value);
  
  void setStatusPosep(Pose value);
  
  void setDxy(Position value);
  
  void setPSpace(Decimal value);
  
  void setStatusVel(VelVec value);
  
  void setWheelVel(Position value);
  
  void setDtWheel(Position value);
  
  /***  GET FUNCTIONS  ******************/
  Pose statusPose();
  
  Pose statusPosep();
  
  Position dxy();
  
  Decimal PSpace();
  
  VelVec statusVel();
  
  Position wheelVel();
  
  Position dtWheel();
  
};


class TrajControl{
 private:
  
//   Decimal _prevTime;
//   Decimal _currTime;
  
  Decimal _oldDrive;
  Decimal _oldTurnRate;
  
  Decimal _csi;         
  Decimal _prevErr;
  Decimal _prevU;
//   Decimal normalize(Decimal a);
  Timer _timer;
 public:
  
  TrajControl();
  
  void openLoop(AnalyticalTrajectory ref, VelVec &Vr, Time time);
  
  void nonlinear(AnalyticalTrajectory refTraj, VelVec &vr, Time time, TrajectoryState mState);
  
  void DFL(AnalyticalTrajectory &trif, VelVec &vr, Decimal tos, Time time, TrajectoryState mState);
  
  void FL(AnalyticalTrajectory &trif, VelVec &vr, Decimal tos, Time time, TrajectoryState mState);
  
  void openLoop(OpenLoopIn &input, VelVec &vr, Time &time);
  
  void nonlinear(NonlinearIn &input, VelVec &vr, Time &time);
  
  void DFL(DFLIn &input, VelVec &vr);
  
  void Tustin(Angle angReff, VelVec &Vr, TrajectoryState mState);
  
  void reset(){
   _csi = 0.0;
   _timer.reset();
  }
};

#endif


/* @} */