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);
}
示例#2
0
文件: motor.cpp 项目: k29/voidMain
int Motor::setGoalPosition()
{
	return setGoalPosition(goalPosition);
}