Started working on servo code