Adding Differential control too..
PID is now complete. :P