int DynamixelSimpleAPI::setGoalPosition(const int id, const int position, const int speed)
{
int status = 0;
if (checkId(id) == true)
{
// Set goal speed, and if success proceed to set goal position
if (setGoalSpeed(id, speed) != -1)
{
// Valid positions are in range [0:1023] for most servo series, and [0:4095] for high-end servo series
if ((position >= 0) && (position <= 4095))
{
int addr = getRegisterAddr(ct, REG_GOAL_POSITION);
dxl_write_word(id, addr, position);
if (dxl_print_error() == 0)
{
status = 1;
}
}
else
{
TRACE_ERROR(DAPI, "[#%i] Cannot set goal position '%i' for this servo: out of range\n", id, position);
}
}
}
return status;
}
void Motor::setSpeed(double speed)
{
setGoalSpeed(speed);
setDirty(1);
goalSpeedInit = 1;
}
