added servo control
serial working, also tests