Added code for plotting control and bug fixes
Added independent x and y PID gain values and added dynamically turning i on and off and ignore bogus d values