Autonomous code

Added come Autonomous code which will hopefully work. May need to modify
the values for precision.