Cleaned autonomous code

Made magic numbers constants and removed test code