Moved the control law to its own function. Added a variable to hold the error integration.