AI Loop

Working (sorta) heading code. Need to fix motor control