Implemented AutoDrive

goalDistance has yet to be determined