Added PID library, some bug fixes