Added code for IR servo

IR servo is still not working right