Fix memory problems and update sensor states with data from robot