Updated PIDmotor library with new function