Changed integrator for better performance; fiddled with gains some.