Made attitude controller.
Final Attitude Controller fixes.