Problem with Servo and Encoder

Other Hardware Motors, Mechanics, Power and CNC
6 
    delay(50);

Not a good idea. This will cause it to miss encoder steps.