Fixed compilation error when compiling without servo
Fixed compilation error when compiled with 2 motors