Added support for Kp values over 1000.

Fixed logging interval.
Merge pull request #3 from bradydoll/develop

Support Kp values over 1000.