/***************************************************************
 * PrSTGBoard.cpp
 *
 * This objects content code specific to the 
 * Trident Robotic Puma controller / ServoToGo board
 * Association
 *
 ****************************************************************/

/*
 * modification history
 *----------------------
 *
 * 02/24/99 N. Turro : created
 * 22/02/03 Weyn Ong : Used to be named StgPuma.  Renamed for consistency.
 *                     Moved #defines to PrSTDDefn.h.
 *
 * TODO :
 *-------
 * check the return values with classServoToGo
 */

#include "driver/PrSTGBoard.h"
#include "driver/ServoToGo.h"
#include "driver/PrSTGDefn.h"

PrSTGBoard::PrSTGBoard(){
  stg_ = new ServoToGo();
  stg_->Init(STG_ARM_IRQ,STG_ARM_ADDRESS);
  stg_->DioDirection(0x0c);

  flag_ = PR_STGBOARD_OK;
}

PrSTGBoard::~PrSTGBoard(){

  // Set DAC's to zero's
  for (unsigned short nAxis = 0; nAxis < STG_MAX_AXIS; nAxis++)
    stg_->RawDAC(nAxis, 0);
  delete stg_;
}

void
PrSTGBoard::zero( void ) {
  for( int axis=0; axis<6 ; axis++){
    stg_->SetEncoderCounts( axis , 0x00000000 );
  }
}

void
PrSTGBoard::jointForce(const Float *tau){
  static const Float motorScale_[]= {-0.08971, 0.10500, -0.15368, 0.45082, 0.51529,  0.43906}; // guess

  static long counts;

  for( unsigned char i=0; i<6; i++ )
  {
    counts = (long int) (tau[i] * motorScale_[i] * 
        (1.0/AMPS_PER_VOLT) * COUNTS_PER_VOLT );
    stg_->RawDAC(  i , counts);  // RawDAC will check MAX/MIN values
  }
}

void
PrSTGBoard::readPos(Float *q){
  static const int encoderCountPerRev[] = PR_ARM_ENCODER_COUNT_PER_REV;
  static const Float gearRatio_[]=PR_ARM_GEARRATIO;
  static STG_LONGBYTE enc[8];

  stg_->EncoderLatch();
  stg_->EncReadAll(enc);

  for(int i=0;i<6;i++){
    q[i]=-2*M_PI*enc[i].Long/(Float)encoderCountPerRev[i]/gearRatio_[i];
  }
}

int PrSTGBoard::handOpen()
{
  stg_->HandOpen(); 
  return PR_STGBOARD_OK;
}

int PrSTGBoard::handClose()
{
  stg_->HandClose(); 
  return PR_STGBOARD_OK;
}
