Broken implementation of the cleaned up PID.
Cleaned up PID. Now gets fed a struct containing state and settings for better generalization.