Broke out PID controller function to utils.py and built an actual PID controller