Updated with servo functions and separated files