Fixed some bugs with the AI, going to add pendulum tomorrow.

- AI works slightly better now
- Had to comment out the SAT debugs so I could debug myself