Fix mecanum implementation