Encoder control update

idk about the modularization, just merge this code into the main code.
If there are any problems, let me know