Fixed a bug in motor command #6 and other minor changes