adjustments to final controller. template is being used to test servo control - needs more work