LEGO Technic Move hub - add servo/stepper support

BrickController2/BrickController2.Tests/DeviceManagement/IO/OutputValuesGroupTests.cs

@@ -138,4 +138,78 @@ public void Commit_ExistingChange_NoChangeIsReportedThen()
         group.TryGetChanges(out var values).Should().BeFalse();
         values.Should().NotBeNull().And.BeEmpty();
     }
+
+    [Fact]
+    public void AccumulateOutput_ZeroValue_ReturnsFalseAndNoChangeReported()
+    {
+        // Arrange
+        var group = new OutputValuesGroup<float>(3);
+
+        // Act
+        var result = group.AccumulateOutput(1, 0.0f);
+
+        // Assert
+        result.Should().BeFalse();
+        group.TryGetValues(out _).Should().BeFalse();
+    }
+
+    [Fact]
+    public void AccumulateOutput_NonZeroValue_ReturnsTrueAndChangeReported()
+    {
+        // Arrange
+        var group = new OutputValuesGroup<float>(3);
+
+        // Act
+        var result = group.AccumulateOutput(1, 2.5f);
+
+        // Assert
+        result.Should().BeTrue();
+        group.TryGetValues(out var values).Should().BeTrue();
+        values[1].Should().Be(2.5f);
+    }
+
+    [Fact]
+    public void AccumulateOutput_NegativeValue_AccumulatesNegatively()
+    {
+        // Arrange
+        var group = new OutputValuesGroup<int>(2);
+        group.AccumulateOutput(1, 10);
+        group.TryGetValues(out _);
+        group.Commit();
+
+        // Act
+        group.AccumulateOutput(1, -4);
+
+        // Assert
+        group.TryGetValues(out var values).Should().BeTrue();
+        values[1].Should().Be(6);
+    }
+
+    [Fact]
+    public void AccumulateOutput_AfterSetOutput_AddsOnTop()
+    {
+        // Arrange
+        var group = new OutputValuesGroup<float>(2);
+        group.SetOutput(0, 1.0f);
+
+        // Act
+        group.AccumulateOutput(0, 2.0f);
+
+        // Assert
+        group.TryGetValues(out var values).Should().BeTrue();
+        values[0].Should().Be(3.0f);
+    }
+
+    [Fact]
+    public void AccumulateOutput_NonZeroValue_SetsMaxSendAttempts()
+    {
+        // Arrange
+        var group = new OutputValuesGroup<int>(1);
+        group.AccumulateOutput(0, 1);
+
+        // Assert — change must be reported one time, then stop
+        group.TryGetValues(out _).Should().BeTrue($"attempt 1 should return true");
+        group.Commit();
+        group.TryGetValues(out _).Should().BeFalse("no more send attempts should remain");
+    }
 }

BrickController2/BrickController2/DeviceManagement/IO/OutputValuesGroup.cs

@@ -41,6 +41,25 @@ public bool SetOutput(int channel, TValue value)
         return false;
     }
 
+    /// <summary>
+    /// Accumulates the value into the channel's current output.
+    /// Non-zero values are added; zero values are ignored (input released).
+    /// </summary>
+    public bool AccumulateOutput(int channel, TValue value)
+    {
+        if (value == TValue.Zero)
+        {
+            return false;
+        }
+
+        lock (_outputLock)
+        {
+            _outputValues[channel] += value;
+            _sendAttemptsLeft = MAX_SEND_ATTEMPTS;
+            return true;
+        }
+    }
+
     public void Initialize()
     {
         lock (_outputLock)

BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs

@@ -45,6 +45,7 @@ public override Task<DeviceConnectionResult> ConnectAsync(
     {
         // reset output values & positions
         ResetOutputValues();
+        InitializeChannelInfo();
         ChannelConfigs.Clear();
 
         // Initialize configuration per channel
@@ -73,14 +74,19 @@ protected virtual bool TryGetChannelIndex(byte portId, out int channelIndex)
         return portId < NumberOfChannels;
     }
 
-    protected virtual void ResetOutputValues()
+    protected virtual void InitializeChannelInfo()
     {
         // reset status values & positions
         ChannelAbsPositions.Clear();
         ChannelRelativePositions.Clear();
         AttachedPeripherals.Clear();
     }
 
+    protected virtual void ResetOutputValues()
+    {
+        // any output values to be sent should be reset
+    }
+
     protected override async Task ProcessOutputsAsync(CancellationToken token)
     {
         try

BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs

@@ -29,9 +29,9 @@ internal class TechnicMoveDevice : WirelessProtocolBasedDevice
 
         private readonly OutputValuesGroup<Half> _outputValues = new(9);
         private readonly OutputValuesGroup<Half> _playVmValues = new(2);
+        private readonly int[] _calibratedZeroAngles = new int[3]; // zero ABS angles for steering ABC channels in non PLAYVM mode
 
         private bool _applyPlayVmMode;
-        private int _calibratedZeroAngle; // zero ABS angle for steering C channel in non PLAYVM mode
         private TaskCompletionSource<bool>? _playVmCalibrationTcs;
 
         public TechnicMoveDevice(string name,
@@ -51,18 +51,22 @@ public TechnicMoveDevice(string name,
         public bool EnablePlayVmMode => GetSettingValue(EnablePlayVmSettingName, true);
 
         public override bool CanAutoCalibrateOutput(int channel) => false;
-        public override bool CanResetOutput(int channel) => EnablePlayVmMode && channel == CHANNEL_C;
+        public override bool CanResetOutput(int channel) => channel == CHANNEL_C; // only C channel supports reset
 
-        public override bool CanChangeMaxServoAngle(int channel) => false;
+        public override bool CanChangeMaxServoAngle(int channel)
+            => !EnablePlayVmMode && channel == CHANNEL_C;  // standard mode - C channel only
 
         public override bool IsOutputTypeSupported(int channel, ChannelOutputType outputType)
             => outputType switch
             {
                 // motor if not PLAYVM for all channels, if PLAYVM only for other channels than C channel
                 ChannelOutputType.NormalMotor => !EnablePlayVmMode || channel != CHANNEL_C,
-                // servo only for PLAYVM and C channel
-                ChannelOutputType.ServoMotor => EnablePlayVmMode && channel == CHANNEL_C,
-                // other types (such as stepper) are not supported at all
+                // servo for C channel only
+                ChannelOutputType.ServoMotor => channel == CHANNEL_C,
+                // stepper for standard mode but A,B,C channels only
+                ChannelOutputType.StepperMotor => !EnablePlayVmMode && channel <= CHANNEL_C,
+
+                // other types are not supported at all
                 _ => false,
             };
 
@@ -77,7 +81,8 @@ public override Task<DeviceConnectionResult> ConnectAsync(bool reconnect, Action
 
         public override void SetOutput(int channel, float value)
         {
-            var rawValue = (Half)(100 * CutOutputValue(value));
+            var validatedValue = CutOutputValue(value);
+            var rawValue = (Half)(100 * validatedValue);
 
             _ = channel switch
             {
@@ -87,6 +92,9 @@ public override void SetOutput(int channel, float value)
                 CHANNEL_C when _applyPlayVmMode => _playVmValues.SetOutput(PLAYVM_CHANNEL_STEER, rawValue),
                 // Light channels 1 - 6 require absolute value
                 >= CHANNEL_1 and <= CHANNEL_6 => _outputValues.SetOutput(channel, Half.Abs(rawValue)),
+                // stepper channels accumulate the input value as a step coefficient
+                _ when channel >= CHANNEL_A && channel <= CHANNEL_C && GetOutputType(channel) == ChannelOutputType.StepperMotor
+                    => _outputValues.AccumulateOutput(channel, (Half)validatedValue),
                 // rest of ports: such as A, B or C when not in PLAYVM mode - use value as is
                 _ => _outputValues.SetOutput(CheckChannel(channel), rawValue)
             };
@@ -130,7 +138,7 @@ protected override async ValueTask BeforeDisconnectAsync(CancellationToken token
             {
                 // reset hub LED
                 var ledCmd = BuildPortOutput_DirectMode(PORT_HUB_LED, HUB_LED_MODE_COLOR, HUB_LED_COLOR_WHITE);
-                await WriteAsync(ledCmd, token: token);
+                await WriteNoResponseAsync(ledCmd, token: token);
                 await DelayAsync(token);
             }
         }
@@ -168,6 +176,11 @@ protected override async Task<bool> AfterConnectSetupAsync(bool requestDeviceInf
                         await SetupChannelForPortInformationAsync(channel, token);
                         await ResetServoAsync(channel, channelConfig.ServoBaseAngle, token);
                     }
+                    else if (channelConfig.OutputType == ChannelOutputType.StepperMotor)
+                    {
+                        // just configure angle reporting as for servo
+                        await SetupChannelForPortInformationAsync(channel, token);
+                    }
                 }
 
                 return result;
@@ -193,7 +206,12 @@ protected override void ResetOutputValues()
                 // otherwise all channels to be initialized
                 _outputValues.Initialize();
             }
-            _calibratedZeroAngle = default;
+        }
+
+        protected override void InitializeChannelInfo()
+        {
+            base.InitializeChannelInfo();
+            _calibratedZeroAngles.AsSpan().Clear();
         }
 
         protected override async Task<bool> SendOutputValuesAsync(CancellationToken token)
@@ -214,14 +232,17 @@ protected override async Task<bool> SendOutputValuesAsync(CancellationToken toke
                     foreach (KeyValuePair<int, Half> change in changes)
                     {
                         var value = ToByte(change.Value);
+                        var channelOutputType = GetOutputType(change.Key);
 
                         result = change.Key switch
                         {
                             // Light channels 1 - 6 require absolute value
                             >= CHANNEL_1 and <= CHANNEL_6 => await SendPortOutput_6LedAsync(ledIndex: change.Key - CHANNEL_1, value, token),
                             // all channels command - use original value
                             int.MaxValue => await SendAllOutputValuesAsync(change.Value, token),
-                            // classic output command for A, B, C channels
+                            CHANNEL_C when channelOutputType == ChannelOutputType.ServoMotor => await SendPortOutput_ServoAsync(change.Key, change.Value, token),
+                            // classic output command for A, B, C channels (with stepper support)
+                            <= CHANNEL_C when channelOutputType == ChannelOutputType.StepperMotor => await SendPortOutput_StepperAsync(change.Key, change.Value, token),
                             _ => await SendPortOutput_ValueAsync(change.Key, value, token),
                         };
 
@@ -289,11 +310,15 @@ await AwaitPositionChangeAsync(() => ChannelAbsPositions.Get(channel),
                 ChannelRelativePositions.ConsumeUpdate(channel); // clear existing value
                 var inputFormatForRelAngle = BuildPortInputFormatSetup(portId, PORT_MODE_2);
                 await WriteAsync(inputFormatForRelAngle, token);
+                await Task.Delay(50, token);
+
+                // explicitly request current POS value to guarantee an initial notification
+                await WriteAsync([0x05, 0x00, MESSAGE_TYPE_PORT_INFORMATION_REQUEST, portId, 0x00], token);
                 await AwaitPositionChangeAsync(() => ChannelRelativePositions.Get(channel),
                     TimeSpan.FromMilliseconds(250), token);
 
                 // need to recalculate zero angle to support ABS POS commands
-                _calibratedZeroAngle = CalculateCalibratedTarget(channel);
+                _calibratedZeroAngles[channel] = CalculateCalibratedTarget(channel);
 
                 return true;
 
@@ -362,7 +387,8 @@ private async ValueTask<bool> ResetServoAsync(int channel, int baseAngle, Cancel
                 {
                     // use simple Goto ABS position
                     var portId = GetPortId(channel);
-                    var servoCmd = BuildPortOutput_GotoAbsPosition(portId, _calibratedZeroAngle + baseAngle, servoSpeed: 0x28);
+                    var angle = _calibratedZeroAngles[channel] + baseAngle;
+                    var servoCmd = BuildPortOutput_GotoAbsPosition(portId, angle, servoSpeed: 0x28);
                     await WriteAsync(servoCmd, token: token);
 
                     // Wait for position to stabilize before allowing the output loop to start
@@ -421,6 +447,28 @@ private ValueTask<bool> SendPortOutput_ValueAsync(int channel, byte value, Cance
             return WriteAsync(cmd, token);
         }
 
+        private ValueTask<bool> SendPortOutput_ServoAsync(int channel, Half value, CancellationToken token)
+        {
+            var portId = GetPortId(channel);
+            // in non PLAYVM mode, need to apply calibrated base angle as offset to reach correct position
+            var servoAngle = (int)value * GetMaxServoAngle(channel) / 100;
+            var absPosition = _calibratedZeroAngles[channel] + ChannelConfigs.Get(channel).ServoBaseAngle + servoAngle;
+            var cmd = BuildPortOutput_GotoAbsPosition(portId, absPosition, servoSpeed: 50);
+            return WriteAsync(cmd, token);
+        }
+
+        private ValueTask<bool> SendPortOutput_StepperAsync(int channel, Half value, CancellationToken token)
+        {
+            // value is the accumulated step coefficient from SetOutput
+            var targetPosition = _calibratedZeroAngles[channel]
+                + (int)value * ChannelConfigs.Get(channel).StepperAngle;
+
+            var portId = GetPortId(channel);
+            var servoSpeed = channel == CHANNEL_C ? (byte)50 : (byte)30;
+            var cmd = BuildPortOutput_GotoAbsPosition(portId, targetPosition, servoSpeed);
+            return WriteAsync(cmd, token);
+        }
+
         private async ValueTask<bool> SendAllOutputValuesAsync(Half value, CancellationToken token)
         {
             var rawValue = ToByte(value);
@@ -431,10 +479,13 @@ private async ValueTask<bool> SendAllOutputValuesAsync(Half value, CancellationT
             foreach (var channel in new[] { CHANNEL_A, CHANNEL_B, CHANNEL_C })
             {
                 var outputType = GetOutputType(channel);
-                result = result && outputType switch
+                result = result && await (outputType switch
                 {
-                    _ => await SendPortOutput_ValueAsync(channel, rawValue, token),
-                };
+                    ChannelOutputType.ServoMotor => SendPortOutput_ServoAsync(channel, value, token),
+                    ChannelOutputType.StepperMotor => SendPortOutput_StepperAsync(channel, value, token),
+                    _ => SendPortOutput_ValueAsync(channel, rawValue, token),
+                });
+
             }
 
             return result;

BrickController2/BrickController2/UI/ViewModels/DevicePageViewModel.cs

@@ -337,7 +337,7 @@ await _dialogService.ShowProgressDialogAsync(
                         async (progressDialog, token) =>
                         {
                             // send command and later cancel connection
-                            await Device.ActiveShelfModeAsync();
+                            await Device.ActiveShelfModeAsync(token);
                             _connectionTokenSource?.Cancel();
                             // disconnection is expected to be triggered by Back
                             await Task.Delay(500, DisappearingToken);
@@ -435,6 +435,10 @@ private void UpdateCommandsAvailability()
             OpenDeviceSettingsPageCommand.RaiseCanExecuteChanged();
             // to ensure that servo/stepper commands are enabled / disabled properly
             RaisePropertyChanged(nameof(IsServoOrStepperSupported));
+            foreach (var output in DeviceOutputs)
+            {
+                output.UpdateCommandsAvailability();
+            }
         }
 
         private void SetBuWizzOutputLevel(int level)
@@ -482,6 +486,11 @@ public int Output
             public ICommand TouchUpCommand { get; }
             public ICommand TestServoStepperCommand { get; }
 
+            internal void UpdateCommandsAvailability()
+            {
+                TestServoStepperCommand.RaiseCanExecuteChanged();
+            }
+
             private async Task OpenChannelSetupAsync()
             {
                 // enforce disconnection before navigating to channel setup to avoid connection conflicts