First working version of BrusaMotorController !