Odometry3D.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 __ODOMETRY3D_H
#define __ODOMETRY3D_H

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

#include "TrajControl.h"



/* @{ */


class Odometry3D{
 private:
  bool first;//TODO si può omettere se inizializzo tutto nel costruttore?
//   long int N_LeftW_prec,N_RightW_prec;//Input di update
  long int _leastLeftRead, _leastRightRead;
//    Timer _encTimer;
  Time _leastLeftTime,_leastRightTime;
 public:
  
  bool updateOdometry3D(long int currLeftRead, long int currRightRead, long int encRes, Time currLeftTime, Time currRightTime,TrajectoryState &trajStatus)
  {
   Decimal dT,dTR,dTL;  
   Decimal vr, vl;
   long   N_LeftW, N_RightW;
   Decimal vel_tick_r,vel_tick_l;
//    Time currLeftTime,currRightTime;
   
   static Decimal Deltang=(2.0*M_PI/encRes);
   if(first)
   {//TODO verifica
    _leastLeftRead  = 0;//NOTE origine della lettura?
    _leastRightRead = 0;//NOTE origine della lettura?
    _leastLeftTime=Time(0,0);
    _leastRightTime=Time(0,0);
//     _encTimer.reset();
    first = false;
   }
  
//   wheelCountersL = getLeftEncoder();//NOTE nuova della lettura?
//   double ltime   = leggiTempo3(tvl);
//   currLeftTime=_encTimer.get();
//   wheelCountersR = getRightEncoder();//NOTE nuova della lettura?
// //   double rtime   = leggiTempo3(tvr);
//   currRightTime=_encTimer.get();
  
  int tent = 0;
  int maxTent =2;
  while( ( (currLeftRead - _leastLeftRead > 200) || (currLeftRead - _leastLeftRead < -200) ) && (tent<maxTent))
  {/*provo a filtrare letture fallaci*/
//    wheelCountersL = getLeftEncoder();
//    ltime          = leggiTempo3(tvl);
   //TODO aggiornare comunque least readings?qui wheelCountersL
   tent++;
   return false;
  }
  tent=0;
  N_LeftW = currLeftRead - _leastLeftRead;
  while( ( (currRightRead - _leastRightRead > 200)|| (currRightRead - _leastRightRead < -200) )  && (tent<maxTent))
  {/*provo a filtrare letture fallaci*/
//    wheelCountersR = getRightEncoder();
//    rtime          = leggiTempo3(tvr);
   //TODO aggiornare comunque least readings?qui wheelCountersR
   tent++;
   return false;
  }
  N_RightW = currRightRead - _leastRightRead;
//  if(wheelCountersL > 2765){
//    wheelCountersL  = getLeftEncoder();
//    ltime = leggiTempo3(tvl);
//  }
//  N_LeftW=wheelCountersL-N_LeftW_prec;
  //  /*!filtraggio per evitare letture incoerenti (maggiori della risoluzione max dell'encoder)*/
//  if(wheelCountersR > 2765)
//      { 
//   wheelCountersR  = getRightEncoder();
//   rtime = leggiTempo3(tvr);
//  }
//  N_RightW=wheelCountersR-N_RightW_prec;
  Time dtLeft=currLeftTime-_leastLeftTime;
  Time dtRight=currRightTime-_leastRightTime;
  dTL = dtLeft.dCast();
  dTR =dtRight.dCast();
  Time dTime=(Time)(dtRight+dtLeft)*0.5;
  dT=dTime.dCast();
 
 //Calcolo di vl, vr, v & omega
  vel_tick_l = rint(N_LeftW/dTL);  //pulse/s
  vel_tick_r = rint(N_RightW/dTR); //pulse/s

  _leastLeftTime = currLeftTime;
  _leastRightTime = currRightTime;
  
  // TODO decommentare queste righe e far compilare ###
/*  vl=(2.0/ (E_d+1.0))          * (N_LeftW  * Deltang * (wheel_diameter/2.0)) / dTL;  // m/s
  vr=(2.0/ ( (1.0/E_d) +1.0) ) * (N_RightW * Deltang * (wheel_diameter/2.0)) / dTR; // m/s*/
  
  trajStatus.setDtWheel((Position)(dTR,dTL));
//   MState.dTL = dTL;
//   MState.dTR = dTR;
  trajStatus.setWheelVel((Position)(vr,vl));
//   MState.vl = vl;
//   MState.vr = vr;
// TODO decommentare queste righe e far compilare ###0
//   trajStatus.setStatusVel(VelVec(0.5*(vr+vl),(vr-vl)/(E_b*axle_length),dTime));
//   MState.V  = 0.5*(vr+vl);
//   MState.W  = (vr-vl)/(E_b*axle_length);
//   dT        = 0.5*(dTR + dTL);
 
  //Integrazione Runge-Kutta del II ordine
  Angle theta=trajStatus.statusPose().ori();
  Decimal V=trajStatus.statusVel().linVel();
  Decimal W=trajStatus.statusVel().angVel();
//   Pose dPose=Pose (cos(theta.dCast() + W*dT*0.5 )*V*dT,sin(theta.dCast() + W*dT*0.5 )*V*dT,(Angle)(normalize(theta.dCast()+(W*dT))));
  Pose dPose=Pose (cos(theta.dCast2Pi() + W*dT*0.5 )*V*dT,sin(theta.dCast2Pi() + W*dT*0.5 )*V*dT,(Angle)(theta.dCast2Pi()+(W*dT)));
  trajStatus.setStatusPose(trajStatus.statusPose()+dPose);
    
//   MState.x +=  cos( MState.theta + MState.W*dT*0.5 )*MState.V*dT.dCast();
//   MState.y +=  sin( MState.theta + MState.W*dT*0.5 )*MState.V*dT.dCast();
//   MState.theta = normalizza( MState.theta + MState.W * dT);
         
  //Aggiornamento dello stato
  _leastLeftRead  = currLeftRead;
  _leastRightRead = currRightRead; 

//   if(log_flag){
//    MLogState.x = MState.x;
//    MLogState.y = MState.y;
//    MLogState.theta = MState.theta;
//    MLogState.leftTicks  = wheelCountersL;
//    MLogState.rightTicks = wheelCountersR;
//    MLogState.leftTv = tvl;
//    MLogState.rightTv = tvr;
//   }
  return true;
 }
};


#endif


/* @} */