void ServoDynamixel::moveGoalPosition(int move)
{
//std::cout << "moveGoalPosition(" << move << ")" << std::endl;
access.lock();
int curr = registerTableValues[gid(REG_CURRENT_POSITION)];
int newpos = registerTableValues[gid(REG_CURRENT_POSITION)] + move;
// Wheel mode ?
if (registerTableValues[gid(REG_MIN_POSITION)] == 0 && registerTableValues[gid(REG_MAX_POSITION)] == 0)
{
if (newpos < 0 || newpos > steps)
{
int mod = newpos % steps;
std::cerr << "[#" << servoId << "] moveGoalPosition([" << curr << " > " << newpos << "] [VALUE ERROR] with modulo: " << mod << std::endl;
}
}
access.unlock();
setGoalPosition(newpos);
}
int Motor::setGoalPosition()
{
return setGoalPosition(goalPosition);
}