Completed PID library and fixed a couple examples in FreeIMU to work with updated library