void BiclopsAccess::recordLimits()
{
for(int j = 0; j < BICLOPS_NUM_JOINTS; j++)
{
long hlf = 0, hlr = 0, slf = 0, slr = 0;
PMDAxisControl *axis = _biclops->GetAxis(j);
axis->GetHomingLimits(hlf, hlr, slf, slr);
limits[j][0] = axis->CountsToUnits(hlr);
limits[j][1] = axis->CountsToUnits(hlf);
}
}
double BiclopsAccess::getJointDesiredPosition(int jnt)
{
// Can only ask during HOMING, POSITION and PARKED.
if(_mode <= HOMING) return 0.0;
PMDint32 d;
PMDAxisControl *axis = _biclops->GetAxis(jnt);
axis->GetCommandedPosition(d);
double p = axis->CountsToUnits(d);
p = unitsToRadians(p, jnt);
return p;
}
double BiclopsAccess::getJointPosition(int jnt)
{
// Can only ask during HOMING, POSITION and PARKED.
if(_mode <= HOMING) return 0.0;
PMDint32 d;
PMDAxisControl *axis = _biclops->GetAxis(jnt);
axis->GetActualPosition(d);
double p = axis->CountsToUnits(d);
/*
if (jnt == 0) {
std::cout<<"Inside joint position1 :"<<(int) d<< " --> " << p << " cpUnit: " << axis->GetCountsPerEncoderCycle() << std::endl;
}
*/
p = unitsToRadians(p, jnt);
return p;
}