added servomaske code like dominik said