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