Finished servo and controller input code