Fixed a bug in the PID library and updated Plane leveling testing