Fixed more bugs and updated autonomous