If I understood correctly, you want to go to a specific point in the environment stored in the variable m_sCurrentGoal. The untested code, just out of my head, could be: void CSingleRobot::SetWheelSpeeds(const CCI_PositioningSensor::SReading& s_me_inworld, const CVector2& c_target_inworld) {...