Changes after testing. Most things working, except camera aim at target. This can be the fallback code for the first competition if the upcoming fixes don't work.