Made some values in the pid controller static so they don't reset on each call. Could do with moving the PI coeficients into config.h as defines