Thanks for reply!
I got 4 preset speeds with two relays instead of pwm. Im using VFD's, so 00 - 01 - 10 and 11 on my relays are 4 four different speeds on the motor 
I should remove pwm from the code, but its difficult too remove it all from the code when its in some of the calculations. Yeah, the sensor is not a writing error, because I tried to use one arduino on one motor. Left and Right is the same motor. But if i get two gaps to work, i can use two motors on each arduino any suggestions how to get gap in the function you mentioned?