Controll update #2

- Changed rotation algorithm (Needs testing)
- Added comments