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);
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;
}