Add wheel encoders support + fix major bug
Add small fixes to controller