Moving R2_Servo_Control into a class