Iterating on controller

- Fixing some issues in PID code
- Resetting PID at each goal change
- Added two examples (not working) of autonomous fly
Refactored controller