PID library giving nan out output

When debugging a problem like this, it is extremely useful to pare the code down to the minimum that should work. Remove extraneous and wrong comments, too.

It is then much easier to track down the error, and you can always put the other stuff back in (not that it was your code to start with).