bhandforce.h

Go to the documentation of this file.

/***************************************************************************
                          bhandforce.h
                             -------------------
    copyright            : (c) 2004 Tim Baier
    email                : tbaier@informatik.uni-hamburg.de
 ***************************************************************************/

#ifndef BHANDFORCE_H
#define BHANDFORCE_H

#include <string>
#include <zbsplinefunction.h>

#include "bhand.h"
#include "bhandext.h"
#include "bhkinematic.h"
#include "bhfinger.h"
#include "bhspread.h"

#include "bhspeedgen.h"


#define WITH_STOP

#define DEFAULT_LOOP_CTRL_VEL_COEFF 8

#define DEFAULT_MAX_POS_ERROR_FINGER 25

#define DEFAULT_MAX_POS_ERROR_SPREAD 50

#define MAX_PROFILE_ERROR 350

#define MAX_FORCE_ERROR 10

#define DELTA_POS_COEFF 6

class MoveStruct
{
 public:
   MoveStruct(MotorFeedback *f1, MotorFeedback *f2, MotorFeedback *f3, MotorFeedback *spread);

  ~MoveStruct();

  BHFinger *Fingers[3];

  BHSpread *Spread;
};

//****************************//
//**                        **//
//****************************//
inline MoveStruct::MoveStruct(MotorFeedback *f1, MotorFeedback *f2, MotorFeedback *f3, MotorFeedback *spread)
{
  Fingers[0] = new BHFinger(f1);
  Fingers[1] = new BHFinger(f2);
  Fingers[2] = new BHFinger(f3);

  Spread     = new BHSpread(spread);
}


//****************************//
//**                        **//
//****************************//
inline MoveStruct::~MoveStruct()
{
   delete []Fingers;
   delete Spread;
}


enum RTFlags{
  UNKOWN,         
  FINGER_MODE,    
  FORCE_MODE,     
  MOVE_MODE,       
  GAIN_MODE
};

class BHandForce : public BHandExt  {
public:

  BHandForce(std::string devName);


  virtual ~BHandForce();


  void init() throw (BHandException);

  void setRTDefaults();

  void setRTGainDefaults();

  void setRTForceDefaults();



   void move(const Motors motors, const bool plotIt = false) throw (BHandException);

  void move(const Motors motors, const int position) throw (BHandException);

  void move(const Motors motors, const int position[4]) throw (BHandException);

#if 0

   void moveG(const Motors motors, const int position[4])throw (BHandException){};
#endif
   void moveG(const Motors motors) throw (BHandException);
   void moveG(const Motors motors, const int position) throw (BHandException);


  void openFingers(const Motors motors) throw (BHandException);

  void closeFingers (const Motors motors) throw (BHandException);

  void openSpread() throw (BHandException);

  void closeSpread() throw (BHandException);


  void adjustForce(const Motors motors, const int force) throw (BHandException);

  
  void moveForce(const Motors motors) throw (BHandException);

  void moveForce(const Motors motors, int force[3]) throw (BHandException);
  

  void force2File(std::string fileName, bool showPlot = false);

  void forceCalib(std::string fileName, bool showPlot = false);



   void setSpeed(const Motors motors, const int speed);

   void setMaxForce(const short force, const Motors motors = FINGERS);

   void setMinForce(const short force, const Motors motors = FINGERS);

   void setMaxPosErrorF(const int error);

   void setMaxPosErrorS(const int error);

   void setPos(const Motors motors, const int position);

  void setPropGain(const Motors motors, const unsigned short gain);

  void setVelCoeff(const Motors motors, const unsigned short coeff);

   void initForceSensor(const Motors motors = FINGERS); //throw (BHFingerException);

   void getForceReal(int *forces);

   void getForce();

   void getForce(int *forces, const bool update = true);

   void writeForce(std::ostream *stream);

   int forceInRange(const BHandKin::FINGER f, const bool update = true);


   RTFlags getRTFlags();

   //void forceTester();


  BHFinger& getFinger(const BHandKin::FINGER f) const;

  BHSpread& getSpread() const;

  BHMotorExt& getMotor(const Motors motor) const;

 protected:

   int clipSpeed(int speed);

   int clipSpeed(double speed);


   int clipSSpeed(double speed);


   int clipGain(int gain);

   void resetDeltaPos();


   MoveStruct *MoveParams;

   int MaxPosErrorFinger;

   int MaxPosErrorSpread;

   unsigned short LoopCtrlCoeff;

   ControlBlock Cb;

   FeedbackBlock Fb;

   RTFlags RealTimeFlags;


   ZBSplineFunction *ForceSpline[NUM_FINGERS];
};


// ****************************
// **                        **
// ****************************
inline int BHandForce::forceInRange(const BHandKin::FINGER f, const bool update)
{
  if(update)
    getForce();

  return MoveParams->Fingers[f-1]->forceInRange();
}


// ****************************
// **                        **
// ****************************
inline void BHandForce::getForce(int *forces, const bool update)
{
  if(update){
    try{
      // std::cout << "  BHandForce::getForce() before " << std::endl;
      getForce();
      //std::cout << "  BHandForce::getForce() after " << std::endl;
    }
    catch(...){
      std::cout << " ** getForce(int *forces, const bool update) " << std::endl;
    }
  }

  // std::cout << "  BHandForce::getForce() before1 " << std::endl;
  //  for(int i = NUM_FINGERS-1; i >= 0; i--)
  //  forces[i] =  MoveParams->Fingers[i]->getForce();
  // std::cout << "  BHandForce::getForce() after2 " << std::endl;

  forces[0] =  MoveParams->Fingers[0]->getForce();
  forces[1] =  MoveParams->Fingers[1]->getForce();
  forces[2] =  MoveParams->Fingers[2]->getForce();

}


// ****************************
// **                        **
// ****************************
inline int BHandForce::clipSpeed(int speed)
{
  if(abs(speed) < MIN_FINGER_SPEED)
    return (speed < 0) ? -MIN_FINGER_SPEED :  MIN_FINGER_SPEED;

  if(abs(speed) > MAX_FINGER_SPEED)
    return (speed < 0) ?  -MAX_FINGER_SPEED : MAX_FINGER_SPEED;

  return speed;
}


// ****************************
// **                        **
// ****************************
inline int BHandForce::clipSpeed(double speed)
{
  if (fabs(speed) < MIN_FINGER_SPEED)
    return (speed < 0) ? -MIN_FINGER_SPEED :  MIN_FINGER_SPEED;
  
  if (fabs(speed) > MAX_FINGER_SPEED)
    return (speed < 0) ?  -MAX_FINGER_SPEED : MAX_FINGER_SPEED;

  return (int) speed;
}

// ****************************
// **                        **
// ****************************
inline int BHandForce::clipSSpeed(double speed)
{
  if (fabs(speed) <  MIN_SPREAD_SPEED)
    return (speed < 0) ? -MIN_SPREAD_SPEED :  MIN_SPREAD_SPEED;

  if (fabs(speed) > MAX_SPREAD_SPEED)
    return (speed < 0) ?  -MAX_SPREAD_SPEED : MAX_SPREAD_SPEED;

  return (int) speed;
}


// ****************************
// **                        **
// ****************************
inline int BHandForce::clipGain(int gain)
{
  int result = (gain > 255) ? 255 : gain;
  result = (gain < 1) ?  1 : result;

  return result;
}


// ****************************
// **                        **
// ****************************
inline  void BHandForce::setMaxPosErrorF(const int error)
{
  MaxPosErrorFinger = error;
}


// ****************************
// **                        **
// ****************************
inline  void BHandForce::setMaxPosErrorS(const int error)
{
  MaxPosErrorSpread = error;
}


// ****************************
// **                        **
// ****************************
inline void BHandForce::openFingers (const Motors motors) throw (BHandException)
{
  Motors dummyM = Motors(motors & FINGERS);
  move(dummyM, MIN_FINGER_POS);
}


// ****************************
// **                        **
// ****************************
inline void BHandForce::closeFingers (const Motors motors) throw (BHandException)
{
  Motors dummyM = Motors(motors & FINGERS);
  move(dummyM, MAX_FINGER_POS);
}


// ****************************
// **                        **
// ****************************
inline void BHandForce::openSpread() throw (BHandException)
{
  BHand::open(SPREAD);
}

// ****************************
// **                        **
// ****************************
inline void BHandForce::closeSpread() throw (BHandException)
{
  BHand::close(SPREAD);
}

// ****************************
// **                        **
// ****************************
inline BHFinger& BHandForce::getFinger(const BHandKin::FINGER f) const
{
  return *(MoveParams->Fingers[f-1]);
}

// ****************************
// **                        **
// ****************************
inline BHSpread& BHandForce::getSpread() const
{
  return *(MoveParams->Spread);
}

// ****************************
// **                        **
// ****************************
inline BHMotorExt& BHandForce::getMotor(const Motors motor) const
{
  if(motor & F1)
    return *(MoveParams->Fingers[0]);

  if(motor & F2)
    return *(MoveParams->Fingers[1]);

  if(motor & F3)
    return *(MoveParams->Fingers[2]);

  if(motor & SPREAD)
    return *(MoveParams->Spread);
}


// ****************************
// **                        **
// ****************************
inline void BHandForce::setVelCoeff(const Motors motors, const unsigned short coeff)
{
  LoopCtrlCoeff = coeff;
  if(RtModeEnabled)
    stop();

  setParam(motors, LOOP_CTRL_VEL_COEFF, LoopCtrlCoeff);
}


// ****************************
// **                        **
// ****************************
inline RTFlags BHandForce::getRTFlags()
{
  return RealTimeFlags;
}



// ****************************
// **                        **
// ****************************
inline void BHandForce::resetDeltaPos()
{
  for(int i = NUM_MOTORS-1; i >= 0; i--){
    Fb.mf[i].loopFeedbackDeltaPos = 0;
  }
}

#if 1
// ****************************
// **                        **
// ****************************
inline void BHandForce::writeForce(std::ostream *stream)
{
  int forces[3] = {0,0,0};
  getForce(forces,true);
  *stream << forces[0] <<  "  " << forces[1] << " " << forces[2] << " "; 
}
#endif

#endif

---

Tim Baier bhandlib v0.25

Generated Wed Aug 16 14:57:09 2006 by 1.4.7