added 2 loop, PID loop for angle delta and PI loop for omega delta