bool yarp::dev::UrbtcControl::positionMoveRaw(const double *refs){
bool success = true;
for (int i = 0; i < _intNumControllers; i++)
success = success && positionMoveRaw(i, refs[i]);
return success;
}
bool yarp::dev::UrbtcControl::resetEncoderRaw(int j){
if(j > -1 && j < _intNumControllers){
_mutex.wait();
_cmd[j].retval = 0;
_cmd[j].setoffset = CH0 | CH1 | CH2 | CH3;
_cmd[j].setcounter = CH0 | CH1 | CH2 | CH3;
_cmd[j].resetint = CH0 | CH1 | CH2 | CH3;
_cmd[j].selin = SET_SELECT | SET_CH2_HIN;
_cmd[j].dout = 0;
_cmd[j].selout = SET_SELECT | CH2;
_cmd[j].offset[0] = _cmd[j].offset[1] = _cmd[j].offset[2] = _cmd[j].offset[3] = 32768;
_cmd[j].counter[0] = _cmd[j].counter[1] = _cmd[j].counter[2] = _cmd[j].counter[3] = 0;
_cmd[j].posneg = SET_POSNEG | CH0 | CH1 | CH2 | CH3; /*POS PWM out*/
_cmd[j].breaks = SET_BREAKS | CH0 | CH1 | CH2 | CH3; /*No Brake*/
_cmd[j].magicno = 0x00;
_cmd[j].wrrom = 0; /* WR_MAGIC | WR_OFFSET | WR_SELOUT;*/
// prepare for writing _cmd
if (ioctl(_fd[j], URBTC_COUNTER_SET) < 0) {
printf("Error on ioctl() for device number: %d\n", j);
_mutex.post();
return false;
}
// write the cmd struct out to the device
if (write(_fd[j], &_cmd[j], sizeof(_cmd[j])) < 0) {
printf("Error writing cmd struct to device number: %d\n", j);
_mutex.post();
return false;
}
// prepare for writing _out again
if (ioctl(_fd[j], URBTC_DESIRE_SET) < 0) {
printf("Error ioctl() URBTC_DESIRE_SET\n");
_mutex.post();
return false;
}
if (ioctl(_fd[j], URBTC_CONTINUOUS_READ) < 0) {
printf("Error ioctl() URBTC_CONTINUOUS_READ\n");
_mutex.post();
return false;
}
if (ioctl(_fd[j], URBTC_BUFREAD) < 0) {
printf("Error ioctl() URBTC_BUFREAD\n");
_mutex.post();
return false;
}
// do a read
int test;
if ((test = read(_fd[j], &_in[j], sizeof(_in[j]))) != sizeof(_in[j])) {
printf("Warning: read size mismatch %d != %d\n", test, sizeof(_in[j]));
}
// set position to 0 (otherwise controller would move to the last specified position relative to new origin)
positionMoveRaw(j, 0.0);
_mutex.post();
return true;
}
return false;
}
bool DimaxU2C::positionMoveRaw(const double *refs) {
for (int i=0;i<numJoints;i++) {
positionMoveRaw(i,refs[i]);
}
return true;
}
