Updated code with servo test application (tested using logic analyser)