ServoInterface class for reading added, project controller_test added