Fix pid unbounded i error

Here are some tests that show the behavior you're talking about in this PR: https://github.com/willowgarage/ros_control/pull/14/files