Working lab run code with Lab Model

Tuned enough to consistently run on an 8 foot length.
Post-Site Visit Code Fixes

Resolution of bugs found during site visit:
-Initialization of integral gain values
-Logic errors on error calculation and unchecked cases