Controller: Implement mecanum velocity solver