Thank you.
I changed the code.
Now i have a other problem:
I've tried to tune the motor but i get an following error if i set PID P= 0,05, 1, 20, or 100 or something else. And when i set P=0,00005 or something like this, the motor hold the position, but vibrates extremely.
Can this issue came from the direction signal? I've connected the mesa output directly to the servo input and the input on the servo works. Maybe is the input of the servo too slow?