/***************************************************************
 * PrIPBoardTRC.h
 *
 * This class provides the interface to hardware TRC IP board.
 *
 ****************************************************************/

/*
 * modification history
 *----------------------
 *
 * 10/26/97: K.C. Chang: created.
 */

#ifndef _PrIPBoardTRC_h
#define _PrIPBoardTRC_h

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

class PrTRCBoard;
class PrEncoderIntf;
class PrForceSensorIntf;
class PrAccelSensorIntf;

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

    // hardware reset
    virtual int zero();

    virtual int powerOn();
    virtual int powerOff();
    virtual int thermostat();
    virtual int calibrationCheck();
    virtual int calibrationSet();

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

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

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

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

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

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

    // read distance for base
    virtual int jointPositionBase(Float *q);

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

    virtual int jointVelocity(Float q[PR_ROBOT_NOJ],Float dq[PR_ROBOT_NOJ]);
    virtual int jointVelocityArm(Float *q,Float *dq);
    virtual int jointVelocityBase(Float *q,Float *dq);
    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();

    int handOpen();
    int handClose();

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

    Float motorScale_[PR_ROBOT_NOJ+PR_GRIPPER_NOJ];
    Float gearRatio_[PR_ROBOT_NOJ+PR_GRIPPER_NOJ];
    Float jointPositionAtNest_[PR_ROBOT_NOJ+PR_GRIPPER_NOJ];

    PrTRCBoard *boardTRC_;
    PrEncoderIntf *encoder2_;
    PrForceSensorIntf *fs_;
    PrAccelSensorIntf *accel_;

    Float qOld_[PR_ROBOT_NOJ+PR_GRIPPER_NOJ];
    Float dqOld_[PR_ROBOT_NOJ+PR_GRIPPER_NOJ];

    // initialize all hardware
    int Initialize();
    void InitializeArm();
    void InitializeBase();
    void InitializeGripper();

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

    // copy is not allowed
    PrIPBoardTRC(const PrIPBoardTRC &) {}
};

#endif // _PrIPBoardTRC_h
