Added ID profile to PID init and new init func
Added new global variables and functions to the PID controller which is supposed to sync with the main program
Added pid controller functions and low pass filter functions