Add support for mecanum drive