Fixed #22. Fixed #8. Restructure Robot and Drive