Will need to further change to remove integral wind up and add a low pass filter to filter the noise for the derivative term.
This is the progress that I had made before putting this project on GitHub