moving diagnostics out of robot main