diff --git a/BrickController2/BrickController2.Tests/DeviceManagement/IO/OutputValuesGroupTests.cs b/BrickController2/BrickController2.Tests/DeviceManagement/IO/OutputValuesGroupTests.cs index bc7f29b4..288e4599 100644 --- a/BrickController2/BrickController2.Tests/DeviceManagement/IO/OutputValuesGroupTests.cs +++ b/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(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(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(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(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(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"); + } } diff --git a/BrickController2/BrickController2/DeviceManagement/IO/OutputValuesGroup.cs b/BrickController2/BrickController2/DeviceManagement/IO/OutputValuesGroup.cs index a7448697..b39a19a2 100644 --- a/BrickController2/BrickController2/DeviceManagement/IO/OutputValuesGroup.cs +++ b/BrickController2/BrickController2/DeviceManagement/IO/OutputValuesGroup.cs @@ -41,6 +41,25 @@ public bool SetOutput(int channel, TValue value) return false; } + /// + /// Accumulates the value into the channel's current output. + /// Non-zero values are added; zero values are ignored (input released). + /// + 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) diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs index acc2f9b2..4f6e93b3 100644 --- a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs @@ -45,6 +45,7 @@ public override Task ConnectAsync( { // reset output values & positions ResetOutputValues(); + InitializeChannelInfo(); ChannelConfigs.Clear(); // Initialize configuration per channel @@ -73,7 +74,7 @@ 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(); @@ -81,6 +82,11 @@ protected virtual void ResetOutputValues() AttachedPeripherals.Clear(); } + protected virtual void ResetOutputValues() + { + // any output values to be sent should be reset + } + protected override async Task ProcessOutputsAsync(CancellationToken token) { try diff --git a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs index 1531079b..a85664d7 100644 --- a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs @@ -29,9 +29,9 @@ internal class TechnicMoveDevice : WirelessProtocolBasedDevice private readonly OutputValuesGroup _outputValues = new(9); private readonly OutputValuesGroup _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? _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 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 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 SendOutputValuesAsync(CancellationToken token) @@ -214,6 +232,7 @@ protected override async Task SendOutputValuesAsync(CancellationToken toke foreach (KeyValuePair change in changes) { var value = ToByte(change.Value); + var channelOutputType = GetOutputType(change.Key); result = change.Key switch { @@ -221,7 +240,9 @@ protected override async Task SendOutputValuesAsync(CancellationToken toke >= 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 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 SendPortOutput_ValueAsync(int channel, byte value, Cance return WriteAsync(cmd, token); } + private ValueTask 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 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 SendAllOutputValuesAsync(Half value, CancellationToken token) { var rawValue = ToByte(value); @@ -431,10 +479,13 @@ private async ValueTask 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; diff --git a/BrickController2/BrickController2/UI/ViewModels/DevicePageViewModel.cs b/BrickController2/BrickController2/UI/ViewModels/DevicePageViewModel.cs index 671e18fd..dd9ca0ac 100644 --- a/BrickController2/BrickController2/UI/ViewModels/DevicePageViewModel.cs +++ b/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