Test 1 updated to test also servos