Modified code to control servo (UI still needs to be updated)