/***************************************************************
 * PrIPBoardSTG.h
 *
 * This hardware interface is designed to control a Puma Arm
 * thru a servoToGo model 2 Board.
 * Contains board/amp independant stuff
 *
 ****************************************************************/

/*
 * modification history
 *----------------------
 *
 * 02/14/99 N. Turro : created
 * 02/22/03 Weyn Ong : Used to be PrStgPuma.  Renamed for consistency.
 *                     This implementation does *not* handle a base!
 *                     Should unify with Samm driver.
 */

#ifndef _PrIPBoardSTG_h
#define _PrIPBoardSTG_h

#include "PrGlobalDefn.h"
#include "PrRobotDefn.h"
#include "PrIPBoardIntf.h"

class PrSTGBoard;
class PrForceSensorIntf;
class PrAccelSensorIntf;
class Filter;
class ServoToGo; 

class PrIPBoardSTG : public PrIPBoardIntf
{
 public:
    // constructor sets flag
    // flag() should be called before using
    // calls Initialize() which calls zero()
    PrIPBoardSTG(int servoRate,int armID, int gripperID, int fsID,
                 int accelID, int baseID);
    virtual ~PrIPBoardSTG();
    // error checking for constructor
    virtual int flag();

    // hardware reset
    virtual int zero();

    virtual int powerOn();		// NOT IMPLEMENTED !!
    virtual int powerOff();		// NOT IMPLEMENTED !!
    virtual int thermostat();		// NOT IMPLEMENTED !!
    virtual int calibrationCheck();	// NOT IMPLEMENTED !!
    virtual int calibrationSet();		// NOT IMPLEMENTED !!

    // set torques(force) for base and arm
    virtual int jointForce(Float *tau);

    // set torques for arm
    virtual int jointForceArm(Float *tau);

    // set force for base
    virtual int jointForceBase(Float *tau);
    virtual int jointForceBase(Float *tau , Float *tauExt);

    // set force for gripper
    virtual int gripperForce(Float *tau);

    // read angles(distance) for base and arm
    virtual int jointPosition(Float *q);

    // read angles for arm
    virtual int jointPositionArm(Float *q);

    // read distance for base
    virtual int jointPositionBase(Float *q); //DUMMY, NOT IMPLEMENTED

    // read distance for gripper
    virtual int gripperPosition(Float *q);

    virtual int jointVelocity(Float *q,Float *dq);
    virtual int jointVelocityArm(Float *q,Float *dq);
    virtual int jointVelocityBase(Float *q,Float *dq); //DUMMY, NOT IMPLEMENTED

    virtual int gripperVelocity(Float *q,Float *dq);

    // Get a handle on the force sensor.
    virtual PrForceSensorIntf * forceSensor( void ) { return fs_; }

    // read force from the force sensor
    virtual int forceSensor(Float *f);

    // zero the force sensor
    virtual int forceSensorZero();

    // Get a handle on the accel sensor.
    virtual PrAccelSensorIntf * accelSensor( void ) { return accel_; }

    // read accel from the accel sensor
    virtual int accelSensor(Float *f);

    // zero the accel sensor
    virtual int accelSensorZero();

    // return position and velocity of base in local op coords
    int jointPositionVelocityBase(Float *q,Float *dq); // NOT IMPLEMENTED

    int handOpen();
    int handClose();
    
    ServoToGo* GetBoard();

  private:
    PrSTGBoard *boardSTG_;
    PrForceSensorIntf *fs_;
    PrAccelSensorIntf *accel_;

    int flag_;
    const Float tick_;
    int armID_,gripperID_,fsID_,accelID_,baseID_;

    Filter *filter_qdArm;

    Float qOld_[6];
    Float dqOld_[6];

    // initialize all hardware
    int Initialize();

    int VelocityFilter(int noj,Float *qo,Float *q,Float *dqo,Float *dq);
};

#endif // _PrIPBoardSTG_h
