PID functionality added. Parameters still need to be adjusted