added servo shaking routine
implemented 2 modes: oscillate & shake