void Camera::moveHeadToFocus() { Joint *pan = head->getJoint(PAN); Joint *tilt = head->getJoint(TILT); float deltaPan = this->focusPan - pan->getAngle(); float deltaTilt = this->focusTilt - tilt->getAngle(); deltaPan /= this->focusStepDiv*3.0/5.0; deltaTilt /= this->focusStepDiv; pan->setGoalAngle(pan->getAngle() + deltaPan); tilt->setGoalAngle(tilt->getAngle() + deltaTilt); }