Cleanup, clearer status, added robot servo control functions