From 4d66de2c41105dc252ed51546e22b2648b6e819e Mon Sep 17 00:00:00 2001 From: Vit Nemecky Date: Wed, 1 Apr 2026 23:43:52 +0200 Subject: [PATCH 01/29] Technic Move Hub improvements --- .../DeviceManagement/ControlPlusDevice.cs | 156 ++---------------- .../Lego/ControlPlusDeviceBase.cs | 142 ++++++++++++++++ .../DeviceManagement/Lego/RemoteControl.cs | 35 +--- 3 files changed, 164 insertions(+), 169 deletions(-) create mode 100644 BrickController2/BrickController2/DeviceManagement/Lego/ControlPlusDeviceBase.cs diff --git a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs index f7bf38df..a5c90c42 100644 --- a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs @@ -1,9 +1,9 @@ using BrickController2.CreationManagement; +using BrickController2.DeviceManagement.Lego; using BrickController2.PlatformServices.BluetoothLE; using System; using System.Collections.Generic; using System.Diagnostics; -using System.Linq; using System.Threading; using System.Threading.Tasks; @@ -11,7 +11,7 @@ namespace BrickController2.DeviceManagement { - internal abstract class ControlPlusDevice : BluetoothDevice + internal abstract class ControlPlusDevice : ControlPlusDeviceBase { private const int MAX_SEND_ATTEMPTS = 10; @@ -40,8 +40,6 @@ internal abstract class ControlPlusDevice : BluetoothDevice private readonly object _positionLock = new object(); private readonly Stopwatch _lastSent_NormalMotor = new Stopwatch(); - private IGattCharacteristic? _characteristic; - public ControlPlusDevice(string name, string address, IDeviceRepository deviceRepository, IBluetoothLEService bleService) : base(name, address, deviceRepository, bleService) { @@ -166,54 +164,6 @@ public override async Task ResetOutputAsync(int channel, float value, Cancellati return await AutoCalibrateServoAsync(channel, token); } - protected override async Task ValidateServicesAsync(IEnumerable? services, CancellationToken token) - { - var service = services?.FirstOrDefault(s => s.Uuid == ServiceUuid); - _characteristic = service?.Characteristics?.FirstOrDefault(c => c.Uuid == CharacteristicUuid); - - if (_characteristic is not null) - { - return await _bleDevice!.EnableNotificationAsync(_characteristic, token); - } - - return false; - } - - protected override async ValueTask BeforeDisconnectAsync(CancellationToken token) - { - // reset notifications (if possible) - if (_characteristic != null && _bleDevice != null) - { - await _bleDevice.DisableNotificationAsync(_characteristic, token); - } - } - - protected override void BeforeDisconnectCleanup() - { - _characteristic = null; - } - - protected async Task WriteNoResponseAsync(byte[] data, bool withSendDelay = false, CancellationToken token = default) - { - var result = await _bleDevice!.WriteNoResponseAsync(_characteristic!, data, token); - - if (withSendDelay) - { - await Task.Delay(SEND_DELAY, token); - } - return result; - } - - protected Task WriteAsync(byte[] data, CancellationToken token = default) - => _bleDevice!.WriteAsync(_characteristic!, data, token); - - protected virtual byte GetPortId(int channelIndex) => (byte)channelIndex; - protected virtual bool TryGetChannelIndex(byte portId, out int channelIndex) - { - channelIndex = portId; - return portId < NumberOfChannels; - } - protected virtual byte GetChannelValue(int value) // calculate raw motor value => (byte)(value < 0 ? (255 + value) : value); @@ -262,7 +212,7 @@ protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[] switch (messageCode) { - case 0x01: // Hub properties + case MESSAGE_TYPE_HUB_PROPERTIES: // Hub properties ProcessHubPropertyData(data); break; @@ -523,7 +473,7 @@ private async Task SendOutputValueAsync(int channel, CancellationToken tok _lastSent_NormalMotor.Elapsed > ResendDelay_NormalMotor) { var outputCmd = GetOutputCommand(channel, v); - if (await _bleDevice!.WriteAsync(_characteristic!, outputCmd, token)) + if (await WriteAsync(outputCmd, token)) { _lastSent_NormalMotor.Restart(); @@ -556,7 +506,7 @@ private async Task SendOutputValueVirtualAsync(int virtualChannel, int cha _virtualPortSendBuffer[6] = (byte)(value1 < 0 ? (255 + value1) : value1); _virtualPortSendBuffer[7] = (byte)(value2 < 0 ? (255 + value2) : value2); - if (await _bleDevice!.WriteNoResponseAsync(_characteristic!, _virtualPortSendBuffer, token)) + if (await WriteAsync(_virtualPortSendBuffer, token)) { _lastOutputValues[channel1] = value1; _lastOutputValues[channel2] = value2; @@ -602,7 +552,7 @@ private async Task SendServoOutputValueAsync(int channel, CancellationToke } var servoCmd = GetServoCommand(channel, servoValue, servoSpeed); - if (await _bleDevice!.WriteAsync(_characteristic!, servoCmd, token)) + if (await WriteAsync(servoCmd, token)) { _lastOutputValues[channel] = v; ResetSendAttemps(channel, 0); @@ -646,7 +596,7 @@ private async Task SendStepperOutputValueAsync(int channel, CancellationTo if (v != _lastOutputValues[channel] && Math.Abs(v) == 100) { - if (await _bleDevice!.WriteAsync(_characteristic!, _stepperSendBuffer, token)) + if (await WriteAsync(_stepperSendBuffer, token)) { _lastOutputValues[channel] = v; ResetSendAttemps(channel, 0); @@ -683,15 +633,15 @@ protected virtual async Task SetupChannelForPortInformationAsync(int chann var unlockAndEnableBuffer = new byte[] { 0x05, 0x00, 0x42, portId, 0x03 }; var result = true; - result = result && await _bleDevice!.WriteAsync(_characteristic!, lockBuffer, token); + result = result && await WriteAsync(lockBuffer, token); await Task.Delay(20, token); - result = result && await _bleDevice!.WriteAsync(_characteristic!, inputFormatForAbsAngleBuffer, token); + result = result && await WriteAsync(inputFormatForAbsAngleBuffer, token); await Task.Delay(20, token); - result = result && await _bleDevice!.WriteAsync(_characteristic!, inputFormatForRelAngleBuffer, token); + result = result && await WriteAsync(inputFormatForRelAngleBuffer, token); await Task.Delay(20, token); - result = result && await _bleDevice!.WriteAsync(_characteristic!, modeAndDataSetBuffer, token); + result = result && await WriteAsync(modeAndDataSetBuffer, token); await Task.Delay(20, token); - result = result && await _bleDevice!.WriteAsync(_characteristic!, unlockAndEnableBuffer, token); + result = result && await WriteAsync(unlockAndEnableBuffer, token); return result; } @@ -724,7 +674,7 @@ protected virtual async Task ResetServoAsync(int channel, int baseAngle, C var diff = Math.Abs(NormalizeAngle(_absolutePositions[channel] - baseAngle)); if (diff > 5) { - // Can't reset to base angle, rebease to current position not to stress the plastic + // Can't reset to base angle, rebase to current position not to stress the plastic result = result && await ResetAsync(channel, 0, token); result = result && await StopAsync(channel, token); result = result && await TurnAsync(channel, 0, 40, token); @@ -838,7 +788,7 @@ private static async Task WaitForStablePositionAsync(TimeSpan timeout, Func } } - private int NormalizeAngle(int angle) + private static int NormalizeAngle(int angle) { if (angle >= 180) { @@ -889,7 +839,7 @@ private int CalculateServoSpeed(int channel, int targetAngle) private Task StopAsync(int channel, CancellationToken token) { var portId = GetPortId(channel); - return _bleDevice!.WriteAsync(_characteristic!, new byte[] { 0x08, 0x00, 0x81, portId, 0x11, 0x51, 0x00, 0x00 }, token); + return _bleDevice!.WriteAsync(Characteristic!, new byte[] { 0x08, 0x00, 0x81, portId, 0x11, 0x51, 0x00, 0x00 }, token); } private Task TurnAsync(int channel, int angle, int speed, CancellationToken token) @@ -902,7 +852,7 @@ private Task TurnAsync(int channel, int angle, int speed, CancellationToke var a2 = (byte)((angle >> 16) & 0xff); var a3 = (byte)((angle >> 24) & 0xff); - return _bleDevice!.WriteAsync(_characteristic!, new byte[] { 0x0e, 0x00, 0x81, portId, 0x11, 0x0d, a0, a1, a2, a3, (byte)speed, 0x64, 0x7e, 0x00 }, token); + return _bleDevice!.WriteAsync(Characteristic!, new byte[] { 0x0e, 0x00, 0x81, portId, 0x11, 0x0d, a0, a1, a2, a3, (byte)speed, 0x64, 0x7e, 0x00 }, token); } private Task ResetAsync(int channel, int angle, CancellationToken token) @@ -915,79 +865,7 @@ private Task ResetAsync(int channel, int angle, CancellationToken token) var a2 = (byte)((angle >> 16) & 0xff); var a3 = (byte)((angle >> 24) & 0xff); - return _bleDevice!.WriteAsync(_characteristic!, new byte[] { 0x0b, 0x00, 0x81, portId, 0x11, 0x51, 0x02, a0, a1, a2, a3 }, token); - } - - private async Task RequestHubPropertiesAsync(CancellationToken token) - { - try - { - // Request firmware version - await Task.Delay(TimeSpan.FromMilliseconds(300), token); - await _bleDevice!.WriteAsync(_characteristic!, new byte[] { 0x05, 0x00, 0x01, 0x03, 0x05 }, token); - var data = await _bleDevice!.ReadAsync(_characteristic!, token); - ProcessHubPropertyData(data); - - // Request hardware version - await Task.Delay(TimeSpan.FromMilliseconds(300), token); - await _bleDevice!.WriteAsync(_characteristic!, new byte[] { 0x05, 0x00, 0x01, 0x04, 0x05 }, token); - data = await _bleDevice!.ReadAsync(_characteristic!, token); - ProcessHubPropertyData(data); - - // Request battery voltage - await Task.Delay(TimeSpan.FromMilliseconds(300), token); - await _bleDevice!.WriteAsync(_characteristic!, new byte[] { 0x05, 0x00, 0x01, 0x06, 0x05 }, token); - data = await _bleDevice!.ReadAsync(_characteristic!, token); - ProcessHubPropertyData(data); - } - catch { } - } - - private void ProcessHubPropertyData(byte[]? data) - { - try - { - if (data is null || data.Length < 6) - { - return; - } - - var dataLength = data[0]; - var messageId = data[2]; - var propertyId = data[3]; - var propertyOperation = data[4]; - - if (messageId != MESSAGE_TYPE_HUB_PROPERTIES || propertyOperation != HUB_PROPERTY_OPERATION_UPDATE) - { - // Operation is not 'update' - return; - } - - switch (propertyId) - { - case HUB_PROPERTY_FW_VERSION: // FW version - var firmwareVersion = GetVersionString(data.AsSpan(5)); - if (!string.IsNullOrEmpty(firmwareVersion)) - { - FirmwareVersion = firmwareVersion; - } - break; - - case HUB_PROPERTY_HW_VERSION: // HW version - var hardwareVersion = GetVersionString(data.AsSpan(5)); - if (!string.IsNullOrEmpty(hardwareVersion)) - { - HardwareVersion = hardwareVersion; - } - break; - - case HUB_PROPERTY_VOLTAGE: // Battery voltage - var voltage = data[5]; - BatteryVoltage = voltage.ToString("F0"); - break; - } - } - catch { } + return _bleDevice!.WriteAsync(Characteristic!, new byte[] { 0x0b, 0x00, 0x81, portId, 0x11, 0x51, 0x02, a0, a1, a2, a3 }, token); } } } diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/ControlPlusDeviceBase.cs b/BrickController2/BrickController2/DeviceManagement/Lego/ControlPlusDeviceBase.cs new file mode 100644 index 00000000..9c6272cb --- /dev/null +++ b/BrickController2/BrickController2/DeviceManagement/Lego/ControlPlusDeviceBase.cs @@ -0,0 +1,142 @@ +using BrickController2.PlatformServices.BluetoothLE; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Threading; +using System.Threading.Tasks; + + +using static BrickController2.Protocols.LegoWirelessProtocol; + +namespace BrickController2.DeviceManagement.Lego; + +internal abstract class ControlPlusDeviceBase : BluetoothDevice +{ + protected IGattCharacteristic? Characteristic; + + protected ControlPlusDeviceBase(string name, string address, IDeviceRepository deviceRepository, IBluetoothLEService bleService) + : base(name, address, deviceRepository, bleService) + { + } + + protected virtual byte GetPortId(int channelIndex) => (byte)channelIndex; + protected virtual bool TryGetChannelIndex(byte portId, out int channelIndex) + { + channelIndex = portId; + return portId < NumberOfChannels; + } + + protected override async Task ValidateServicesAsync(IEnumerable? services, CancellationToken token) + { + var service = services?.FirstOrDefault(s => s.Uuid == ServiceUuid); + Characteristic = service?.Characteristics?.FirstOrDefault(c => c.Uuid == CharacteristicUuid); + + if (Characteristic is not null) + { + return await _bleDevice!.EnableNotificationAsync(Characteristic, token); + } + + return false; + } + + protected override async ValueTask BeforeDisconnectAsync(CancellationToken token) + { + // reset notifications (if possible) + if (Characteristic != null && _bleDevice != null) + { + await _bleDevice.DisableNotificationAsync(Characteristic, token); + } + } + + protected override void BeforeDisconnectCleanup() + { + Characteristic = null; + } + + protected async ValueTask WriteNoResponseAsync(byte[] data, TimeSpan sentDelay, CancellationToken token = default) + { + var result = await _bleDevice!.WriteNoResponseAsync(Characteristic!, data, token); + await Task.Delay(sentDelay, token); + return result; + } + + protected async ValueTask WriteNoResponseAsync(byte[] data, CancellationToken token = default) + => await _bleDevice!.WriteNoResponseAsync(Characteristic!, data, token); + + protected async ValueTask WriteAsync(byte[] data, CancellationToken token = default) + => await _bleDevice!.WriteAsync(Characteristic!, data, token); + + protected async ValueTask RequestHubPropertiesAsync(CancellationToken token) + { + try + { + // Request firmware version + await RequestHubPropertyAsync(HUB_PROPERTY_FW_VERSION, token); + // Request hardware version + await RequestHubPropertyAsync(HUB_PROPERTY_HW_VERSION, token); + // Request battery voltage + await RequestHubPropertyAsync(HUB_PROPERTY_VOLTAGE, token); + } + catch { } + } + + protected async ValueTask RequestHubPropertyAsync(byte propertyId, CancellationToken token) + { + try + { + // Request firmware version + await Task.Delay(TimeSpan.FromMilliseconds(100), token); + await _bleDevice!.WriteAsync(Characteristic!, [0x05, 0x00, 0x01, propertyId, 0x05], token); + var data = await _bleDevice!.ReadAsync(Characteristic!, token); + ProcessHubPropertyData(data); + } + catch { } + } + + protected void ProcessHubPropertyData(ReadOnlySpan data) + { + try + { + if (data.Length < 6) + { + return; + } + + var dataLength = data[0]; + var messageId = data[2]; + var propertyId = data[3]; + var propertyOperation = data[4]; + + if (messageId != MESSAGE_TYPE_HUB_PROPERTIES || propertyOperation != HUB_PROPERTY_OPERATION_UPDATE) + { + // Operation is not 'update' + return; + } + + switch (propertyId) + { + case HUB_PROPERTY_FW_VERSION: // FW version + var firmwareVersion = GetVersionString(data.Slice(5)); + if (!string.IsNullOrEmpty(firmwareVersion)) + { + FirmwareVersion = firmwareVersion; + } + break; + + case HUB_PROPERTY_HW_VERSION: // HW version + var hardwareVersion = GetVersionString(data.Slice(5)); + if (!string.IsNullOrEmpty(hardwareVersion)) + { + HardwareVersion = hardwareVersion; + } + break; + + case HUB_PROPERTY_VOLTAGE: // Battery voltage + var voltage = data[5]; + BatteryVoltage = voltage.ToString("F0"); + break; + } + } + catch { } + } +} diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs b/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs index 9891d966..ccac8fdd 100644 --- a/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs +++ b/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs @@ -15,7 +15,7 @@ namespace BrickController2.DeviceManagement.Lego; /// /// Represents a LEGO® Powered Up 88010 Remote Control /// -internal class RemoteControl : BluetoothDevice +internal class RemoteControl : ControlPlusDeviceBase { private const string ENABLED_SETTING_NAME = "RemoteControlEnabled"; private const bool DEFAULT_ENABLED = false; @@ -64,19 +64,6 @@ internal void ResetEvents() => RaiseButtonEvents( protected override Task ProcessOutputsAsync(CancellationToken token) => Task.CompletedTask; - protected override async Task ValidateServicesAsync(IEnumerable? services, CancellationToken token) - { - var service = services?.FirstOrDefault(s => s.Uuid == ServiceUuid); - _characteristic = service?.Characteristics?.FirstOrDefault(c => c.Uuid == CharacteristicUuid); - - if (_characteristic is not null) - { - return await _bleDevice!.EnableNotificationAsync(_characteristic, token); - } - - return false; - } - protected override async Task AfterConnectSetupAsync(bool requestDeviceInformation, CancellationToken token) { // wait until ports finish communicating with the hub @@ -84,22 +71,15 @@ protected override async Task AfterConnectSetupAsync(bool requestDeviceInf if (requestDeviceInformation) { - // Request battery voltage - await _bleDevice!.WriteAsync(_characteristic!, [0x05, 0x00, 0x01, 0x06, 0x05], token); - await Task.Delay(TimeSpan.FromMilliseconds(50), token); + await RequestHubPropertiesAsync(token); } // setup ports - 0x04 - REMOTE_MODE_KEYS var remoteButtonA = BuildPortInputFormatSetup(REMOTE_BUTTONS_LEFT, REMOTE_MODE_KEYS, interval: 1); - await _bleDevice!.WriteAsync(_characteristic!, remoteButtonA, token); + await WriteAsync(remoteButtonA, token); var remoteButtonB = BuildPortInputFormatSetup(REMOTE_BUTTONS_RIGHT, REMOTE_MODE_KEYS, interval: 1); - return await _bleDevice!.WriteAsync(_characteristic!, remoteButtonB, token); - } - - protected override void BeforeDisconnectCleanup() - { - _characteristic = null; + return await WriteAsync(remoteButtonB, token); } protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[] data) @@ -114,12 +94,7 @@ protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[] switch (messageCode) { case MESSAGE_TYPE_HUB_PROPERTIES: // Hub properties - if (data.Length >= 6 && - data[3] == HUB_PROPERTY_VOLTAGE && - data[4] == HUB_PROPERTY_OPERATION_UPDATE) - { - BatteryVoltage = data[5].ToString("F0"); - } + ProcessHubPropertyData(data); break; case MESSAGE_TYPE_HW_NETWORK_COMMANDS: // HW network commands From 6b118eeb1bd51ee2cbebfb85cc406246c4a6596e Mon Sep 17 00:00:00 2001 From: Vit Nemecky Date: Thu, 2 Apr 2026 21:08:09 +0200 Subject: [PATCH 02/29] nitpicks --- .../BrickController2/DeviceManagement/ControlPlusDevice.cs | 2 -- .../DeviceManagement/Lego/ControlPlusDeviceBase.cs | 2 ++ .../BrickController2/DeviceManagement/Lego/RemoteControl.cs | 3 --- 3 files changed, 2 insertions(+), 5 deletions(-) diff --git a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs index a5c90c42..24eb482e 100644 --- a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs @@ -58,8 +58,6 @@ public ControlPlusDevice(string name, string address, IDeviceRepository deviceRe _positionUpdateTimes = new DateTime[NumberOfChannels]; } - public override string BatteryVoltageSign => "%"; - public override bool IsOutputTypeSupported(int channel, ChannelOutputType outputType) // support all output types on all channels => true; diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/ControlPlusDeviceBase.cs b/BrickController2/BrickController2/DeviceManagement/Lego/ControlPlusDeviceBase.cs index 9c6272cb..1217612a 100644 --- a/BrickController2/BrickController2/DeviceManagement/Lego/ControlPlusDeviceBase.cs +++ b/BrickController2/BrickController2/DeviceManagement/Lego/ControlPlusDeviceBase.cs @@ -19,6 +19,8 @@ protected ControlPlusDeviceBase(string name, string address, IDeviceRepository d { } + public override string BatteryVoltageSign => "%"; + protected virtual byte GetPortId(int channelIndex) => (byte)channelIndex; protected virtual bool TryGetChannelIndex(byte portId, out int channelIndex) { diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs b/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs index ccac8fdd..aac682cb 100644 --- a/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs +++ b/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs @@ -20,7 +20,6 @@ internal class RemoteControl : ControlPlusDeviceBase private const string ENABLED_SETTING_NAME = "RemoteControlEnabled"; private const bool DEFAULT_ENABLED = false; - private IGattCharacteristic? _characteristic; private InputDeviceBase? _inputController; public RemoteControl(string name, string address, IEnumerable settings, IDeviceRepository deviceRepository, IBluetoothLEService bleService) @@ -33,8 +32,6 @@ public RemoteControl(string name, string address, IEnumerable sett public override int NumberOfChannels => 0; - public override string BatteryVoltageSign => "%"; - public bool IsEnabled => GetSettingValue(ENABLED_SETTING_NAME, DEFAULT_ENABLED); protected override bool AutoConnectOnFirstConnect => false; From 089422d172ffb10ec6f2a604d28568db367d3a9d Mon Sep 17 00:00:00 2001 From: Vit Nemecky Date: Thu, 2 Apr 2026 21:08:20 +0200 Subject: [PATCH 03/29] work save --- .../DeviceManagement/TechnicMoveDevice.cs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs index fbe36cff..03a59531 100644 --- a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs @@ -37,17 +37,17 @@ 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; - public override bool CanChangeMaxServoAngle(int channel) => false; + public override bool CanChangeMaxServoAngle(int channel) => !EnablePlayVmMode && channel != CHANNEL_C; 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, + // servo for both PLAYVM and normal mode but C channel only + ChannelOutputType.ServoMotor => channel == CHANNEL_C, // other types (such as stepper) are not supported at all _ => false, }; From 1d9a26339097060d0d7bbb245ce22853ca3bc883 Mon Sep 17 00:00:00 2001 From: Vit Nemecky Date: Fri, 3 Apr 2026 00:11:29 +0200 Subject: [PATCH 04/29] try servo via GOTO ABS --- .../DeviceManagement/ControlPlusDevice.cs | 95 ++++++++----------- .../DeviceManagement/TechnicMoveDevice.cs | 39 ++++++-- .../Protocols/LegoWirelessProtocol.cs | 14 +++ 3 files changed, 88 insertions(+), 60 deletions(-) diff --git a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs index 24eb482e..8f417ec8 100644 --- a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs @@ -30,7 +30,7 @@ internal abstract class ControlPlusDevice : ControlPlusDeviceBase private readonly ChannelOutputType[] _channelOutputTypes; private readonly int[] _maxServoAngles; - private readonly int[] _servoBaseAngles; + protected readonly int[] _servoBaseAngles; private readonly int[] _stepperAngles; private readonly int[] _absolutePositions; @@ -282,44 +282,32 @@ protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[] } var modeMask = data[5]; - var dataIndex = 6; + var currentData = data.AsSpan(6); // start at index 6 if ((modeMask & 0x01) != 0) { - var absPosBuffer = BitConverter.IsLittleEndian ? - new byte[] { data[dataIndex + 0], data[dataIndex + 1] } : - new byte[] { data[dataIndex + 1], data[dataIndex + 0] }; - - var absPosition = BitConverter.ToInt16(absPosBuffer, 0); + var absPosition = ToInt16(currentData); _absolutePositions[channel] = absPosition; - dataIndex += 2; + currentData = currentData.Slice(2); } if ((modeMask & 0x02) != 0) { // TODO: Read the post value format response and determine the value length accordingly - if ((dataIndex + 3) < data.Length) + if (currentData.Length >= 4) { - var relPosBuffer = BitConverter.IsLittleEndian ? - new byte[] { data[dataIndex + 0], data[dataIndex + 1], data[dataIndex + 2], data[dataIndex + 3] } : - new byte[] { data[dataIndex + 3], data[dataIndex + 2], data[dataIndex + 1], data[dataIndex + 0] }; - - var relPosition = BitConverter.ToInt32(relPosBuffer, 0); + var relPosition = ToInt32(currentData); _relativePositions[channel] = relPosition; } - else if ((dataIndex + 1) < data.Length) + else if (currentData.Length >= 2) { - var relPosBuffer = BitConverter.IsLittleEndian ? - new byte[] { data[dataIndex + 0], data[dataIndex + 1] } : - new byte[] { data[dataIndex + 1], data[dataIndex + 0] }; - - var relPosition = BitConverter.ToInt16(relPosBuffer, 0); + var relPosition = ToInt16(currentData); _relativePositions[channel] = relPosition; } else { - _relativePositions[channel] = data[dataIndex]; + _relativePositions[channel] = currentData[0]; } _positionsUpdated[channel] = true; @@ -688,15 +676,38 @@ protected virtual async Task ResetServoAsync(int channel, int baseAngle, C } } - protected Task AwaitStableAbsPositionAsync(int channel, TimeSpan timeout, CancellationToken token) + protected int CalculateCalibratedTarget(int channel, int targetBaseAngle = 0) { - return WaitForStablePositionAsync(timeout, GetCurrentAbsPosition, token); + int currentAbsPos; + int currentRelativeAngle; - int GetCurrentAbsPosition() + lock (_positionLock) + { + currentAbsPos = _absolutePositions[channel]; + currentRelativeAngle = _relativePositions[channel]; + } + + // Normalize the hardware relative angle to a clean 0-359 range + // (Crucial if your motor firmware reports APOS as -180 to 179) + int normalizedRelative = ((currentRelativeAngle % 360) + 360) % 360; + int normalizedTarget = ((targetBaseAngle % 360) + 360) % 360; + + // Calculate the raw difference + normalize + int diff = NormalizeAngle(normalizedTarget - normalizedRelative); + + // Offset the current accumulated position by the physical difference + return currentAbsPos + diff; + } + + protected Task AwaitStableRelativePositionAsync(int channel, TimeSpan timeout, CancellationToken token) + { + return WaitForStablePositionAsync(timeout, GetCurrentRelativePosition, token); + + int GetCurrentRelativePosition() { lock (_positionLock) { - return _absolutePositions[channel]; + return _relativePositions[channel]; } } } @@ -786,21 +797,7 @@ private static async Task WaitForStablePositionAsync(TimeSpan timeout, Func } } - private static int NormalizeAngle(int angle) - { - if (angle >= 180) - { - return angle - (360 * ((angle + 180) / 360)); - } - else if (angle < -180) - { - return angle + (360 * ((180 - angle) / 360)); - } - - return angle; - } - - private int RoundAngleToNearest90(int angle) + private static int RoundAngleToNearest90(int angle) { angle = NormalizeAngle(angle); if (angle < -135) return -180; @@ -837,7 +834,7 @@ private int CalculateServoSpeed(int channel, int targetAngle) private Task StopAsync(int channel, CancellationToken token) { var portId = GetPortId(channel); - return _bleDevice!.WriteAsync(Characteristic!, new byte[] { 0x08, 0x00, 0x81, portId, 0x11, 0x51, 0x00, 0x00 }, token); + return _bleDevice!.WriteAsync(Characteristic!, [0x08, 0x00, PORT_OUTPUT_COMMAND, portId, 0x11, PORT_OUTPUT_SUBCOMMAND_WRITE_DIRECT, 0x00, 0x00], token); } private Task TurnAsync(int channel, int angle, int speed, CancellationToken token) @@ -845,12 +842,8 @@ private Task TurnAsync(int channel, int angle, int speed, CancellationToke angle = NormalizeAngle(angle); var portId = GetPortId(channel); - var a0 = (byte)(angle & 0xff); - var a1 = (byte)((angle >> 8) & 0xff); - var a2 = (byte)((angle >> 16) & 0xff); - var a3 = (byte)((angle >> 24) & 0xff); - - return _bleDevice!.WriteAsync(Characteristic!, new byte[] { 0x0e, 0x00, 0x81, portId, 0x11, 0x0d, a0, a1, a2, a3, (byte)speed, 0x64, 0x7e, 0x00 }, token); + ToBytes(angle, out var a0, out var a1, out var a2, out var a3); + return _bleDevice!.WriteAsync(Characteristic!, [0x0e, 0x00, PORT_OUTPUT_COMMAND, portId, 0x11, 0x0d, a0, a1, a2, a3, (byte)speed, 0x64, 0x7e, 0x00], token); } private Task ResetAsync(int channel, int angle, CancellationToken token) @@ -858,12 +851,8 @@ private Task ResetAsync(int channel, int angle, CancellationToken token) angle = NormalizeAngle(angle); var portId = GetPortId(channel); - var a0 = (byte)(angle & 0xff); - var a1 = (byte)((angle >> 8) & 0xff); - var a2 = (byte)((angle >> 16) & 0xff); - var a3 = (byte)((angle >> 24) & 0xff); - - return _bleDevice!.WriteAsync(Characteristic!, new byte[] { 0x0b, 0x00, 0x81, portId, 0x11, 0x51, 0x02, a0, a1, a2, a3 }, token); + ToBytes(angle, out var a0, out var a1, out var a2, out var a3); + return _bleDevice!.WriteAsync(Characteristic!, [0x0b, 0x00, PORT_OUTPUT_COMMAND, portId, 0x11, PORT_OUTPUT_SUBCOMMAND_WRITE_DIRECT, 0x02, a0, a1, a2, a3], token); } } } diff --git a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs index 03a59531..e0d9ed70 100644 --- a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs @@ -19,6 +19,7 @@ internal class TechnicMoveDevice : ControlPlusDevice private bool _applyPlayVmMode; private volatile byte _virtualMotorValue; + private int _calibratedZeroAngle; // zero ABS angle for steering C channel in non PLAYVM mode public TechnicMoveDevice(string name, string address, @@ -39,7 +40,7 @@ public TechnicMoveDevice(string name, public override bool CanAutoCalibrateOutput(int channel) => false; public override bool CanResetOutput(int channel) => channel == CHANNEL_C; - public override bool CanChangeMaxServoAngle(int channel) => !EnablePlayVmMode && channel != CHANNEL_C; + public override bool CanChangeMaxServoAngle(int channel) => !EnablePlayVmMode && channel == CHANNEL_C; public override bool IsOutputTypeSupported(int channel, ChannelOutputType outputType) => outputType switch @@ -145,7 +146,9 @@ protected override byte[] GetServoCommand(int channel, int servoValue, int servo } var portId = GetPortId(channel); - return BuildPortOutput_GotoAbsPosition(portId, servoValue, (byte)servoSpeed); + // in non PLAYVM mode, need to apply calibrated base angle as offset to reach correct position + var value = _calibratedZeroAngle + _servoBaseAngles[channel] + servoValue; + return BuildPortOutput_GotoAbsPosition(portId, value, (byte)servoSpeed); } protected override async Task AfterConnectSetupAsync(bool requestDeviceInformation, CancellationToken token) @@ -179,10 +182,32 @@ protected override async Task SetupChannelForPortInformationAsync(int chan { try { - // setup channel to report ABS position var portId = GetPortId(channel); - var inputFormatForAbsAngle = BuildPortInputFormatSetup(portId, PORT_MODE_3); - return await WriteAsync(inputFormatForAbsAngle, token); + var inputFormatForRelAngle = BuildPortInputFormatSetup(portId, PORT_MODE_2); + + if (_applyPlayVmMode) + { + // setup channel to report POS position regularly + return await WriteAsync(inputFormatForRelAngle, token); + } + + // setup channel to for APOS, but no notifications + var inputFormatForAbsAngle = BuildPortInputFormatSetup(portId, PORT_MODE_3, notification: PORT_VALUE_NOTIFICATION_DISABLED); + await WriteAsync(inputFormatForAbsAngle, token); + await Task.Delay(50, token); + + // query current APOS + await WriteAsync([0x05, 0x00, 0x21, portId, 0x00], token); + await Task.Delay(250, token); //TODO wait for change + + // setup channel to report POS position regularly + await WriteAsync(inputFormatForRelAngle, token); + await Task.Delay(250, token); //TODO wait for change + + // need to recalculate base angle to support ABS POS commands + _calibratedZeroAngle = CalculateCalibratedTarget(channel); + + return true; } catch { @@ -210,12 +235,12 @@ protected override async Task ResetServoAsync(int channel, int baseAngle, { // use simple Goto ABS position var portId = GetPortId(channel); - var servoCmd = BuildPortOutput_GotoAbsPosition(portId, baseAngle, servoSpeed: 0x28); + var servoCmd = BuildPortOutput_GotoAbsPosition(portId, _calibratedZeroAngle + baseAngle, servoSpeed: 0x28); await WriteAsync(servoCmd, token: token); } // need to wait till it completes - await AwaitStableAbsPositionAsync(channel, TimeSpan.FromSeconds(4), token); + await AwaitStableRelativePositionAsync(channel, TimeSpan.FromSeconds(4), token); return true; } diff --git a/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs b/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs index fec3dcab..6b9fd247 100644 --- a/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs +++ b/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs @@ -112,6 +112,20 @@ public static void ToBytes(int value, out byte b0, out byte b1, out byte b2, out public static short ToInt16(ReadOnlySpan value) => BinaryPrimitives.ReadInt16LittleEndian(value); public static int ToInt32(ReadOnlySpan value) => BinaryPrimitives.ReadInt32LittleEndian(value); + public static int NormalizeAngle(int angle) + { + if (angle >= 180) + { + return angle - (360 * ((angle + 180) / 360)); + } + else if (angle < -180) + { + return angle + (360 * ((180 - angle) / 360)); + } + + return angle; + } + public static string GetVersionString(ReadOnlySpan data) { if (data.Length < 4) From 239d7cfd2cd11b40d7cdea5511927933db0b9fcf Mon Sep 17 00:00:00 2001 From: Vit Nemecky Date: Fri, 3 Apr 2026 00:22:24 +0200 Subject: [PATCH 05/29] reset led in the ned --- .../DeviceManagement/TechnicMoveDevice.cs | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs index e0d9ed70..73df2a03 100644 --- a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs @@ -151,6 +151,19 @@ protected override byte[] GetServoCommand(int channel, int servoValue, int servo return BuildPortOutput_GotoAbsPosition(portId, value, (byte)servoSpeed); } + protected override async ValueTask BeforeDisconnectAsync(CancellationToken token) + { + await base.BeforeDisconnectAsync(token); + + if (_applyPlayVmMode) + { + // reset hub LED + var ledCmd = BuildPortOutput_DirectMode(PORT_HUB_LED, HUB_LED_MODE_COLOR, HUB_LED_COLOR_WHITE); + await WriteAsync(ledCmd, token: token); + await Task.Delay(20, token); + } + } + protected override async Task AfterConnectSetupAsync(bool requestDeviceInformation, CancellationToken token) { if (await base.AfterConnectSetupAsync(requestDeviceInformation, token)) From 2b905c126964ca38b96e62b8238c4b6c9f8c2634 Mon Sep 17 00:00:00 2001 From: Vit Nemecky Date: Sat, 4 Apr 2026 14:02:34 +0200 Subject: [PATCH 06/29] work save --- .../DeviceManagement/ControlPlusDevice.cs | 16 ----- .../DeviceManagement/IO/ChannelConfig.cs | 11 ++++ .../Lego/ControlPlusDeviceBase.cs | 59 ++++++++++++++++++- .../DeviceManagement/TechnicMoveDevice.cs | 2 +- 4 files changed, 70 insertions(+), 18 deletions(-) create mode 100644 BrickController2/BrickController2/DeviceManagement/IO/ChannelConfig.cs diff --git a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs index 8f417ec8..7a9607fb 100644 --- a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs @@ -121,22 +121,6 @@ public async override Task ConnectAsync( return await base.ConnectAsync(reconnect, onDeviceDisconnected, channelConfigurations, startOutputProcessing, requestDeviceInformation, token); } - public override void SetOutput(int channel, float value) - { - CheckChannel(channel); - value = CutOutputValue(value); - - var intValue = (int)(100 * value); - - lock (_outputLock) - { - if (_outputValues[channel] != intValue) - { - _outputValues[channel] = intValue; - _sendAttemptsLeft[channel] = MAX_SEND_ATTEMPTS; - } - } - } public override bool CanResetOutput(int channel) => true; diff --git a/BrickController2/BrickController2/DeviceManagement/IO/ChannelConfig.cs b/BrickController2/BrickController2/DeviceManagement/IO/ChannelConfig.cs new file mode 100644 index 00000000..6ae1a8cf --- /dev/null +++ b/BrickController2/BrickController2/DeviceManagement/IO/ChannelConfig.cs @@ -0,0 +1,11 @@ +using BrickController2.CreationManagement; + +namespace BrickController2.DeviceManagement.IO; + +internal readonly record struct ChannelConfig +{ + public ChannelOutputType ChannelOutputType { get; init; } + public int MaxServoAngle { get; init; } + public int ServoBaseAngle { get; init; } + public int StepperAngle { get; init; } +} diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/ControlPlusDeviceBase.cs b/BrickController2/BrickController2/DeviceManagement/Lego/ControlPlusDeviceBase.cs index 1217612a..0e9775a1 100644 --- a/BrickController2/BrickController2/DeviceManagement/Lego/ControlPlusDeviceBase.cs +++ b/BrickController2/BrickController2/DeviceManagement/Lego/ControlPlusDeviceBase.cs @@ -1,4 +1,6 @@ -using BrickController2.PlatformServices.BluetoothLE; +using BrickController2.CreationManagement; +using BrickController2.DeviceManagement.IO; +using BrickController2.PlatformServices.BluetoothLE; using System; using System.Collections.Generic; using System.Linq; @@ -12,15 +14,70 @@ namespace BrickController2.DeviceManagement.Lego; internal abstract class ControlPlusDeviceBase : BluetoothDevice { + protected readonly OutputValuesGroup OutputValues; + protected readonly ChannelConfig[] ChannelConfigs; + protected IGattCharacteristic? Characteristic; protected ControlPlusDeviceBase(string name, string address, IDeviceRepository deviceRepository, IBluetoothLEService bleService) : base(name, address, deviceRepository, bleService) { + OutputValues = new(NumberOfChannels); + ChannelConfigs = new ChannelConfig[NumberOfChannels]; } public override string BatteryVoltageSign => "%"; + public override void SetOutput(int channel, float value) + { + CheckChannel(channel); + + var percentValue = (Half)(100 * CutOutputValue(value)); + OutputValues.SetOutput(channel, percentValue); + } + + public override Task ConnectAsync( + bool reconnect, + Action onDeviceDisconnected, + IEnumerable channelConfigurations, + bool startOutputProcessing, + bool requestDeviceInformation, + CancellationToken token) + { + // reset output values + OutputValues.Clear(); + + //TODO angles + + // Initialize configuration per channel + + // build dictionary + var configs = channelConfigurations.ToDictionary(c => c.Channel, c => c); + + for (int i = 0; i < NumberOfChannels; i++) + { + configs.TryGetValue(i, out var config); + + ChannelConfigs[i] = config.ChannelOutputType switch + { + ChannelOutputType.ServoMotor => new() + { + ChannelOutputType = ChannelOutputType.ServoMotor, + MaxServoAngle = config.MaxServoAngle, + ServoBaseAngle = config.ServoBaseAngle + }, + ChannelOutputType.StepperMotor => new() + { + ChannelOutputType = ChannelOutputType.StepperMotor, + StepperAngle = config.StepperAngle + }, + _ => new() + }; + } + + return base.ConnectAsync(reconnect, onDeviceDisconnected, channelConfigurations, startOutputProcessing, requestDeviceInformation, token); + } + protected virtual byte GetPortId(int channelIndex) => (byte)channelIndex; protected virtual bool TryGetChannelIndex(byte portId, out int channelIndex) { diff --git a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs index 73df2a03..3441cbd3 100644 --- a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs @@ -217,7 +217,7 @@ protected override async Task SetupChannelForPortInformationAsync(int chan await WriteAsync(inputFormatForRelAngle, token); await Task.Delay(250, token); //TODO wait for change - // need to recalculate base angle to support ABS POS commands + // need to recalculate zero angle to support ABS POS commands _calibratedZeroAngle = CalculateCalibratedTarget(channel); return true; From 977da8264c4c0775d04eb2965de51485d2376e03 Mon Sep 17 00:00:00 2001 From: Vit Nemecky Date: Tue, 28 Apr 2026 23:53:18 +0200 Subject: [PATCH 07/29] keep current positions in base class --- .../DeviceManagement/ControlPlusDevice.cs | 304 ++++------------ .../DeviceManagement/IO/ChannelConfig.cs | 2 +- .../DeviceManagement/IO/ChannelStateStore.cs | 64 ++++ .../Lego/ChannelPositionState.cs | 31 ++ .../Lego/ControlPlusDeviceBase.cs | 201 ----------- .../DeviceManagement/Lego/RemoteControl.cs | 34 +- .../Lego/WirelessProtocolBasedDevice.cs | 336 ++++++++++++++++++ .../DeviceManagement/TechnicMoveDevice.cs | 260 ++++++++++---- .../Protocols/LegoWirelessProtocol.cs | 1 + 9 files changed, 702 insertions(+), 531 deletions(-) create mode 100644 BrickController2/BrickController2/DeviceManagement/IO/ChannelStateStore.cs create mode 100644 BrickController2/BrickController2/DeviceManagement/Lego/ChannelPositionState.cs delete mode 100644 BrickController2/BrickController2/DeviceManagement/Lego/ControlPlusDeviceBase.cs create mode 100644 BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs diff --git a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs index 7a9607fb..7e5e5e70 100644 --- a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs @@ -11,7 +11,7 @@ namespace BrickController2.DeviceManagement { - internal abstract class ControlPlusDevice : ControlPlusDeviceBase + internal abstract class ControlPlusDevice : WirelessProtocolBasedDevice { private const int MAX_SEND_ATTEMPTS = 10; @@ -28,16 +28,6 @@ internal abstract class ControlPlusDevice : ControlPlusDeviceBase private readonly int[] _sendAttemptsLeft; private readonly object _outputLock = new object(); - private readonly ChannelOutputType[] _channelOutputTypes; - private readonly int[] _maxServoAngles; - protected readonly int[] _servoBaseAngles; - private readonly int[] _stepperAngles; - - private readonly int[] _absolutePositions; - private readonly int[] _relativePositions; - private readonly bool[] _positionsUpdated; - private readonly DateTime[] _positionUpdateTimes; - private readonly object _positionLock = new object(); private readonly Stopwatch _lastSent_NormalMotor = new Stopwatch(); public ControlPlusDevice(string name, string address, IDeviceRepository deviceRepository, IBluetoothLEService bleService) @@ -46,16 +36,6 @@ public ControlPlusDevice(string name, string address, IDeviceRepository deviceRe _outputValues = new int[NumberOfChannels]; _lastOutputValues = new int[NumberOfChannels]; _sendAttemptsLeft = new int[NumberOfChannels]; - - _channelOutputTypes = new ChannelOutputType[NumberOfChannels]; - _maxServoAngles = new int[NumberOfChannels]; - _servoBaseAngles = new int[NumberOfChannels]; - _stepperAngles = new int[NumberOfChannels]; - - _absolutePositions = new int[NumberOfChannels]; - _relativePositions = new int[NumberOfChannels]; - _positionsUpdated = new bool[NumberOfChannels]; - _positionUpdateTimes = new DateTime[NumberOfChannels]; } public override bool IsOutputTypeSupported(int channel, ChannelOutputType outputType) @@ -79,45 +59,14 @@ public async override Task ConnectAsync( CancellationToken token) { lock (_outputLock) - lock (_positionLock) { for (int c = 0; c < NumberOfChannels; c++) { _outputValues[c] = 0; _lastOutputValues[c] = 0; - - _channelOutputTypes[c] = ChannelOutputType.NormalMotor; - _maxServoAngles[c] = 0; - _servoBaseAngles[c] = 0; - _stepperAngles[c] = 0; - - _absolutePositions[c] = 0; - _relativePositions[c] = 0; - _positionsUpdated[c] = false; - _positionUpdateTimes[c] = DateTime.MinValue; } } - foreach (var channelConfig in channelConfigurations) - { - _channelOutputTypes[channelConfig.Channel] = channelConfig.ChannelOutputType; - - switch (channelConfig.ChannelOutputType) - { - case ChannelOutputType.NormalMotor: - break; - - case ChannelOutputType.ServoMotor: - _maxServoAngles[channelConfig.Channel] = channelConfig.MaxServoAngle; - _servoBaseAngles[channelConfig.Channel] = channelConfig.ServoBaseAngle; - break; - - case ChannelOutputType.StepperMotor: - _stepperAngles[channelConfig.Channel] = channelConfig.StepperAngle; - break; - } - } - return await base.ConnectAsync(reconnect, onDeviceDisconnected, channelConfigurations, startOutputProcessing, requestDeviceInformation, token); } @@ -146,6 +95,35 @@ public override async Task ResetOutputAsync(int channel, float value, Cancellati return await AutoCalibrateServoAsync(channel, token); } + public override void SetOutput(int channel, float value) + { + CheckChannel(channel); + value = CutOutputValue(value); + + var intValue = (int)(100 * value); + + lock (_outputLock) + { + if (_outputValues[channel] != intValue) + { + _outputValues[channel] = intValue; + _sendAttemptsLeft[channel] = MAX_SEND_ATTEMPTS; + } + } + } + + protected override void ResetOutputValues() + { + lock (_outputLock) + { + for (int c = 0; c < NumberOfChannels; c++) + { + _outputValues[c] = 0; + _lastOutputValues[c] = 0; + } + } + } + protected virtual byte GetChannelValue(int value) // calculate raw motor value => (byte)(value < 0 ? (255 + value) : value); @@ -183,21 +161,10 @@ protected virtual byte[] GetServoCommand(int channel, int servoValue, int servoS return _servoSendBuffer; } - protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[] data) + protected override bool TryProcessMessageData(byte messageType, ReadOnlySpan data) { - if (characteristicGuid != CharacteristicUuid || data.Length < 4) - { - return; - } - - var messageCode = data[2]; - - switch (messageCode) + switch (messageType) { - case MESSAGE_TYPE_HUB_PROPERTIES: // Hub properties - ProcessHubPropertyData(data); - break; - case 0x02: // Hub actions DumpData("Hub actions", data); break; @@ -230,77 +197,6 @@ protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[] DumpData("Port mode information", data); break; - case 0x45: // Port value (single mode) - lock (_positionLock) - { - if (data.Length == 6) - { - // assume 16bit data is ABS - if (TryGetChannelIndex(portId: data[3], out var channel)) - { - var absPosition = ToInt16(data, 4); - _absolutePositions[channel] = absPosition; - } - } - else if (data.Length == 8) - { - // assume 32 bit data is REL - if (TryGetChannelIndex(portId: data[3], out var channel)) - { - var relPosition = ToInt32(data, 4); - _relativePositions[channel] = relPosition; - - _positionsUpdated[channel] = true; - _positionUpdateTimes[channel] = DateTime.Now; - } - } - } - break; - - case 0x46: // Port value (combined mode) - lock (_positionLock) - { - if (!TryGetChannelIndex(portId: data[3], out var channel)) - { - break; - } - - var modeMask = data[5]; - var currentData = data.AsSpan(6); // start at index 6 - - if ((modeMask & 0x01) != 0) - { - var absPosition = ToInt16(currentData); - _absolutePositions[channel] = absPosition; - - currentData = currentData.Slice(2); - } - - if ((modeMask & 0x02) != 0) - { - // TODO: Read the post value format response and determine the value length accordingly - if (currentData.Length >= 4) - { - var relPosition = ToInt32(currentData); - _relativePositions[channel] = relPosition; - } - else if (currentData.Length >= 2) - { - var relPosition = ToInt16(currentData); - _relativePositions[channel] = relPosition; - } - else - { - _relativePositions[channel] = currentData[0]; - } - - _positionsUpdated[channel] = true; - _positionUpdateTimes[channel] = DateTime.Now; - } - } - - break; - case 0x47: // Port input format (Single mode) DumpData("Port input format (single)", data); break; @@ -313,13 +209,16 @@ protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[] DumpData("Output command feedback", data); break; } + + // continue with default processing (e.g. for hub properties) + return base.TryProcessMessageData(messageType, data); } - private static void DumpData(string header, byte[] data) + private static void DumpData(string header, ReadOnlySpan data) { #if DEBUG - var s = BitConverter.ToString(data); - Debug.WriteLine($"{DateTimeOffset.Now:HH:mm:ss.f} {header}-{s}"); + var s = Convert.ToHexString(data); + Debug.WriteLine($"{DateTimeOffset.Now:HH:mm:ss.f} {header}-{s}"); #endif } @@ -328,7 +227,6 @@ protected override async Task ProcessOutputsAsync(CancellationToken token) try { lock (_outputLock) - lock (_positionLock) { for (int channel = 0; channel < NumberOfChannels; channel++) { @@ -358,8 +256,8 @@ protected virtual void InitializeChannelInfo(int channel, _outputValues[channel] = 0; _lastOutputValues[channel] = lastOutputValue; _sendAttemptsLeft[channel] = sendAttemptsLeft; - _positionsUpdated[channel] = false; - _positionUpdateTimes[channel] = DateTime.MinValue; + + ChannelPositions.Set(channel); } protected override async Task AfterConnectSetupAsync(bool requestDeviceInformation, CancellationToken token) @@ -376,11 +274,12 @@ protected override async Task AfterConnectSetupAsync(bool requestDeviceInf for (int channel = 0; channel < NumberOfChannels; channel++) { - if (_channelOutputTypes[channel] == ChannelOutputType.ServoMotor) + var channelConfig = ChannelConfigs[channel]; + if (channelConfig.OutputType == ChannelOutputType.ServoMotor) { await SetupChannelForPortInformationAsync(channel, token); await Task.Delay(300, token); - await ResetServoAsync(channel, _servoBaseAngles[channel], token); + await ResetServoAsync(channel, channelConfig.ServoBaseAngle, token); } } @@ -400,14 +299,14 @@ private async Task SendOutputValuesAsync(CancellationToken token) for (int channel = 0; channel < NumberOfChannels; channel++) { - switch (_channelOutputTypes[channel]) + var outputType = ChannelConfigs[channel].OutputType; + switch (outputType) { case ChannelOutputType.NormalMotor: result = result && await SendOutputValueAsync(channel, token); break; case ChannelOutputType.ServoMotor: - var maxServoAngle = _maxServoAngles[channel]; result = result && await SendServoOutputValueAsync(channel, token); break; @@ -513,7 +412,7 @@ private async Task SendServoOutputValueAsync(int channel, CancellationToke if (v != _lastOutputValues[channel] || sendAttemptsLeft > 0) { - var servoValue = _maxServoAngles[channel] * v / 100; + var servoValue = ChannelConfigs[channel].MaxServoAngle * v / 100; var servoSpeed = CalculateServoSpeed(channel, servoValue); if (servoSpeed == 0) @@ -556,7 +455,7 @@ private async Task SendStepperOutputValueAsync(int channel, CancellationTo _sendAttemptsLeft[channel] = sendAttemptsLeft > 0 ? sendAttemptsLeft - 1 : 0; } - var stepperAngle = _stepperAngles[channel]; + var stepperAngle = ChannelConfigs[channel].StepperAngle; _stepperSendBuffer[3] = GetPortId(channel); _stepperSendBuffer[6] = (byte)(stepperAngle & 0xff); _stepperSendBuffer[7] = (byte)((stepperAngle >> 8) & 0xff); @@ -627,7 +526,7 @@ protected virtual async Task ResetServoAsync(int channel, int baseAngle, C { baseAngle = Math.Max(-180, Math.Min(179, baseAngle)); - var resetToAngle = NormalizeAngle(_absolutePositions[channel] - baseAngle); + var resetToAngle = NormalizeAngle(ChannelPositions.Get(channel).AbsolutePosition - baseAngle); var result = true; @@ -641,7 +540,7 @@ protected virtual async Task ResetServoAsync(int channel, int baseAngle, C await Task.Delay(500, token); result = result && await StopAsync(channel, token); - var diff = Math.Abs(NormalizeAngle(_absolutePositions[channel] - baseAngle)); + var diff = Math.Abs(NormalizeAngle(ChannelPositions.Get(channel).AbsolutePosition - baseAngle)); if (diff > 5) { // Can't reset to base angle, rebase to current position not to stress the plastic @@ -660,77 +559,6 @@ protected virtual async Task ResetServoAsync(int channel, int baseAngle, C } } - protected int CalculateCalibratedTarget(int channel, int targetBaseAngle = 0) - { - int currentAbsPos; - int currentRelativeAngle; - - lock (_positionLock) - { - currentAbsPos = _absolutePositions[channel]; - currentRelativeAngle = _relativePositions[channel]; - } - - // Normalize the hardware relative angle to a clean 0-359 range - // (Crucial if your motor firmware reports APOS as -180 to 179) - int normalizedRelative = ((currentRelativeAngle % 360) + 360) % 360; - int normalizedTarget = ((targetBaseAngle % 360) + 360) % 360; - - // Calculate the raw difference + normalize - int diff = NormalizeAngle(normalizedTarget - normalizedRelative); - - // Offset the current accumulated position by the physical difference - return currentAbsPos + diff; - } - - protected Task AwaitStableRelativePositionAsync(int channel, TimeSpan timeout, CancellationToken token) - { - return WaitForStablePositionAsync(timeout, GetCurrentRelativePosition, token); - - int GetCurrentRelativePosition() - { - lock (_positionLock) - { - return _relativePositions[channel]; - } - } - } - - private static async Task WaitForStablePositionAsync(TimeSpan timeout, Func getPosition, CancellationToken token) - { - var interval = TimeSpan.FromMilliseconds(50); - var stabilityTimeout = TimeSpan.FromMilliseconds(500); - - using var linkedCts = CancellationTokenSource.CreateLinkedTokenSource(token); - linkedCts.CancelAfter(timeout); - - var lastPosition = getPosition(); - var stableSince = Stopwatch.StartNew(); - - try - { - while (!linkedCts.Token.IsCancellationRequested) - { - await Task.Delay(interval, linkedCts.Token); - - var currentPosition = getPosition(); - if (currentPosition != lastPosition) - { - lastPosition = currentPosition; - stableSince.Restart(); - } - else if (stableSince.Elapsed >= stabilityTimeout) - { - break; // position stable for the required duration - } - } - } - catch (OperationCanceledException) when (!token.IsCancellationRequested) - { - // total timeout elapsed — treat as completed - } - } - private async Task<(bool, float)> AutoCalibrateServoAsync(int channel, CancellationToken token) { try @@ -743,17 +571,17 @@ private static async Task WaitForStablePositionAsync(TimeSpan timeout, Func await Task.Delay(600, token); result = result && await StopAsync(channel, token); await Task.Delay(500, token); - var absPositionAt0 = _absolutePositions[channel]; + var absPositionAt0 = ChannelPositions.Get(channel).AbsolutePosition; result = result && await TurnAsync(channel, -160, 60, token); await Task.Delay(600, token); result = result && await StopAsync(channel, token); await Task.Delay(500, token); - var absPositionAtMin160 = _absolutePositions[channel]; + var absPositionAtMin160 = ChannelPositions.Get(channel).AbsolutePosition; result = result && await TurnAsync(channel, 160, 60, token); await Task.Delay(600, token); result = result && await StopAsync(channel, token); await Task.Delay(500, token); - var absPositionAt160 = _absolutePositions[channel]; + var absPositionAt160 = ChannelPositions.Get(channel).AbsolutePosition; var midPoint1 = NormalizeAngle((absPositionAtMin160 + absPositionAt160) / 2); var midPoint2 = NormalizeAngle(midPoint1 + 180); @@ -761,7 +589,7 @@ private static async Task WaitForStablePositionAsync(TimeSpan timeout, Func var baseAngle = (Math.Abs(NormalizeAngle(midPoint1 - absPositionAt0)) < Math.Abs(NormalizeAngle(midPoint2 - absPositionAt0))) ? RoundAngleToNearest90(midPoint1) : RoundAngleToNearest90(midPoint2); - var resetToAngle = NormalizeAngle(_absolutePositions[channel] - baseAngle); + var resetToAngle = NormalizeAngle(ChannelPositions.Get(channel).AbsolutePosition - baseAngle); result = result && await ResetAsync(channel, 0, token); result = result && await StopAsync(channel, token); @@ -793,23 +621,21 @@ private static int RoundAngleToNearest90(int angle) private int CalculateServoSpeed(int channel, int targetAngle) { - lock (_positionLock) + var channelPositions = ChannelPositions.Get(channel); + + if (channelPositions.IsUpdated) { - if (_positionsUpdated[channel]) - { - var diffAngle = Math.Abs(_relativePositions[channel] - targetAngle); - _positionsUpdated[channel] = false; + var diffAngle = Math.Abs(channelPositions.RelativePosition - targetAngle); + ChannelPositions.Update(channel, x => x with { IsUpdated = false }); - return Math.Max(20, Math.Min(100, diffAngle)); - } + return Math.Max(20, Math.Min(100, diffAngle)); + } - var positionUpdateTime = _positionUpdateTimes[channel]; - if (positionUpdateTime == DateTime.MinValue || - POSITION_EXPIRATION < DateTime.Now - positionUpdateTime) - { - // Position update never happened or too old - return 50; - } + if (channelPositions.UpdateTime == DateTime.MinValue || + POSITION_EXPIRATION < DateTime.Now - channelPositions.UpdateTime) + { + // Position update never happened or too old + return 50; } return 0; diff --git a/BrickController2/BrickController2/DeviceManagement/IO/ChannelConfig.cs b/BrickController2/BrickController2/DeviceManagement/IO/ChannelConfig.cs index 6ae1a8cf..96513b7a 100644 --- a/BrickController2/BrickController2/DeviceManagement/IO/ChannelConfig.cs +++ b/BrickController2/BrickController2/DeviceManagement/IO/ChannelConfig.cs @@ -4,7 +4,7 @@ namespace BrickController2.DeviceManagement.IO; internal readonly record struct ChannelConfig { - public ChannelOutputType ChannelOutputType { get; init; } + public ChannelOutputType OutputType { get; init; } public int MaxServoAngle { get; init; } public int ServoBaseAngle { get; init; } public int StepperAngle { get; init; } diff --git a/BrickController2/BrickController2/DeviceManagement/IO/ChannelStateStore.cs b/BrickController2/BrickController2/DeviceManagement/IO/ChannelStateStore.cs new file mode 100644 index 00000000..3896788c --- /dev/null +++ b/BrickController2/BrickController2/DeviceManagement/IO/ChannelStateStore.cs @@ -0,0 +1,64 @@ +using System; +using System.Threading; + +namespace BrickController2.DeviceManagement.IO; + +/// +/// Thread-safe, channel-indexed store for per-channel state structs. +/// Supports atomic read, write, and functional update. +/// +internal sealed class ChannelStateStore where T : struct +{ + private readonly T[] _states; + private readonly Lock _lock = new(); + + public ChannelStateStore(int channelCount, T initialState = default) + { + _states = new T[channelCount]; + _states.AsSpan().Fill(initialState); + } + + public int ChannelCount => _states.Length; + + /// Returns the current state for the given channel. + public T Get(int channel) + { + lock (_lock) return _states[channel]; + } + + /// Replaces the state for the given channel. + public void Set(int channel, T state = default) + { + lock (_lock) _states[channel] = state; + } + + /// + /// Atomically updates the state for the given channel using the provided updater function. + /// Returns the new state. + /// + public T Update(int channel, Func updater) + { + lock (_lock) + { + var updated = updater(_states[channel]); + _states[channel] = updated; + return updated; + } + } + + /// Resets all channels to the given state. + public void ResetAll(T state = default) + { + lock (_lock) _states.AsSpan().Fill(state); + } + + /// Resets all channels using a per-channel factory. + public void ResetAll(Func factory) + { + lock (_lock) + { + for (int i = 0; i < _states.Length; i++) + _states[i] = factory(i); + } + } +} diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/ChannelPositionState.cs b/BrickController2/BrickController2/DeviceManagement/Lego/ChannelPositionState.cs new file mode 100644 index 00000000..95e7dcbb --- /dev/null +++ b/BrickController2/BrickController2/DeviceManagement/Lego/ChannelPositionState.cs @@ -0,0 +1,31 @@ +using System; + +namespace BrickController2.DeviceManagement.Lego; + +/// +/// Immutable snapshot of a motor channel's position feedback. +/// +internal record struct ChannelPositionState( + int AbsolutePosition, + int RelativePosition, + bool IsUpdated, + DateTime UpdateTime) +{ + public static readonly ChannelPositionState Initial = new( + AbsolutePosition: 0, + RelativePosition: 0, + IsUpdated: false, + UpdateTime: DateTime.MinValue); + + /// Returns a new state with an updated relative position and timestamp. + public ChannelPositionState WithRelativePosition(int relativePosition) => + this with { RelativePosition = relativePosition, IsUpdated = true, UpdateTime = DateTime.Now }; + + /// Returns a new state with an updated absolute position. + public ChannelPositionState WithAbsolutePosition(int absolutePosition) => + this with { AbsolutePosition = absolutePosition, IsUpdated = true, UpdateTime = DateTime.Now }; + + /// Clears the IsUpdated flag after the update has been consumed. + public ChannelPositionState ConsumeUpdate() => + this with { IsUpdated = false }; +} diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/ControlPlusDeviceBase.cs b/BrickController2/BrickController2/DeviceManagement/Lego/ControlPlusDeviceBase.cs deleted file mode 100644 index 0e9775a1..00000000 --- a/BrickController2/BrickController2/DeviceManagement/Lego/ControlPlusDeviceBase.cs +++ /dev/null @@ -1,201 +0,0 @@ -using BrickController2.CreationManagement; -using BrickController2.DeviceManagement.IO; -using BrickController2.PlatformServices.BluetoothLE; -using System; -using System.Collections.Generic; -using System.Linq; -using System.Threading; -using System.Threading.Tasks; - - -using static BrickController2.Protocols.LegoWirelessProtocol; - -namespace BrickController2.DeviceManagement.Lego; - -internal abstract class ControlPlusDeviceBase : BluetoothDevice -{ - protected readonly OutputValuesGroup OutputValues; - protected readonly ChannelConfig[] ChannelConfigs; - - protected IGattCharacteristic? Characteristic; - - protected ControlPlusDeviceBase(string name, string address, IDeviceRepository deviceRepository, IBluetoothLEService bleService) - : base(name, address, deviceRepository, bleService) - { - OutputValues = new(NumberOfChannels); - ChannelConfigs = new ChannelConfig[NumberOfChannels]; - } - - public override string BatteryVoltageSign => "%"; - - public override void SetOutput(int channel, float value) - { - CheckChannel(channel); - - var percentValue = (Half)(100 * CutOutputValue(value)); - OutputValues.SetOutput(channel, percentValue); - } - - public override Task ConnectAsync( - bool reconnect, - Action onDeviceDisconnected, - IEnumerable channelConfigurations, - bool startOutputProcessing, - bool requestDeviceInformation, - CancellationToken token) - { - // reset output values - OutputValues.Clear(); - - //TODO angles - - // Initialize configuration per channel - - // build dictionary - var configs = channelConfigurations.ToDictionary(c => c.Channel, c => c); - - for (int i = 0; i < NumberOfChannels; i++) - { - configs.TryGetValue(i, out var config); - - ChannelConfigs[i] = config.ChannelOutputType switch - { - ChannelOutputType.ServoMotor => new() - { - ChannelOutputType = ChannelOutputType.ServoMotor, - MaxServoAngle = config.MaxServoAngle, - ServoBaseAngle = config.ServoBaseAngle - }, - ChannelOutputType.StepperMotor => new() - { - ChannelOutputType = ChannelOutputType.StepperMotor, - StepperAngle = config.StepperAngle - }, - _ => new() - }; - } - - return base.ConnectAsync(reconnect, onDeviceDisconnected, channelConfigurations, startOutputProcessing, requestDeviceInformation, token); - } - - protected virtual byte GetPortId(int channelIndex) => (byte)channelIndex; - protected virtual bool TryGetChannelIndex(byte portId, out int channelIndex) - { - channelIndex = portId; - return portId < NumberOfChannels; - } - - protected override async Task ValidateServicesAsync(IEnumerable? services, CancellationToken token) - { - var service = services?.FirstOrDefault(s => s.Uuid == ServiceUuid); - Characteristic = service?.Characteristics?.FirstOrDefault(c => c.Uuid == CharacteristicUuid); - - if (Characteristic is not null) - { - return await _bleDevice!.EnableNotificationAsync(Characteristic, token); - } - - return false; - } - - protected override async ValueTask BeforeDisconnectAsync(CancellationToken token) - { - // reset notifications (if possible) - if (Characteristic != null && _bleDevice != null) - { - await _bleDevice.DisableNotificationAsync(Characteristic, token); - } - } - - protected override void BeforeDisconnectCleanup() - { - Characteristic = null; - } - - protected async ValueTask WriteNoResponseAsync(byte[] data, TimeSpan sentDelay, CancellationToken token = default) - { - var result = await _bleDevice!.WriteNoResponseAsync(Characteristic!, data, token); - await Task.Delay(sentDelay, token); - return result; - } - - protected async ValueTask WriteNoResponseAsync(byte[] data, CancellationToken token = default) - => await _bleDevice!.WriteNoResponseAsync(Characteristic!, data, token); - - protected async ValueTask WriteAsync(byte[] data, CancellationToken token = default) - => await _bleDevice!.WriteAsync(Characteristic!, data, token); - - protected async ValueTask RequestHubPropertiesAsync(CancellationToken token) - { - try - { - // Request firmware version - await RequestHubPropertyAsync(HUB_PROPERTY_FW_VERSION, token); - // Request hardware version - await RequestHubPropertyAsync(HUB_PROPERTY_HW_VERSION, token); - // Request battery voltage - await RequestHubPropertyAsync(HUB_PROPERTY_VOLTAGE, token); - } - catch { } - } - - protected async ValueTask RequestHubPropertyAsync(byte propertyId, CancellationToken token) - { - try - { - // Request firmware version - await Task.Delay(TimeSpan.FromMilliseconds(100), token); - await _bleDevice!.WriteAsync(Characteristic!, [0x05, 0x00, 0x01, propertyId, 0x05], token); - var data = await _bleDevice!.ReadAsync(Characteristic!, token); - ProcessHubPropertyData(data); - } - catch { } - } - - protected void ProcessHubPropertyData(ReadOnlySpan data) - { - try - { - if (data.Length < 6) - { - return; - } - - var dataLength = data[0]; - var messageId = data[2]; - var propertyId = data[3]; - var propertyOperation = data[4]; - - if (messageId != MESSAGE_TYPE_HUB_PROPERTIES || propertyOperation != HUB_PROPERTY_OPERATION_UPDATE) - { - // Operation is not 'update' - return; - } - - switch (propertyId) - { - case HUB_PROPERTY_FW_VERSION: // FW version - var firmwareVersion = GetVersionString(data.Slice(5)); - if (!string.IsNullOrEmpty(firmwareVersion)) - { - FirmwareVersion = firmwareVersion; - } - break; - - case HUB_PROPERTY_HW_VERSION: // HW version - var hardwareVersion = GetVersionString(data.Slice(5)); - if (!string.IsNullOrEmpty(hardwareVersion)) - { - HardwareVersion = hardwareVersion; - } - break; - - case HUB_PROPERTY_VOLTAGE: // Battery voltage - var voltage = data[5]; - BatteryVoltage = voltage.ToString("F0"); - break; - } - } - catch { } - } -} diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs b/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs index aac682cb..890860df 100644 --- a/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs +++ b/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs @@ -15,7 +15,7 @@ namespace BrickController2.DeviceManagement.Lego; /// /// Represents a LEGO® Powered Up 88010 Remote Control /// -internal class RemoteControl : ControlPlusDeviceBase +internal class RemoteControl : WirelessProtocolBasedDevice { private const string ENABLED_SETTING_NAME = "RemoteControlEnabled"; private const bool DEFAULT_ENABLED = false; @@ -34,8 +34,6 @@ public RemoteControl(string name, string address, IEnumerable sett public bool IsEnabled => GetSettingValue(ENABLED_SETTING_NAME, DEFAULT_ENABLED); - protected override bool AutoConnectOnFirstConnect => false; - public override void SetOutput(int channel, float value) => throw new InvalidOperationException(); internal void ConnectInputController(TController inputController) where TController : InputDeviceBase @@ -79,27 +77,21 @@ protected override async Task AfterConnectSetupAsync(bool requestDeviceInf return await WriteAsync(remoteButtonB, token); } - protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[] data) + protected override void ResetOutputValues() { - if (data.Length < 4) - { - return; - } - - var messageCode = data[2]; + // nothing to reset + } - switch (messageCode) + protected override bool TryProcessMessageData(byte messageType, ReadOnlySpan data) + { + switch (messageType) { - case MESSAGE_TYPE_HUB_PROPERTIES: // Hub properties - ProcessHubPropertyData(data); - break; - case MESSAGE_TYPE_HW_NETWORK_COMMANDS: // HW network commands if (data.Length == 5 && data[3] == 0x02) { // HW button state RaiseButtonEvents([("Home", GetButtonValue(data[4]))]); - break; + return true; } break; case MESSAGE_TYPE_PORT_VALUE: // 0x45 Port Value / RemoteButton @@ -108,19 +100,17 @@ protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[] switch (data[3]) { case REMOTE_BUTTONS_LEFT: - OnButtonEvents("A.Plus", "A", "A.Minus", data.AsSpan(4)); + OnButtonEvents("A.Plus", "A", "A.Minus", data.Slice(4)); break; case REMOTE_BUTTONS_RIGHT: - OnButtonEvents("B.Plus", "B", "B.Minus", data.AsSpan(4)); - break; - default: + OnButtonEvents("B.Plus", "B", "B.Minus", data.Slice(4)); break; } + return true; } break; - default: - break; } + return base.TryProcessMessageData(messageType, data); } private void OnButtonEvents(string plus, string stop, string minus, ReadOnlySpan flags) diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs new file mode 100644 index 00000000..a39d0711 --- /dev/null +++ b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs @@ -0,0 +1,336 @@ +using BrickController2.CreationManagement; +using BrickController2.DeviceManagement.IO; +using BrickController2.PlatformServices.BluetoothLE; +using System; +using System.Collections.Generic; +using System.Diagnostics; +using System.Linq; +using System.Threading; +using System.Threading.Tasks; + +using static BrickController2.Protocols.LegoWirelessProtocol; + +namespace BrickController2.DeviceManagement.Lego; + +internal abstract class WirelessProtocolBasedDevice : BluetoothDevice +{ + protected readonly ChannelConfig[] ChannelConfigs; + protected readonly ChannelStateStore ChannelPositions; + + protected IGattCharacteristic? Characteristic; + + protected WirelessProtocolBasedDevice(string name, string address, IDeviceRepository deviceRepository, IBluetoothLEService bleService) + : base(name, address, deviceRepository, bleService) + { + ChannelConfigs = new ChannelConfig[NumberOfChannels]; + ChannelPositions = new(NumberOfChannels, ChannelPositionState.Initial); + } + + public override string BatteryVoltageSign => "%"; + protected override bool AutoConnectOnFirstConnect => false; + + public override Task ConnectAsync( + bool reconnect, + Action onDeviceDisconnected, + IEnumerable channelConfigurations, + bool startOutputProcessing, + bool requestDeviceInformation, + CancellationToken token) + { + // reset output values & positions + ResetOutputValues(); + ChannelPositions.ResetAll(); + + // Initialize configuration per channel + + // build dictionary, but for supported ones only + var configs = channelConfigurations + .Where(c => IsOutputTypeSupported(c.Channel, c.ChannelOutputType)) + .ToDictionary(c => c.Channel, c => c); + + for (int i = 0; i < NumberOfChannels; i++) + { + configs.TryGetValue(i, out var config); + + ChannelConfigs[i] = config.ChannelOutputType switch + { + ChannelOutputType.ServoMotor => new() + { + OutputType = ChannelOutputType.ServoMotor, + MaxServoAngle = config.MaxServoAngle, + ServoBaseAngle = config.ServoBaseAngle + }, + ChannelOutputType.StepperMotor => new() + { + OutputType = ChannelOutputType.StepperMotor, + StepperAngle = config.StepperAngle + }, + _ => new() + }; + } + + return base.ConnectAsync(reconnect, onDeviceDisconnected, channelConfigurations, startOutputProcessing, requestDeviceInformation, token); + } + + protected virtual byte GetPortId(int channelIndex) => (byte)channelIndex; + protected virtual bool TryGetChannelIndex(byte portId, out int channelIndex) + { + channelIndex = portId; + return portId < NumberOfChannels; + } + + protected abstract void ResetOutputValues(); + + protected override async Task ValidateServicesAsync(IEnumerable? services, CancellationToken token) + { + var service = services?.FirstOrDefault(s => s.Uuid == ServiceUuid); + Characteristic = service?.Characteristics?.FirstOrDefault(c => c.Uuid == CharacteristicUuid); + + if (Characteristic is not null) + { + return await _bleDevice!.EnableNotificationAsync(Characteristic, token); + } + + return false; + } + + protected override async ValueTask BeforeDisconnectAsync(CancellationToken token) + { + // reset notifications (if possible) + if (Characteristic != null && _bleDevice != null) + { + await _bleDevice.DisableNotificationAsync(Characteristic, token); + } + } + + protected override void BeforeDisconnectCleanup() + { + Characteristic = null; + } + + protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[] data) + { + if (data.Length < 4 || Characteristic is null || characteristicGuid != Characteristic.Uuid) + { + return; + } + + TryProcessMessageData(messageType: data[2], data); + } + + protected virtual bool TryProcessMessageData(byte messageType, ReadOnlySpan data) + { + switch (messageType) + { + case MESSAGE_TYPE_HUB_PROPERTIES: // Hub properties + ProcessHubPropertyData(data); + return true; + + case MESSAGE_TYPE_PORT_VALUE: // Port value (single mode) + { + if (!TryGetChannelIndex(portId: data[3], out var channel)) + { + break; + } + if (data.Length == 6) + { + // assume 16bit data is ABS + var absPosition = ToInt16(data.Slice(4)); + ChannelPositions.Update(channel, pos => pos.WithAbsolutePosition(absPosition)); + } + else if (data.Length == 8) + { + // assume 32 bit data is REL + var relPosition = ToInt32(data.Slice(4)); + ChannelPositions.Update(channel, pos => pos.WithRelativePosition(relPosition)); + } + return true; + } + + case MESSAGE_TYPE_PORT_VALUE_COMBINED: // Port value (combined mode) + { + if (!TryGetChannelIndex(portId: data[3], out var channel)) + { + break; + } + + var modeMask = data[5]; + var currentData = data.Slice(6); // start at index 6 + + if ((modeMask & 0x01) != 0) + { + var absPosition = ToInt16(currentData); + ChannelPositions.Update(channel, pos => pos.WithAbsolutePosition(absPosition)); + + currentData = currentData.Slice(2); + } + + if ((modeMask & 0x02) != 0) + { + // TODO: Read the post value format response and determine the value length accordingly + int relPosition = currentData.Length switch + { + >= 4 => ToInt32(currentData), + >= 2 => ToInt16(currentData), + _ => currentData[0] + }; + ChannelPositions.Update(channel, pos => pos.WithRelativePosition(relPosition)); + } + return true; + } +#if DEBUG + default: + var s = Convert.ToHexString(data); + Debug.WriteLine($"{DateTimeOffset.Now:HH:mm:ss.f} {messageType}-{s}"); + break; +#endif + } + + return false; + } + + protected async ValueTask WriteNoResponseAsync(byte[] data, TimeSpan sentDelay, CancellationToken token = default) + { + var result = await _bleDevice!.WriteNoResponseAsync(Characteristic!, data, token); + await Task.Delay(sentDelay, token); + return result; + } + + protected async ValueTask WriteNoResponseAsync(byte[] data, CancellationToken token = default) + => await _bleDevice!.WriteNoResponseAsync(Characteristic!, data, token); + + protected async ValueTask WriteAsync(byte[] data, CancellationToken token = default) + => await _bleDevice!.WriteAsync(Characteristic!, data, token); + + protected async ValueTask RequestHubPropertiesAsync(CancellationToken token) + { + try + { + // Request firmware version + await RequestHubPropertyAsync(HUB_PROPERTY_FW_VERSION, token); + // Request hardware version + await RequestHubPropertyAsync(HUB_PROPERTY_HW_VERSION, token); + // Request battery voltage + await RequestHubPropertyAsync(HUB_PROPERTY_VOLTAGE, token); + } + catch { } + } + + protected async ValueTask RequestHubPropertyAsync(byte propertyId, CancellationToken token) + { + try + { + // Request firmware version + await Task.Delay(TimeSpan.FromMilliseconds(100), token); + await _bleDevice!.WriteAsync(Characteristic!, [0x05, 0x00, 0x01, propertyId, 0x05], token); + var data = await _bleDevice!.ReadAsync(Characteristic!, token); + ProcessHubPropertyData(data); + } + catch { } + } + + protected void ProcessHubPropertyData(ReadOnlySpan data) + { + try + { + if (data.Length < 6) + { + return; + } + + var dataLength = data[0]; + var messageId = data[2]; + var propertyId = data[3]; + var propertyOperation = data[4]; + + if (messageId != MESSAGE_TYPE_HUB_PROPERTIES || propertyOperation != HUB_PROPERTY_OPERATION_UPDATE) + { + // Operation is not 'update' + return; + } + + switch (propertyId) + { + case HUB_PROPERTY_FW_VERSION: // FW version + var firmwareVersion = GetVersionString(data.Slice(5)); + if (!string.IsNullOrEmpty(firmwareVersion)) + { + FirmwareVersion = firmwareVersion; + } + break; + + case HUB_PROPERTY_HW_VERSION: // HW version + var hardwareVersion = GetVersionString(data.Slice(5)); + if (!string.IsNullOrEmpty(hardwareVersion)) + { + HardwareVersion = hardwareVersion; + } + break; + + case HUB_PROPERTY_VOLTAGE: // Battery voltage + var voltage = data[5]; + BatteryVoltage = voltage.ToString("F0"); + break; + } + } + catch { } + } + + protected int CalculateCalibratedTarget(int channel, int targetBaseAngle = 0) + { + var position = ChannelPositions.Get(channel); + + // Normalize the hardware relative angle to a clean 0-359 range + // (Crucial if your motor firmware reports APOS as -180 to 179) + int normalizedRelative = ((position.RelativePosition % 360) + 360) % 360; + int normalizedTarget = ((targetBaseAngle % 360) + 360) % 360; + + // Calculate the raw difference + normalize + int diff = NormalizeAngle(normalizedTarget - normalizedRelative); + + // Offset the current accumulated position by the physical difference + return position.AbsolutePosition + diff; + } + + protected Task AwaitStableRelativePositionAsync(int channel, TimeSpan timeout, CancellationToken token) + { + return WaitForStablePositionAsync(timeout, GetCurrentRelativePosition, token); + + int GetCurrentRelativePosition() => ChannelPositions.Get(channel).RelativePosition; + } + + private static async Task WaitForStablePositionAsync(TimeSpan timeout, Func getPosition, CancellationToken token) + { + var interval = TimeSpan.FromMilliseconds(50); + var stabilityTimeout = TimeSpan.FromMilliseconds(500); + + using var linkedCts = CancellationTokenSource.CreateLinkedTokenSource(token); + linkedCts.CancelAfter(timeout); + + var lastPosition = getPosition(); + var stableSince = Stopwatch.StartNew(); + + try + { + while (!linkedCts.Token.IsCancellationRequested) + { + await Task.Delay(interval, linkedCts.Token); + + var currentPosition = getPosition(); + if (currentPosition != lastPosition) + { + lastPosition = currentPosition; + stableSince.Restart(); + } + else if (stableSince.Elapsed >= stabilityTimeout) + { + break; // position stable for the required duration + } + } + } + catch (OperationCanceledException) when (!token.IsCancellationRequested) + { + // total timeout elapsed — treat as completed + } + } +} diff --git a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs index 3441cbd3..d07ddfc5 100644 --- a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs @@ -1,4 +1,6 @@ using BrickController2.CreationManagement; +using BrickController2.DeviceManagement.IO; +using BrickController2.DeviceManagement.Lego; using BrickController2.PlatformServices.BluetoothLE; using BrickController2.Settings; using System; @@ -6,19 +8,28 @@ using System.Linq; using System.Threading; using System.Threading.Tasks; + using static BrickController2.Protocols.LegoWirelessProtocol; namespace BrickController2.DeviceManagement { - internal class TechnicMoveDevice : ControlPlusDevice + internal class TechnicMoveDevice : WirelessProtocolBasedDevice { public const int CHANNEL_VM = 12; // artificial channel to mimic combined AB ports in PLAYVM + private const int CHANNEL_A = 0; + private const int CHANNEL_B = 1; private const int CHANNEL_C = 2; + private const int CHANNEL_1 = 3; // Light #1 + private const int CHANNEL_6 = 8; // Light #6 + private const int PLAYVM_CHANNEL_DRIVE = 0; + private const int PLAYVM_CHANNEL_STEER = 1; private const string EnablePlayVmSettingName = "PlayVmEnabled"; + private readonly OutputValuesGroup _outputValues = new(9); + private readonly OutputValuesGroup _playVmValues = new(2); + private bool _applyPlayVmMode; - private volatile byte _virtualMotorValue; private int _calibratedZeroAngle; // zero ABS angle for steering C channel in non PLAYVM mode public TechnicMoveDevice(string name, @@ -59,29 +70,33 @@ public override Task ConnectAsync(bool reconnect, Action _applyPlayVmMode = startOutputProcessing && EnablePlayVmMode && channelConfigurations.Any(c => c.Channel == CHANNEL_VM || (c.Channel == CHANNEL_C && c.ChannelOutputType == ChannelOutputType.ServoMotor)); - // filter out non-standard channels and configurations with unsupported output types - var filteredConfigurations = channelConfigurations - .Where(c => c.Channel != CHANNEL_VM) - .Where(c => IsOutputTypeSupported(c.Channel, c.ChannelOutputType)) - .ToArray(); - - return base.ConnectAsync(reconnect, onDeviceDisconnected, filteredConfigurations, startOutputProcessing, requestDeviceInformation, token); + return base.ConnectAsync(reconnect, onDeviceDisconnected, channelConfigurations, startOutputProcessing, requestDeviceInformation, token); } public override void SetOutput(int channel, float value) { - if (channel == CHANNEL_VM) - { - // reset servo writes to enforce update - ResetSendAttemps(CHANNEL_C); - // store virtual motor value to be later send with PLAYVM - var intValue = (int)(100 * CutOutputValue(value)); - _virtualMotorValue = GetChannelValue(intValue); - } - else + var rawValue = (Half)(100 * CutOutputValue(value)); + + _ = channel switch { - base.SetOutput(channel, value); - } + // store A+B virtual channel value for PLAYVM + CHANNEL_VM => _playVmValues.SetOutput(PLAYVM_CHANNEL_DRIVE, rawValue), + // store C channel value for PLAYVM + 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)), + // rest of ports: such as A, B or C when not in PLAYVM mode - use value as is + _ => _outputValues.SetOutput(CheckChannel(channel), rawValue) + }; + } + + public override async Task ResetOutputAsync(int channel, float value, CancellationToken token) + { + CheckChannel(channel); + + await SetupChannelForPortInformationAsync(channel, token); + await Task.Delay(300, token); + await ResetServoAsync(channel, Convert.ToInt32(value * 180), token); } protected override byte GetPortId(int channelIndex) => channelIndex switch @@ -100,57 +115,12 @@ protected override bool TryGetChannelIndex(byte portId, out int channelIndex) PORT_DRIVE_MOTOR_1 => 0, PORT_DRIVE_MOTOR_2 => 1, PORT_STEERING_MOTOR => 2, - + // all other ports (PORT_6LEDS, PORT_PLAYVM, PORT_HUB_LED, etc.) are not tracked _ => -1 }; return channelIndex >= 0; } - protected override byte GetChannelValue(int value) => ToByte(value); - - protected override void InitializeChannelInfo(int channel, int lastOutputValue = 1, int sendAttemptsLeft = 10) - { - // if PLAYVM enabled, reset A / B channels differently in order to avoid output writes - if (_applyPlayVmMode && channel < CHANNEL_C) - { - lastOutputValue = 0; - sendAttemptsLeft = 0; - } - else if (channel > CHANNEL_C) - { - // no need to update lights - lastOutputValue = 0; - sendAttemptsLeft = 0; - } - base.InitializeChannelInfo(channel, lastOutputValue, sendAttemptsLeft); - } - - protected override byte[] GetOutputCommand(int channel, int value) - { - // 6LED - var ledIndex = channel - 3; - if (ledIndex >= 0) - { - var rawValue = ToByte(Math.Abs(value)); - var ledMask = ToByte(1 << ledIndex); - return BuildPortOutput_LedMask(PORT_6LEDS, PORT_MODE_0, ledMask, rawValue); - } - return base.GetOutputCommand(channel, value); - } - - protected override byte[] GetServoCommand(int channel, int servoValue, int servoSpeed) - { - if (_applyPlayVmMode) - { - return BuildPortOutput_PlayVm(speedValue: _virtualMotorValue, servoValue: servoValue); - } - - var portId = GetPortId(channel); - // in non PLAYVM mode, need to apply calibrated base angle as offset to reach correct position - var value = _calibratedZeroAngle + _servoBaseAngles[channel] + servoValue; - return BuildPortOutput_GotoAbsPosition(portId, value, (byte)servoSpeed); - } - protected override async ValueTask BeforeDisconnectAsync(CancellationToken token) { await base.BeforeDisconnectAsync(token); @@ -170,6 +140,11 @@ protected override async Task AfterConnectSetupAsync(bool requestDeviceInf { try { + if (requestDeviceInformation) + { + await RequestHubPropertiesAsync(token); + } + // hub LED var color = _applyPlayVmMode ? HUB_LED_COLOR_MAGENTA : HUB_LED_COLOR_WHITE; var ledCmd = BuildPortOutput_DirectMode(PORT_HUB_LED, HUB_LED_MODE_COLOR, color); @@ -191,7 +166,38 @@ protected override async Task AfterConnectSetupAsync(bool requestDeviceInf return false; } - protected override async Task SetupChannelForPortInformationAsync(int channel, CancellationToken token) + protected override void ResetOutputValues() + { + if (_applyPlayVmMode) + { + _playVmValues.Initialize(); + // output values - clear always lights — suppress initial burst to avoid flooding the hub + _outputValues.Clear(); + } + else + { + _playVmValues.Clear(); + // otherwise all channels to be initialized + _outputValues.Initialize(); + } + } + + protected override async Task ProcessOutputsAsync(CancellationToken token) + { + try + { + while (!token.IsCancellationRequested) + { + if (!await SendOutputValuesAsync(token).ConfigureAwait(false)) + { + await Task.Delay(10, token).ConfigureAwait(false); + } + } + } + catch { } + } + + private async Task SetupChannelForPortInformationAsync(int channel, CancellationToken token) { try { @@ -228,7 +234,7 @@ protected override async Task SetupChannelForPortInformationAsync(int chan } } - protected override async Task ResetServoAsync(int channel, int baseAngle, CancellationToken token) + private async Task ResetServoAsync(int channel, int baseAngle, CancellationToken token) { try { @@ -262,5 +268,123 @@ protected override async Task ResetServoAsync(int channel, int baseAngle, return false; } } + + private async Task SendOutputValuesAsync(CancellationToken token) + { + try + { + // conditionally send PLAYVM command if PLAYVM mode is active + var result = await SendPlayVmOutputValueAsync(token); + + // process changes for other channels as it's a light or a classic drive + if (result && _outputValues.TryGetChanges(out var changes)) + { + foreach (KeyValuePair change in changes) + { + var value = ToByte(change.Value); + var channelOutputType = change.Key < NumberOfChannels + ? ChannelConfigs[change.Key].OutputType + : ChannelOutputType.NormalMotor; + + 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 + int.MaxValue => await SendAllOutputValuesAsync(value, token), + // classic output command for A, B, C channels (with servo support) + CHANNEL_C when channelOutputType == ChannelOutputType.ServoMotor => await SendServoValue(change.Key, value, token), + _ => await SendPortOutput_ValueAsync(change.Key, value, token), + }; + + if (!result) + { + return false; + } + } + + _outputValues.Commit(); + } + + return result; + } + catch + { + return false; + } + } + + private async Task SendPlayVmOutputValueAsync(CancellationToken token) + { + try + { + if (_applyPlayVmMode && _playVmValues.TryGetValues(out var values)) + { + var speed = ToByte(values[PLAYVM_CHANNEL_DRIVE]); + var maxServoAngle = ChannelConfigs[CHANNEL_C].MaxServoAngle; + var servoValue = maxServoAngle * (int)values[PLAYVM_CHANNEL_STEER] / 100; + var playVmCmd = BuildPortOutput_PlayVm(speed, servoValue); + + if (!await WriteAsync(playVmCmd, token)) + { + await Task.Delay(20, token); //TODO + return false; + } + + // commit when successfully sent + _playVmValues.Commit(); + } + return true; + } + catch + { + return false; + } + } + + private Task SendPortOutput_6LedAsync(int ledIndex, byte value, CancellationToken token) + => SendPortOutput_6LedMaskAsync(ToByte(1 << ledIndex), value, token); + + private async Task SendPortOutput_6LedMaskAsync(byte lightMask, byte value, CancellationToken token) + { + var cmd = BuildPortOutput_LedMask(PORT_6LEDS, PORT_MODE_0, lightMask, value); + return await WriteAsync(cmd, token); + } + + private async Task SendPortOutput_ValueAsync(int channel, byte value, CancellationToken token) + { + byte[] cmd = [8, 0x00, 0x81, GetPortId(channel), 0x11, 0x51, 0x00, value]; + return await WriteAsync(cmd, token); + } + + private async Task SendServoValue(int channel, int value, CancellationToken token) + { + var portId = GetPortId(channel); + // in non PLAYVM mode, need to apply calibrated base angle as offset to reach correct position + var absPosition = _calibratedZeroAngle + ChannelConfigs[channel].ServoBaseAngle + value; + var cmd = BuildPortOutput_GotoAbsPosition(portId, absPosition, servoSpeed: 50); + return await WriteAsync(cmd, token); + } + + private async Task SendAllOutputValuesAsync(byte value, CancellationToken token) + { + // all LEDs at once + var result = await SendPortOutput_6LedMaskAsync(PORT_6LEDS_ALL_LIGHTS, value, token); + + // A, B, C channels + foreach (var channel in new[] { CHANNEL_A, CHANNEL_B, CHANNEL_C }) + { + var outputType = ChannelConfigs[channel].OutputType; + result = result && outputType switch + { + ChannelOutputType.ServoMotor => await SendServoValue(channel, value, token), + _ => await SendPortOutput_ValueAsync(channel, value, token), + }; + + result = result && await SendPortOutput_ValueAsync(channel, value, token); + } + + return result; + } } } diff --git a/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs b/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs index 6b9fd247..d05ebbe8 100644 --- a/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs +++ b/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs @@ -22,6 +22,7 @@ internal static class LegoWirelessProtocol public const byte MESSAGE_TYPE_HUB_PROPERTIES = 0x01; public const byte MESSAGE_TYPE_HW_NETWORK_COMMANDS = 0x08; public const byte MESSAGE_TYPE_PORT_VALUE = 0x45; + public const byte MESSAGE_TYPE_PORT_VALUE_COMBINED = 0x46; // TechnicMove hub ports public const byte PORT_DRIVE_MOTOR_1 = 0x32; From f4c22b9052439b3743000e27aa2d73c6153f8c0b Mon Sep 17 00:00:00 2001 From: Vit Nemecky Date: Wed, 29 Apr 2026 23:09:21 +0200 Subject: [PATCH 08/29] work save --- .../DeviceManagement/ControlPlusDevice.cs | 82 ++----------- .../Lego/ChannelPositionState.cs | 16 +-- .../Lego/WirelessProtocolBasedDevice.cs | 114 +++++++++++++++--- .../DeviceManagement/TechnicMoveDevice.cs | 44 ++++--- 4 files changed, 138 insertions(+), 118 deletions(-) diff --git a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs index 7e5e5e70..5c55534a 100644 --- a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs @@ -161,67 +161,6 @@ protected virtual byte[] GetServoCommand(int channel, int servoValue, int servoS return _servoSendBuffer; } - protected override bool TryProcessMessageData(byte messageType, ReadOnlySpan data) - { - switch (messageType) - { - case 0x02: // Hub actions - DumpData("Hub actions", data); - break; - - case 0x03: // Hub alerts - DumpData("Hub alerts", data); - break; - - case 0x04: // Hub attached I/O - DumpData("Hub attached I/O", data); - break; - - case 0x05: // Generic error messages - DumpData("Generic error messages", data); - break; - - case 0x08: // HW network commands - DumpData("HW network commands", data); - break; - - case 0x13: // FW lock status - DumpData("FW lock status", data); - break; - - case 0x43: // Port information - DumpData("Port information", data); - break; - - case 0x44: // Port mode information - DumpData("Port mode information", data); - break; - - case 0x47: // Port input format (Single mode) - DumpData("Port input format (single)", data); - break; - - case 0x48: // Port input format (Combined mode) - DumpData("Port input format (combined)", data); - break; - - case 0x82: // Port output command feedback - DumpData("Output command feedback", data); - break; - } - - // continue with default processing (e.g. for hub properties) - return base.TryProcessMessageData(messageType, data); - } - - private static void DumpData(string header, ReadOnlySpan data) - { -#if DEBUG - var s = Convert.ToHexString(data); - Debug.WriteLine($"{DateTimeOffset.Now:HH:mm:ss.f} {header}-{s}"); -#endif - } - protected override async Task ProcessOutputsAsync(CancellationToken token) { try @@ -257,7 +196,8 @@ protected virtual void InitializeChannelInfo(int channel, _lastOutputValues[channel] = lastOutputValue; _sendAttemptsLeft[channel] = sendAttemptsLeft; - ChannelPositions.Set(channel); + ChannelAbsPositions.Set(channel); + ChannelRelativePositions.Set(channel); } protected override async Task AfterConnectSetupAsync(bool requestDeviceInformation, CancellationToken token) @@ -526,7 +466,7 @@ protected virtual async Task ResetServoAsync(int channel, int baseAngle, C { baseAngle = Math.Max(-180, Math.Min(179, baseAngle)); - var resetToAngle = NormalizeAngle(ChannelPositions.Get(channel).AbsolutePosition - baseAngle); + var resetToAngle = NormalizeAngle(ChannelAbsPositions.Get(channel).Current - baseAngle); var result = true; @@ -540,7 +480,7 @@ protected virtual async Task ResetServoAsync(int channel, int baseAngle, C await Task.Delay(500, token); result = result && await StopAsync(channel, token); - var diff = Math.Abs(NormalizeAngle(ChannelPositions.Get(channel).AbsolutePosition - baseAngle)); + var diff = Math.Abs(NormalizeAngle(ChannelAbsPositions.Get(channel).Current - baseAngle)); if (diff > 5) { // Can't reset to base angle, rebase to current position not to stress the plastic @@ -571,17 +511,17 @@ protected virtual async Task ResetServoAsync(int channel, int baseAngle, C await Task.Delay(600, token); result = result && await StopAsync(channel, token); await Task.Delay(500, token); - var absPositionAt0 = ChannelPositions.Get(channel).AbsolutePosition; + var absPositionAt0 = ChannelAbsPositions.Get(channel).Current; result = result && await TurnAsync(channel, -160, 60, token); await Task.Delay(600, token); result = result && await StopAsync(channel, token); await Task.Delay(500, token); - var absPositionAtMin160 = ChannelPositions.Get(channel).AbsolutePosition; + var absPositionAtMin160 = ChannelAbsPositions.Get(channel).Current; result = result && await TurnAsync(channel, 160, 60, token); await Task.Delay(600, token); result = result && await StopAsync(channel, token); await Task.Delay(500, token); - var absPositionAt160 = ChannelPositions.Get(channel).AbsolutePosition; + var absPositionAt160 = ChannelAbsPositions.Get(channel).Current; var midPoint1 = NormalizeAngle((absPositionAtMin160 + absPositionAt160) / 2); var midPoint2 = NormalizeAngle(midPoint1 + 180); @@ -589,7 +529,7 @@ protected virtual async Task ResetServoAsync(int channel, int baseAngle, C var baseAngle = (Math.Abs(NormalizeAngle(midPoint1 - absPositionAt0)) < Math.Abs(NormalizeAngle(midPoint2 - absPositionAt0))) ? RoundAngleToNearest90(midPoint1) : RoundAngleToNearest90(midPoint2); - var resetToAngle = NormalizeAngle(ChannelPositions.Get(channel).AbsolutePosition - baseAngle); + var resetToAngle = NormalizeAngle(ChannelAbsPositions.Get(channel).Current - baseAngle); result = result && await ResetAsync(channel, 0, token); result = result && await StopAsync(channel, token); @@ -621,12 +561,12 @@ private static int RoundAngleToNearest90(int angle) private int CalculateServoSpeed(int channel, int targetAngle) { - var channelPositions = ChannelPositions.Get(channel); + var channelPositions = ChannelRelativePositions.Get(channel); if (channelPositions.IsUpdated) { - var diffAngle = Math.Abs(channelPositions.RelativePosition - targetAngle); - ChannelPositions.Update(channel, x => x with { IsUpdated = false }); + var diffAngle = Math.Abs(channelPositions.Current - targetAngle); + ChannelRelativePositions.Update(channel, x => x.ConsumeUpdate()); return Math.Max(20, Math.Min(100, diffAngle)); } diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/ChannelPositionState.cs b/BrickController2/BrickController2/DeviceManagement/Lego/ChannelPositionState.cs index 95e7dcbb..ae95887e 100644 --- a/BrickController2/BrickController2/DeviceManagement/Lego/ChannelPositionState.cs +++ b/BrickController2/BrickController2/DeviceManagement/Lego/ChannelPositionState.cs @@ -6,24 +6,18 @@ namespace BrickController2.DeviceManagement.Lego; /// Immutable snapshot of a motor channel's position feedback. /// internal record struct ChannelPositionState( - int AbsolutePosition, - int RelativePosition, + int Current, bool IsUpdated, DateTime UpdateTime) { public static readonly ChannelPositionState Initial = new( - AbsolutePosition: 0, - RelativePosition: 0, + Current: 0, IsUpdated: false, UpdateTime: DateTime.MinValue); - /// Returns a new state with an updated relative position and timestamp. - public ChannelPositionState WithRelativePosition(int relativePosition) => - this with { RelativePosition = relativePosition, IsUpdated = true, UpdateTime = DateTime.Now }; - - /// Returns a new state with an updated absolute position. - public ChannelPositionState WithAbsolutePosition(int absolutePosition) => - this with { AbsolutePosition = absolutePosition, IsUpdated = true, UpdateTime = DateTime.Now }; + /// Returns a new state with an updated position and timestamp. + public ChannelPositionState WithPosition(int position) => + this with { Current = position, IsUpdated = true, UpdateTime = DateTime.Now }; /// Clears the IsUpdated flag after the update has been consumed. public ChannelPositionState ConsumeUpdate() => diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs index a39d0711..cac5b629 100644 --- a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs @@ -8,6 +8,7 @@ using System.Threading; using System.Threading.Tasks; +using static BrickController2.CreationManagement.ControllerDefaults; using static BrickController2.Protocols.LegoWirelessProtocol; namespace BrickController2.DeviceManagement.Lego; @@ -15,7 +16,8 @@ namespace BrickController2.DeviceManagement.Lego; internal abstract class WirelessProtocolBasedDevice : BluetoothDevice { protected readonly ChannelConfig[] ChannelConfigs; - protected readonly ChannelStateStore ChannelPositions; + protected readonly ChannelStateStore ChannelAbsPositions; + protected readonly ChannelStateStore ChannelRelativePositions; protected IGattCharacteristic? Characteristic; @@ -23,7 +25,8 @@ protected WirelessProtocolBasedDevice(string name, string address, IDeviceReposi : base(name, address, deviceRepository, bleService) { ChannelConfigs = new ChannelConfig[NumberOfChannels]; - ChannelPositions = new(NumberOfChannels, ChannelPositionState.Initial); + ChannelAbsPositions = new(NumberOfChannels, ChannelPositionState.Initial); + ChannelRelativePositions = new(NumberOfChannels, ChannelPositionState.Initial); } public override string BatteryVoltageSign => "%"; @@ -39,7 +42,8 @@ public override Task ConnectAsync( { // reset output values & positions ResetOutputValues(); - ChannelPositions.ResetAll(); + ChannelAbsPositions.ResetAll(); + ChannelRelativePositions.ResetAll(); // Initialize configuration per channel @@ -136,14 +140,15 @@ protected virtual bool TryProcessMessageData(byte messageType, ReadOnlySpan pos.WithAbsolutePosition(absPosition)); + ChannelAbsPositions.Update(channel, pos => pos.WithPosition(absPosition)); } else if (data.Length == 8) { // assume 32 bit data is REL var relPosition = ToInt32(data.Slice(4)); - ChannelPositions.Update(channel, pos => pos.WithRelativePosition(relPosition)); + ChannelRelativePositions.Update(channel, pos => pos.WithPosition(relPosition)); } + DumpData("PORT_VALUE", data); return true; } @@ -160,7 +165,7 @@ protected virtual bool TryProcessMessageData(byte messageType, ReadOnlySpan pos.WithAbsolutePosition(absPosition)); + ChannelAbsPositions.Update(channel, pos => pos.WithPosition(absPosition)); currentData = currentData.Slice(2); } @@ -174,18 +179,64 @@ protected virtual bool TryProcessMessageData(byte messageType, ReadOnlySpan= 2 => ToInt16(currentData), _ => currentData[0] }; - ChannelPositions.Update(channel, pos => pos.WithRelativePosition(relPosition)); + ChannelRelativePositions.Update(channel, pos => pos.WithPosition(relPosition)); } + DumpData("PORT_VALUE_COMBINED", data); return true; } #if DEBUG - default: - var s = Convert.ToHexString(data); - Debug.WriteLine($"{DateTimeOffset.Now:HH:mm:ss.f} {messageType}-{s}"); + case 0x02: // Hub actions + DumpData("Hub actions", data); + break; + + case 0x03: // Hub alerts + DumpData("Hub alerts", data); + break; + + case 0x04: // Hub attached I/O + DumpData("Hub attached I/O", data); + break; + + case 0x05: // Generic error messages + DumpData("Generic error messages", data); + break; + + case 0x08: // HW network commands + DumpData("HW network commands", data); + break; + + case 0x13: // FW lock status + DumpData("FW lock status", data); + break; + + case 0x43: // Port information + DumpData("Port information", data); + break; + + case 0x44: // Port mode information + DumpData("Port mode information", data); + break; + + case 0x47: // Port input format (Single mode) + DumpData("Port input format (single)", data); + break; + + case 0x48: // Port input format (Combined mode) + DumpData("Port input format (combined)", data); + break; + + case 0x82: // Port output command feedback + DumpData("Output command feedback", data); break; #endif } + static void DumpData(string label, ReadOnlySpan data) + { + var s = Convert.ToHexString(data); + Debug.WriteLine($"{DateTimeOffset.Now:HH:mm:ss.f} {label}: {s}"); + } + return false; } @@ -202,6 +253,21 @@ protected async ValueTask WriteNoResponseAsync(byte[] data, CancellationTo protected async ValueTask WriteAsync(byte[] data, CancellationToken token = default) => await _bleDevice!.WriteAsync(Characteristic!, data, token); + protected static Task DelayAsync(CancellationToken token = default) => Task.Delay(20, token); + + protected ChannelOutputType GetOutputType(int channel) => (channel < 0 || channel >= NumberOfChannels) + ? ChannelOutputType.NormalMotor // fallback to a default type if out of range + : ChannelConfigs[channel].OutputType; + + protected int GetMaxServoAngle(int channel) + { + var maxServoAngle = channel >= 0 && channel < NumberOfChannels + ? ChannelConfigs[channel].MaxServoAngle + : default; + + return maxServoAngle > 0 ? maxServoAngle : DEFAULT_MAX_SERVO_ANGLE; + } + protected async ValueTask RequestHubPropertiesAsync(CancellationToken token) { try @@ -278,28 +344,31 @@ protected void ProcessHubPropertyData(ReadOnlySpan data) protected int CalculateCalibratedTarget(int channel, int targetBaseAngle = 0) { - var position = ChannelPositions.Get(channel); + var position = ChannelRelativePositions.Get(channel).Current; // Normalize the hardware relative angle to a clean 0-359 range // (Crucial if your motor firmware reports APOS as -180 to 179) - int normalizedRelative = ((position.RelativePosition % 360) + 360) % 360; + int normalizedRelative = ((position % 360) + 360) % 360; int normalizedTarget = ((targetBaseAngle % 360) + 360) % 360; // Calculate the raw difference + normalize int diff = NormalizeAngle(normalizedTarget - normalizedRelative); // Offset the current accumulated position by the physical difference - return position.AbsolutePosition + diff; + return ChannelAbsPositions.Get(channel).Current + diff; } protected Task AwaitStableRelativePositionAsync(int channel, TimeSpan timeout, CancellationToken token) { - return WaitForStablePositionAsync(timeout, GetCurrentRelativePosition, token); + return WaitForStablePositionAsync(timeout, () => ChannelRelativePositions.Get(channel), token); + } - int GetCurrentRelativePosition() => ChannelPositions.Get(channel).RelativePosition; + protected Task AwaitStableAbsolutePositionAsync(int channel, TimeSpan timeout, CancellationToken token) + { + return WaitForStablePositionAsync(timeout, () => ChannelAbsPositions.Get(channel), token); } - private static async Task WaitForStablePositionAsync(TimeSpan timeout, Func getPosition, CancellationToken token) + private static async Task WaitForStablePositionAsync(TimeSpan timeout, Func getPosition, CancellationToken token) { var interval = TimeSpan.FromMilliseconds(50); var stabilityTimeout = TimeSpan.FromMilliseconds(500); @@ -307,7 +376,7 @@ private static async Task WaitForStablePositionAsync(TimeSpan timeout, Func using var linkedCts = CancellationTokenSource.CreateLinkedTokenSource(token); linkedCts.CancelAfter(timeout); - var lastPosition = getPosition(); + var lastPosition = getPosition().Current; var stableSince = Stopwatch.StartNew(); try @@ -317,20 +386,25 @@ private static async Task WaitForStablePositionAsync(TimeSpan timeout, Func await Task.Delay(interval, linkedCts.Token); var currentPosition = getPosition(); - if (currentPosition != lastPosition) + if (!currentPosition.IsUpdated || currentPosition.Current != lastPosition) { - lastPosition = currentPosition; + lastPosition = currentPosition.Current; stableSince.Restart(); + + Debug.WriteLine($"Position changed to {lastPosition}, resetting stability timer."); } else if (stableSince.Elapsed >= stabilityTimeout) { - break; // position stable for the required duration + Debug.WriteLine($"Position stable at {lastPosition} for {stabilityTimeout.TotalMilliseconds} ms."); + return; // position stable for the required duration } } + Debug.WriteLine($"Position time outed having {lastPosition}."); } catch (OperationCanceledException) when (!token.IsCancellationRequested) { // total timeout elapsed — treat as completed + Debug.WriteLine($"Position canceled at {lastPosition} for {stabilityTimeout.TotalMilliseconds} ms."); } } } diff --git a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs index d07ddfc5..e86243b2 100644 --- a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs @@ -130,12 +130,15 @@ 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 Task.Delay(20, token); + await DelayAsync(token); } } protected override async Task AfterConnectSetupAsync(bool requestDeviceInformation, CancellationToken token) { + // Wait until ports finish communicating with the hub + await Task.Delay(1000, token); //TODO + if (await base.AfterConnectSetupAsync(requestDeviceInformation, token)) { try @@ -149,12 +152,24 @@ protected override async Task AfterConnectSetupAsync(bool requestDeviceInf var color = _applyPlayVmMode ? HUB_LED_COLOR_MAGENTA : HUB_LED_COLOR_WHITE; var ledCmd = BuildPortOutput_DirectMode(PORT_HUB_LED, HUB_LED_MODE_COLOR, color); await WriteAsync(ledCmd, token: token); - await Task.Delay(20, token); + await DelayAsync(token); // switch lights off var lightsOffCmd = BuildPortOutput_LedMask(PORT_6LEDS, PORT_MODE_0, PORT_6LEDS_ALL_LIGHTS, 0x00); var result = await WriteAsync(lightsOffCmd, token: token); - await Task.Delay(20, token); + await DelayAsync(token); + + // port configuration + for (int channel = 0; channel < NumberOfChannels; channel++) + { + var channelConfig = ChannelConfigs[channel]; + if (channelConfig.OutputType == ChannelOutputType.ServoMotor) + { + await SetupChannelForPortInformationAsync(channel, token); + await Task.Delay(300, token); + await ResetServoAsync(channel, channelConfig.ServoBaseAngle, token); + } + } return result; } @@ -170,16 +185,16 @@ protected override void ResetOutputValues() { if (_applyPlayVmMode) { - _playVmValues.Initialize(); // output values - clear always lights — suppress initial burst to avoid flooding the hub _outputValues.Clear(); } else { - _playVmValues.Clear(); // otherwise all channels to be initialized _outputValues.Initialize(); } + _playVmValues.Clear(); + _calibratedZeroAngle = default; } protected override async Task ProcessOutputsAsync(CancellationToken token) @@ -249,6 +264,8 @@ private async Task ResetServoAsync(int channel, int baseAngle, Cancellatio // do calibration var calibrateCmd = BuildPortOutput_PlayVm(servoValue: baseAngle, vmCmd: PLAYVM_CALIBRATE_STEERING); await WriteAsync(calibrateCmd, token: token); + + await AwaitStableRelativePositionAsync(channel, TimeSpan.FromSeconds(4), token); } else { @@ -256,10 +273,9 @@ private async Task ResetServoAsync(int channel, int baseAngle, Cancellatio var portId = GetPortId(channel); var servoCmd = BuildPortOutput_GotoAbsPosition(portId, _calibratedZeroAngle + baseAngle, servoSpeed: 0x28); await WriteAsync(servoCmd, token: token); - } - // need to wait till it completes - await AwaitStableRelativePositionAsync(channel, TimeSpan.FromSeconds(4), token); + await AwaitStableAbsolutePositionAsync(channel, TimeSpan.FromSeconds(1), token); + } return true; } @@ -282,9 +298,7 @@ private async Task SendOutputValuesAsync(CancellationToken token) foreach (KeyValuePair change in changes) { var value = ToByte(change.Value); - var channelOutputType = change.Key < NumberOfChannels - ? ChannelConfigs[change.Key].OutputType - : ChannelOutputType.NormalMotor; + var channelOutputType = GetOutputType(change.Key); result = change.Key switch { @@ -320,14 +334,14 @@ private async Task SendPlayVmOutputValueAsync(CancellationToken token) { if (_applyPlayVmMode && _playVmValues.TryGetValues(out var values)) { + var maxServoAngle = GetMaxServoAngle(CHANNEL_C); var speed = ToByte(values[PLAYVM_CHANNEL_DRIVE]); - var maxServoAngle = ChannelConfigs[CHANNEL_C].MaxServoAngle; var servoValue = maxServoAngle * (int)values[PLAYVM_CHANNEL_STEER] / 100; var playVmCmd = BuildPortOutput_PlayVm(speed, servoValue); if (!await WriteAsync(playVmCmd, token)) { - await Task.Delay(20, token); //TODO + await DelayAsync(token); return false; } @@ -361,7 +375,7 @@ private async Task SendServoValue(int channel, int value, CancellationToke { var portId = GetPortId(channel); // in non PLAYVM mode, need to apply calibrated base angle as offset to reach correct position - var absPosition = _calibratedZeroAngle + ChannelConfigs[channel].ServoBaseAngle + value; + var absPosition = _calibratedZeroAngle + ChannelConfigs[channel].ServoBaseAngle + value; //TODO MAX SERVO ANGLE var cmd = BuildPortOutput_GotoAbsPosition(portId, absPosition, servoSpeed: 50); return await WriteAsync(cmd, token); } @@ -380,8 +394,6 @@ private async Task SendAllOutputValuesAsync(byte value, CancellationToken ChannelOutputType.ServoMotor => await SendServoValue(channel, value, token), _ => await SendPortOutput_ValueAsync(channel, value, token), }; - - result = result && await SendPortOutput_ValueAsync(channel, value, token); } return result; From 8c7789683ef915d49cb9d42b07744b61853b15e6 Mon Sep 17 00:00:00 2001 From: Vit Nemecky Date: Wed, 29 Apr 2026 23:51:08 +0200 Subject: [PATCH 09/29] nitpicks --- .../DeviceManagement/TechnicMoveDevice.cs | 26 ++++++++++--------- 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs index e86243b2..41f2939d 100644 --- a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs @@ -217,17 +217,17 @@ private async Task SetupChannelForPortInformationAsync(int channel, Cancel try { var portId = GetPortId(channel); - var inputFormatForRelAngle = BuildPortInputFormatSetup(portId, PORT_MODE_2); if (_applyPlayVmMode) { - // setup channel to report POS position regularly - return await WriteAsync(inputFormatForRelAngle, token); + // setup channel to report APOS position + var inputFormatForAbsAngle = BuildPortInputFormatSetup(portId, PORT_MODE_3); + return await WriteAsync(inputFormatForAbsAngle, token); } // setup channel to for APOS, but no notifications - var inputFormatForAbsAngle = BuildPortInputFormatSetup(portId, PORT_MODE_3, notification: PORT_VALUE_NOTIFICATION_DISABLED); - await WriteAsync(inputFormatForAbsAngle, token); + var inputFormatForAbsAngleDisabled = BuildPortInputFormatSetup(portId, PORT_MODE_3, notification: PORT_VALUE_NOTIFICATION_DISABLED); + await WriteAsync(inputFormatForAbsAngleDisabled, token); await Task.Delay(50, token); // query current APOS @@ -235,6 +235,7 @@ private async Task SetupChannelForPortInformationAsync(int channel, Cancel await Task.Delay(250, token); //TODO wait for change // setup channel to report POS position regularly + var inputFormatForRelAngle = BuildPortInputFormatSetup(portId, PORT_MODE_2); await WriteAsync(inputFormatForRelAngle, token); await Task.Delay(250, token); //TODO wait for change @@ -265,7 +266,7 @@ private async Task ResetServoAsync(int channel, int baseAngle, Cancellatio var calibrateCmd = BuildPortOutput_PlayVm(servoValue: baseAngle, vmCmd: PLAYVM_CALIBRATE_STEERING); await WriteAsync(calibrateCmd, token: token); - await AwaitStableRelativePositionAsync(channel, TimeSpan.FromSeconds(4), token); + await AwaitStableAbsolutePositionAsync(channel, TimeSpan.FromSeconds(4), token); } else { @@ -304,8 +305,8 @@ private async Task SendOutputValuesAsync(CancellationToken token) { // 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 - int.MaxValue => await SendAllOutputValuesAsync(value, token), + // all channels command - use original value + int.MaxValue => await SendAllOutputValuesAsync(change.Value, token), // classic output command for A, B, C channels (with servo support) CHANNEL_C when channelOutputType == ChannelOutputType.ServoMotor => await SendServoValue(change.Key, value, token), _ => await SendPortOutput_ValueAsync(change.Key, value, token), @@ -380,10 +381,11 @@ private async Task SendServoValue(int channel, int value, CancellationToke return await WriteAsync(cmd, token); } - private async Task SendAllOutputValuesAsync(byte value, CancellationToken token) + private async Task SendAllOutputValuesAsync(Half value, CancellationToken token) { + var rawValue = ToByte(value); // all LEDs at once - var result = await SendPortOutput_6LedMaskAsync(PORT_6LEDS_ALL_LIGHTS, value, token); + var result = await SendPortOutput_6LedMaskAsync(PORT_6LEDS_ALL_LIGHTS, rawValue, token); // A, B, C channels foreach (var channel in new[] { CHANNEL_A, CHANNEL_B, CHANNEL_C }) @@ -391,8 +393,8 @@ private async Task SendAllOutputValuesAsync(byte value, CancellationToken var outputType = ChannelConfigs[channel].OutputType; result = result && outputType switch { - ChannelOutputType.ServoMotor => await SendServoValue(channel, value, token), - _ => await SendPortOutput_ValueAsync(channel, value, token), + ChannelOutputType.ServoMotor => await SendServoValue(channel, (int)value, token), + _ => await SendPortOutput_ValueAsync(channel, rawValue, token), }; } From b8899f15f8eedfb2cd346c29f8bd956628df4eae Mon Sep 17 00:00:00 2001 From: Vit Nemecky Date: Thu, 30 Apr 2026 20:44:48 +0200 Subject: [PATCH 10/29] work save - wrrking somehow --- .../DeviceManagement/IO/ChannelStateStore.cs | 73 +++++----- .../Lego/ChannelAttachmentInfo.cs | 26 ++++ .../Lego/WirelessProtocolBasedDevice.cs | 126 ++++++++++++----- .../DeviceManagement/TechnicMoveDevice.cs | 127 +++++++++++------- .../BrickController2/Diagnostics/Logs.cs | 20 +++ .../Protocols/LegoWirelessProtocol.cs | 3 + 6 files changed, 254 insertions(+), 121 deletions(-) create mode 100644 BrickController2/BrickController2/DeviceManagement/Lego/ChannelAttachmentInfo.cs create mode 100644 BrickController2/BrickController2/Diagnostics/Logs.cs diff --git a/BrickController2/BrickController2/DeviceManagement/IO/ChannelStateStore.cs b/BrickController2/BrickController2/DeviceManagement/IO/ChannelStateStore.cs index 3896788c..baf88ea6 100644 --- a/BrickController2/BrickController2/DeviceManagement/IO/ChannelStateStore.cs +++ b/BrickController2/BrickController2/DeviceManagement/IO/ChannelStateStore.cs @@ -1,64 +1,63 @@ using System; -using System.Threading; +using System.Collections.Concurrent; +using System.Collections.Generic; namespace BrickController2.DeviceManagement.IO; /// -/// Thread-safe, channel-indexed store for per-channel state structs. +/// Thread-safe, key-indexed store for per-key state structs. /// Supports atomic read, write, and functional update. /// -internal sealed class ChannelStateStore where T : struct +internal sealed class ChannelStateStore + where TKey : notnull + where TValue : struct { - private readonly T[] _states; - private readonly Lock _lock = new(); + private readonly ConcurrentDictionary _states = new(); + private readonly TValue _default; - public ChannelStateStore(int channelCount, T initialState = default) + public ChannelStateStore(TValue initialState = default) { - _states = new T[channelCount]; - _states.AsSpan().Fill(initialState); + _states = new(); + _default = initialState; } - public int ChannelCount => _states.Length; + public int Count => _states.Count; - /// Returns the current state for the given channel. - public T Get(int channel) - { - lock (_lock) return _states[channel]; - } + /// Returns the current state for the given key or the default state if the key does not exist. + public TValue Get(TKey key) => _states.TryGetValue(key, out var value) ? value : _default; + + /// Removes the current state for the given key. + public bool Remove(TKey key) => _states.TryRemove(key, out var _); - /// Replaces the state for the given channel. - public void Set(int channel, T state = default) + /// Upsert the state for the given key. + public void Set(TKey key, TValue state = default) { - lock (_lock) _states[channel] = state; + _states[key] = state; } /// - /// Atomically updates the state for the given channel using the provided updater function. + /// Atomically updates the state for the given key using the provided updater function. /// Returns the new state. /// - public T Update(int channel, Func updater) - { - lock (_lock) - { - var updated = updater(_states[channel]); - _states[channel] = updated; - return updated; - } - } + public TValue Update(TKey key, Func updater) => _states.AddOrUpdate(key, (k) => updater(_default), (k, o) => updater(o)); - /// Resets all channels to the given state. - public void ResetAll(T state = default) - { - lock (_lock) _states.AsSpan().Fill(state); - } + /// Clears all persisted states. + public void Clear() => _states.Clear(); - /// Resets all channels using a per-channel factory. - public void ResetAll(Func factory) + /// Returns the maximum value of a projection over all stored states, or default if empty. + public TResult? Max(Func selector) { - lock (_lock) + TResult? max = default; + bool first = true; + foreach (var kvp in _states) { - for (int i = 0; i < _states.Length; i++) - _states[i] = factory(i); + var val = selector(kvp.Value); + if (first || Comparer.Default.Compare(val, max) > 0) + { + max = val; + first = false; + } } + return max; } } diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/ChannelAttachmentInfo.cs b/BrickController2/BrickController2/DeviceManagement/Lego/ChannelAttachmentInfo.cs new file mode 100644 index 00000000..54c25cc7 --- /dev/null +++ b/BrickController2/BrickController2/DeviceManagement/Lego/ChannelAttachmentInfo.cs @@ -0,0 +1,26 @@ +using System; +using System.Numerics; + +namespace BrickController2.DeviceManagement.Lego; + +/// +/// Immutable snapshot of a motor channel's position feedback. +/// +internal record struct ChannelAttachmentInfo( + ushort DeviceId, + bool IsUpdated, + DateTime UpdateTime) +{ + public static readonly ChannelAttachmentInfo Initial = new( + DeviceId: 0, + IsUpdated: false, + UpdateTime: DateTime.MinValue); + + /// Returns a new state with an updated device ID and timestamp. + public ChannelAttachmentInfo WithDevice(ushort deviceId) => + this with { DeviceId = deviceId, IsUpdated = true, UpdateTime = DateTime.Now }; + + /// Clears the IsUpdated flag after the update has been consumed. + public ChannelAttachmentInfo ConsumeUpdate() => + this with { IsUpdated = false }; +} diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs index cac5b629..5c566be5 100644 --- a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs @@ -9,6 +9,7 @@ using System.Threading.Tasks; using static BrickController2.CreationManagement.ControllerDefaults; +using static BrickController2.Diagnostics.Logs; using static BrickController2.Protocols.LegoWirelessProtocol; namespace BrickController2.DeviceManagement.Lego; @@ -16,8 +17,10 @@ namespace BrickController2.DeviceManagement.Lego; internal abstract class WirelessProtocolBasedDevice : BluetoothDevice { protected readonly ChannelConfig[] ChannelConfigs; - protected readonly ChannelStateStore ChannelAbsPositions; - protected readonly ChannelStateStore ChannelRelativePositions; + protected readonly ChannelStateStore ChannelAbsPositions; + protected readonly ChannelStateStore ChannelRelativePositions; + + protected readonly ChannelStateStore AttachedHubs; protected IGattCharacteristic? Characteristic; @@ -25,8 +28,9 @@ protected WirelessProtocolBasedDevice(string name, string address, IDeviceReposi : base(name, address, deviceRepository, bleService) { ChannelConfigs = new ChannelConfig[NumberOfChannels]; - ChannelAbsPositions = new(NumberOfChannels, ChannelPositionState.Initial); - ChannelRelativePositions = new(NumberOfChannels, ChannelPositionState.Initial); + ChannelAbsPositions = new(ChannelPositionState.Initial); + ChannelRelativePositions = new(ChannelPositionState.Initial); + AttachedHubs = new(ChannelAttachmentInfo.Initial); } public override string BatteryVoltageSign => "%"; @@ -42,8 +46,9 @@ public override Task ConnectAsync( { // reset output values & positions ResetOutputValues(); - ChannelAbsPositions.ResetAll(); - ChannelRelativePositions.ResetAll(); + ChannelAbsPositions.Clear(); + ChannelRelativePositions.Clear(); + AttachedHubs.Clear(); // Initialize configuration per channel @@ -148,7 +153,7 @@ protected virtual bool TryProcessMessageData(byte messageType, ReadOnlySpan pos.WithPosition(relPosition)); } - DumpData("PORT_VALUE", data); + Dump("PORT_VALUE", data); return true; } @@ -181,62 +186,72 @@ protected virtual bool TryProcessMessageData(byte messageType, ReadOnlySpan pos.WithPosition(relPosition)); } - DumpData("PORT_VALUE_COMBINED", data); + Dump("PORT_VALUE_COMBINED", data); + return true; + } + case MESSAGE_TYPE_HUB_ATTACHED_IO: // Hub attached I/O + { + Dump("HUB_ATTACHED_IO", data); + + if (TryGetChannelIndex(portId: data[3], out var channel)) + { + byte eventType = data[4]; // 0x01 = Attached, 0x00 = Detached, 0x02 = Attached Virtual + + if (eventType == 0x01 || eventType == 0x02) + { + var deviceId = ToUInt16(data.Slice(5)); + AttachedHubs.Update(channel, info => info.WithDevice(deviceId)); // store portId as "position" for simplicity + } + else if (eventType == 0x00) + { + AttachedHubs.Remove(channel); + } + } return true; } #if DEBUG case 0x02: // Hub actions - DumpData("Hub actions", data); + Dump("Hub actions", data); break; case 0x03: // Hub alerts - DumpData("Hub alerts", data); - break; - - case 0x04: // Hub attached I/O - DumpData("Hub attached I/O", data); + Dump("Hub alerts", data); break; case 0x05: // Generic error messages - DumpData("Generic error messages", data); + Dump("Generic error messages", data); break; case 0x08: // HW network commands - DumpData("HW network commands", data); + Dump("HW network commands", data); break; case 0x13: // FW lock status - DumpData("FW lock status", data); + Dump("FW lock status", data); break; case 0x43: // Port information - DumpData("Port information", data); + Dump("Port information", data); break; case 0x44: // Port mode information - DumpData("Port mode information", data); + Dump("Port mode information", data); break; case 0x47: // Port input format (Single mode) - DumpData("Port input format (single)", data); + Dump("Port input format (single)", data); break; case 0x48: // Port input format (Combined mode) - DumpData("Port input format (combined)", data); - break; - - case 0x82: // Port output command feedback - DumpData("Output command feedback", data); + Dump("Port input format (combined)", data); break; #endif - } - static void DumpData(string label, ReadOnlySpan data) - { - var s = Convert.ToHexString(data); - Debug.WriteLine($"{DateTimeOffset.Now:HH:mm:ss.f} {label}: {s}"); + case 0x82: // Port output command feedback + Dump("Output command feedback", data); + OnPortOutputCommandFeedback(data); + return true; } - return false; } @@ -253,6 +268,8 @@ protected async ValueTask WriteNoResponseAsync(byte[] data, CancellationTo protected async ValueTask WriteAsync(byte[] data, CancellationToken token = default) => await _bleDevice!.WriteAsync(Characteristic!, data, token); + protected virtual void OnPortOutputCommandFeedback(ReadOnlySpan data) { } + protected static Task DelayAsync(CancellationToken token = default) => Task.Delay(20, token); protected ChannelOutputType GetOutputType(int channel) => (channel < 0 || channel >= NumberOfChannels) @@ -368,6 +385,44 @@ protected Task AwaitStableAbsolutePositionAsync(int channel, TimeSpan timeout, C return WaitForStablePositionAsync(timeout, () => ChannelAbsPositions.Get(channel), token); } + protected async Task AwaitForHubConnectedAsync(TimeSpan timeout, CancellationToken token) + { + var interval = TimeSpan.FromMilliseconds(50); + var stabilityTimeout = TimeSpan.FromMilliseconds(250); + + using var linkedCts = CancellationTokenSource.CreateLinkedTokenSource(token); + linkedCts.CancelAfter(timeout); + + var stableSince = Stopwatch.StartNew(); + var lastUpdated = AttachedHubs.Max(x => x.UpdateTime); + + try + { + while (!linkedCts.Token.IsCancellationRequested) + { + await Task.Delay(interval, linkedCts.Token); + + var currentValue = AttachedHubs.Max(x => x.UpdateTime); + if (currentValue != lastUpdated) + { + lastUpdated = currentValue; + stableSince.Restart(); + } + else if (stableSince.Elapsed >= stabilityTimeout) + { + Dump("Hub connection: HUBS", AttachedHubs.Count); + return; // position stable for the required duration + } + } + Dump("Hub connection: TIMEOUT", lastUpdated); + } + catch (OperationCanceledException) when (!token.IsCancellationRequested) + { + // total timeout elapsed — treat as completed + Dump("Hub connection: CANCELLED", lastUpdated); + } + } + private static async Task WaitForStablePositionAsync(TimeSpan timeout, Func getPosition, CancellationToken token) { var interval = TimeSpan.FromMilliseconds(50); @@ -390,21 +445,20 @@ private static async Task WaitForStablePositionAsync(TimeSpan timeout, Func= stabilityTimeout) { - Debug.WriteLine($"Position stable at {lastPosition} for {stabilityTimeout.TotalMilliseconds} ms."); + Dump("Servo position: STABLE", lastPosition); return; // position stable for the required duration } } - Debug.WriteLine($"Position time outed having {lastPosition}."); + Dump("Servo position: TIMEOUT", lastPosition); } catch (OperationCanceledException) when (!token.IsCancellationRequested) { // total timeout elapsed — treat as completed - Debug.WriteLine($"Position canceled at {lastPosition} for {stabilityTimeout.TotalMilliseconds} ms."); + Dump("Servo position: CANCELLED", lastPosition); } } + } diff --git a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs index 41f2939d..544b344e 100644 --- a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs @@ -9,6 +9,7 @@ using System.Threading; using System.Threading.Tasks; +using static BrickController2.Diagnostics.Logs; using static BrickController2.Protocols.LegoWirelessProtocol; namespace BrickController2.DeviceManagement @@ -31,6 +32,7 @@ internal class TechnicMoveDevice : WirelessProtocolBasedDevice private bool _applyPlayVmMode; private int _calibratedZeroAngle; // zero ABS angle for steering C channel in non PLAYVM mode + private TaskCompletionSource? _playVmCalibrationTcs; public TechnicMoveDevice(string name, string address, @@ -95,7 +97,6 @@ public override async Task ResetOutputAsync(int channel, float value, Cancellati CheckChannel(channel); await SetupChannelForPortInformationAsync(channel, token); - await Task.Delay(300, token); await ResetServoAsync(channel, Convert.ToInt32(value * 180), token); } @@ -121,6 +122,15 @@ protected override bool TryGetChannelIndex(byte portId, out int channelIndex) return channelIndex >= 0; } + protected override void OnPortOutputCommandFeedback(ReadOnlySpan data) + { + // PORT_PLAYVM completion feedback (0x82) signals calibration finished + if (data.Length >= 5 && data[3] == PORT_PLAYVM && (data[4] & 0x02) != 0) + { + _playVmCalibrationTcs?.TrySetResult(true); + } + } + protected override async ValueTask BeforeDisconnectAsync(CancellationToken token) { await base.BeforeDisconnectAsync(token); @@ -136,63 +146,49 @@ protected override async ValueTask BeforeDisconnectAsync(CancellationToken token protected override async Task AfterConnectSetupAsync(bool requestDeviceInformation, CancellationToken token) { - // Wait until ports finish communicating with the hub - await Task.Delay(1000, token); //TODO - - if (await base.AfterConnectSetupAsync(requestDeviceInformation, token)) + try { - try + // wait until ports finish communicating with the hub + await AwaitForHubConnectedAsync(TimeSpan.FromSeconds(1), token); + + if (requestDeviceInformation) { - if (requestDeviceInformation) - { - await RequestHubPropertiesAsync(token); - } + await RequestHubPropertiesAsync(token); + } - // hub LED - var color = _applyPlayVmMode ? HUB_LED_COLOR_MAGENTA : HUB_LED_COLOR_WHITE; - var ledCmd = BuildPortOutput_DirectMode(PORT_HUB_LED, HUB_LED_MODE_COLOR, color); - await WriteAsync(ledCmd, token: token); - await DelayAsync(token); + // hub LED + var color = _applyPlayVmMode ? HUB_LED_COLOR_MAGENTA : HUB_LED_COLOR_WHITE; + var ledCmd = BuildPortOutput_DirectMode(PORT_HUB_LED, HUB_LED_MODE_COLOR, color); + await WriteAsync(ledCmd, token: token); + await DelayAsync(token); - // switch lights off - var lightsOffCmd = BuildPortOutput_LedMask(PORT_6LEDS, PORT_MODE_0, PORT_6LEDS_ALL_LIGHTS, 0x00); - var result = await WriteAsync(lightsOffCmd, token: token); - await DelayAsync(token); + // switch lights off + var lightsOffCmd = BuildPortOutput_LedMask(PORT_6LEDS, PORT_MODE_0, PORT_6LEDS_ALL_LIGHTS, 0x00); + var result = await WriteAsync(lightsOffCmd, token: token); + await DelayAsync(token); - // port configuration - for (int channel = 0; channel < NumberOfChannels; channel++) + // port configuration + for (int channel = 0; channel < NumberOfChannels; channel++) + { + var channelConfig = ChannelConfigs[channel]; + if (channelConfig.OutputType == ChannelOutputType.ServoMotor) { - var channelConfig = ChannelConfigs[channel]; - if (channelConfig.OutputType == ChannelOutputType.ServoMotor) - { - await SetupChannelForPortInformationAsync(channel, token); - await Task.Delay(300, token); - await ResetServoAsync(channel, channelConfig.ServoBaseAngle, token); - } + await SetupChannelForPortInformationAsync(channel, token); + await ResetServoAsync(channel, channelConfig.ServoBaseAngle, token); } - - return result; } - catch - { - } - } - return false; + return result; + } + catch + { + return false; + } } protected override void ResetOutputValues() { - if (_applyPlayVmMode) - { - // output values - clear always lights — suppress initial burst to avoid flooding the hub - _outputValues.Clear(); - } - else - { - // otherwise all channels to be initialized - _outputValues.Initialize(); - } + _outputValues.Clear(); _playVmValues.Clear(); _calibratedZeroAngle = default; } @@ -201,6 +197,19 @@ protected override async Task ProcessOutputsAsync(CancellationToken token) { try { + if (_applyPlayVmMode) + { + _playVmValues.Initialize(); + // output values - clear always lights — suppress initial burst to avoid flooding the hub + _outputValues.Clear(); + } + else + { + _playVmValues.Clear(); + // otherwise all channels to be initialized + _outputValues.Initialize(); + } + while (!token.IsCancellationRequested) { if (!await SendOutputValuesAsync(token).ConfigureAwait(false)) @@ -222,7 +231,9 @@ private async Task SetupChannelForPortInformationAsync(int channel, Cancel { // setup channel to report APOS position var inputFormatForAbsAngle = BuildPortInputFormatSetup(portId, PORT_MODE_3); - return await WriteAsync(inputFormatForAbsAngle, token); + await WriteAsync(inputFormatForAbsAngle, token); + await Task.Delay(300, token); + return true; } // setup channel to for APOS, but no notifications @@ -262,11 +273,30 @@ private async Task ResetServoAsync(int channel, int baseAngle, Cancellatio await WriteAsync(servoCmd, token: token); await Task.Delay(100, token); + // set up completion waiter before sending calibrate to avoid the race where + // feedback arrives before we start waiting + _playVmCalibrationTcs = new TaskCompletionSource(TaskCreationOptions.RunContinuationsAsynchronously); + // do calibration var calibrateCmd = BuildPortOutput_PlayVm(servoValue: baseAngle, vmCmd: PLAYVM_CALIBRATE_STEERING); await WriteAsync(calibrateCmd, token: token); - await AwaitStableAbsolutePositionAsync(channel, TimeSpan.FromSeconds(4), token); + // wait for the hub's completion feedback instead of position stability + try + { + await _playVmCalibrationTcs.Task.WaitAsync(TimeSpan.FromSeconds(4), token); + Dump("Reset Servo: PLAYVM", ChannelAbsPositions.Get(channel)); + } + catch (TimeoutException) + { + // hub did not respond in time, fall back to a short safety delay + await Task.Delay(500, token); + Dump("Reset Servo: TIMEOUT", ChannelAbsPositions.Get(channel)); + } + finally + { + _playVmCalibrationTcs = null; + } } else { @@ -275,7 +305,8 @@ private async Task ResetServoAsync(int channel, int baseAngle, Cancellatio var servoCmd = BuildPortOutput_GotoAbsPosition(portId, _calibratedZeroAngle + baseAngle, servoSpeed: 0x28); await WriteAsync(servoCmd, token: token); - await AwaitStableAbsolutePositionAsync(channel, TimeSpan.FromSeconds(1), token); + await AwaitStableAbsolutePositionAsync(channel, TimeSpan.FromSeconds(2), token); + Dump("Reset Servo: PLAYVM", ChannelAbsPositions.Get(channel)); } return true; diff --git a/BrickController2/BrickController2/Diagnostics/Logs.cs b/BrickController2/BrickController2/Diagnostics/Logs.cs new file mode 100644 index 00000000..3b5725af --- /dev/null +++ b/BrickController2/BrickController2/Diagnostics/Logs.cs @@ -0,0 +1,20 @@ +using System; +using System.Diagnostics; + +namespace BrickController2.Diagnostics; + +internal static class Logs +{ + [Conditional("DEBUG")] + public static void Dump(string label, ReadOnlySpan data) + { + var s = Convert.ToHexString(data); + Debug.WriteLine($"{DateTimeOffset.Now:HH:mm:ss.f} {label}: {s}"); + } + + [Conditional("DEBUG")] + public static void Dump(string label, T data) where T : struct + { + Debug.WriteLine($"{DateTimeOffset.Now:HH:mm:ss.f} {label}: [{data}]"); + } +} diff --git a/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs b/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs index d05ebbe8..d889cb21 100644 --- a/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs +++ b/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs @@ -20,6 +20,8 @@ internal static class LegoWirelessProtocol // message types public const byte MESSAGE_TYPE_HUB_PROPERTIES = 0x01; + public const byte MESSAGE_TYPE_HUB_ACTIONS = 0x02; + public const byte MESSAGE_TYPE_HUB_ATTACHED_IO = 0x04; public const byte MESSAGE_TYPE_HW_NETWORK_COMMANDS = 0x08; public const byte MESSAGE_TYPE_PORT_VALUE = 0x45; public const byte MESSAGE_TYPE_PORT_VALUE_COMBINED = 0x46; @@ -112,6 +114,7 @@ public static void ToBytes(int value, out byte b0, out byte b1, out byte b2, out public static short ToInt16(ReadOnlySpan value) => BinaryPrimitives.ReadInt16LittleEndian(value); public static int ToInt32(ReadOnlySpan value) => BinaryPrimitives.ReadInt32LittleEndian(value); + public static ushort ToUInt16(ReadOnlySpan value) => BinaryPrimitives.ReadUInt16LittleEndian(value); public static int NormalizeAngle(int angle) { From e428158e6810418f323b32bae2fce12bfa34e59e Mon Sep 17 00:00:00 2001 From: Vit Nemecky Date: Thu, 30 Apr 2026 22:47:12 +0200 Subject: [PATCH 11/29] clean up + finetune --- .../DeviceManagement/ControlPlusDevice.cs | 92 ++++------- .../DeviceManagement/Lego/RemoteControl.cs | 7 +- .../Lego/WirelessProtocolBasedDevice.cs | 40 ++++- .../DeviceManagement/TechnicMoveDevice.cs | 145 ++++++++---------- .../BrickController2/Diagnostics/Logs.cs | 2 +- .../Protocols/LegoWirelessProtocol.cs | 1 + 6 files changed, 132 insertions(+), 155 deletions(-) diff --git a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs index 5c55534a..49bd613a 100644 --- a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs @@ -70,6 +70,22 @@ public async override Task ConnectAsync( return await base.ConnectAsync(reconnect, onDeviceDisconnected, channelConfigurations, startOutputProcessing, requestDeviceInformation, token); } + public override void SetOutput(int channel, float value) + { + CheckChannel(channel); + value = CutOutputValue(value); + + var intValue = (int)(100 * value); + + lock (_outputLock) + { + if (_outputValues[channel] != intValue) + { + _outputValues[channel] = intValue; + _sendAttemptsLeft[channel] = MAX_SEND_ATTEMPTS; + } + } + } public override bool CanResetOutput(int channel) => true; @@ -95,35 +111,6 @@ public override async Task ResetOutputAsync(int channel, float value, Cancellati return await AutoCalibrateServoAsync(channel, token); } - public override void SetOutput(int channel, float value) - { - CheckChannel(channel); - value = CutOutputValue(value); - - var intValue = (int)(100 * value); - - lock (_outputLock) - { - if (_outputValues[channel] != intValue) - { - _outputValues[channel] = intValue; - _sendAttemptsLeft[channel] = MAX_SEND_ATTEMPTS; - } - } - } - - protected override void ResetOutputValues() - { - lock (_outputLock) - { - for (int c = 0; c < NumberOfChannels; c++) - { - _outputValues[c] = 0; - _lastOutputValues[c] = 0; - } - } - } - protected virtual byte GetChannelValue(int value) // calculate raw motor value => (byte)(value < 0 ? (255 + value) : value); @@ -161,43 +148,20 @@ protected virtual byte[] GetServoCommand(int channel, int servoValue, int servoS return _servoSendBuffer; } - protected override async Task ProcessOutputsAsync(CancellationToken token) + protected override void ResetOutputValues() { - try - { - lock (_outputLock) - { - for (int channel = 0; channel < NumberOfChannels; channel++) - { - InitializeChannelInfo(channel); - } - } - _lastSent_NormalMotor.Reset(); + base.ResetOutputValues(); - while (!token.IsCancellationRequested) + lock (_outputLock) + { + for (int channel = 0; channel < NumberOfChannels; channel++) { - if (!await SendOutputValuesAsync(token).ConfigureAwait(false)) - { - await Task.Delay(10, token).ConfigureAwait(false); - } + _outputValues[channel] = 0; + _lastOutputValues[channel] = 1; + _sendAttemptsLeft[channel] = MAX_SEND_ATTEMPTS; } } - catch { } - } - - /// - /// Initialize channel data when output processing is going to be started - /// - protected virtual void InitializeChannelInfo(int channel, - int lastOutputValue = 1, - int sendAttemptsLeft = MAX_SEND_ATTEMPTS) - { - _outputValues[channel] = 0; - _lastOutputValues[channel] = lastOutputValue; - _sendAttemptsLeft[channel] = sendAttemptsLeft; - - ChannelAbsPositions.Set(channel); - ChannelRelativePositions.Set(channel); + _lastSent_NormalMotor.Reset(); } protected override async Task AfterConnectSetupAsync(bool requestDeviceInformation, CancellationToken token) @@ -205,7 +169,7 @@ protected override async Task AfterConnectSetupAsync(bool requestDeviceInf try { // Wait until ports finish communicating with the hub - await Task.Delay(1000, token); + await AwaitForHubConnectedAsync(TimeSpan.FromMilliseconds(1000), token); if (requestDeviceInformation) { @@ -231,7 +195,7 @@ protected override async Task AfterConnectSetupAsync(bool requestDeviceInf } } - private async Task SendOutputValuesAsync(CancellationToken token) + protected override async Task SendOutputValuesAsync(CancellationToken token) { try { @@ -315,7 +279,7 @@ private async Task SendOutputValueVirtualAsync(int virtualChannel, int cha _virtualPortSendBuffer[6] = (byte)(value1 < 0 ? (255 + value1) : value1); _virtualPortSendBuffer[7] = (byte)(value2 < 0 ? (255 + value2) : value2); - if (await WriteAsync(_virtualPortSendBuffer, token)) + if (await WriteNoResponseAsync(_virtualPortSendBuffer, token)) { _lastOutputValues[channel1] = value1; _lastOutputValues[channel2] = value2; diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs b/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs index 890860df..cc50079f 100644 --- a/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs +++ b/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs @@ -62,7 +62,7 @@ internal void ResetEvents() => RaiseButtonEvents( protected override async Task AfterConnectSetupAsync(bool requestDeviceInformation, CancellationToken token) { // wait until ports finish communicating with the hub - await Task.Delay(250, token); + await AwaitForHubConnectedAsync(TimeSpan.FromMilliseconds(250), token); if (requestDeviceInformation) { @@ -77,11 +77,6 @@ protected override async Task AfterConnectSetupAsync(bool requestDeviceInf return await WriteAsync(remoteButtonB, token); } - protected override void ResetOutputValues() - { - // nothing to reset - } - protected override bool TryProcessMessageData(byte messageType, ReadOnlySpan data) { switch (messageType) diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs index 5c566be5..0a6accde 100644 --- a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs @@ -88,7 +88,36 @@ protected virtual bool TryGetChannelIndex(byte portId, out int channelIndex) return portId < NumberOfChannels; } - protected abstract void ResetOutputValues(); + protected virtual void ResetOutputValues() + { + // reset output values & positions + ChannelAbsPositions.Clear(); + ChannelRelativePositions.Clear(); + AttachedHubs.Clear(); + } + + protected override async Task ProcessOutputsAsync(CancellationToken token) + { + try + { + // initialize + ResetOutputValues(); + + while (!token.IsCancellationRequested) + { + if (!await SendOutputValuesAsync(token).ConfigureAwait(false)) + { + await Task.Delay(10, token).ConfigureAwait(false); + } + } + } + catch (Exception ex) + { + Dump("ProcessOutputsAsync: EXCEPTION", ex); + } + } + + protected virtual Task SendOutputValuesAsync(CancellationToken token) => throw new InvalidOperationException(nameof(SendOutputValuesAsync)); protected override async Task ValidateServicesAsync(IEnumerable? services, CancellationToken token) { @@ -245,12 +274,11 @@ protected virtual bool TryProcessMessageData(byte messageType, ReadOnlySpan WriteNoResponseAsync(byte[] data, CancellationTo protected async ValueTask WriteAsync(byte[] data, CancellationToken token = default) => await _bleDevice!.WriteAsync(Characteristic!, data, token); - protected virtual void OnPortOutputCommandFeedback(ReadOnlySpan data) { } - protected static Task DelayAsync(CancellationToken token = default) => Task.Delay(20, token); protected ChannelOutputType GetOutputType(int channel) => (channel < 0 || channel >= NumberOfChannels) diff --git a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs index 544b344e..addf8b6a 100644 --- a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs @@ -122,15 +122,6 @@ protected override bool TryGetChannelIndex(byte portId, out int channelIndex) return channelIndex >= 0; } - protected override void OnPortOutputCommandFeedback(ReadOnlySpan data) - { - // PORT_PLAYVM completion feedback (0x82) signals calibration finished - if (data.Length >= 5 && data[3] == PORT_PLAYVM && (data[4] & 0x02) != 0) - { - _playVmCalibrationTcs?.TrySetResult(true); - } - } - protected override async ValueTask BeforeDisconnectAsync(CancellationToken token) { await base.BeforeDisconnectAsync(token); @@ -188,37 +179,80 @@ protected override async Task AfterConnectSetupAsync(bool requestDeviceInf protected override void ResetOutputValues() { - _outputValues.Clear(); - _playVmValues.Clear(); + base.ResetOutputValues(); + if (_applyPlayVmMode) + { + _playVmValues.Initialize(); + // output values - clear always lights — suppress initial burst to avoid flooding the hub + _outputValues.Clear(); + } + else + { + _playVmValues.Clear(); + // otherwise all channels to be initialized + _outputValues.Initialize(); + } _calibratedZeroAngle = default; } - protected override async Task ProcessOutputsAsync(CancellationToken token) + protected override async Task SendOutputValuesAsync(CancellationToken token) { try { - if (_applyPlayVmMode) - { - _playVmValues.Initialize(); - // output values - clear always lights — suppress initial burst to avoid flooding the hub - _outputValues.Clear(); - } - else - { - _playVmValues.Clear(); - // otherwise all channels to be initialized - _outputValues.Initialize(); - } + // conditionally send PLAYVM command if PLAYVM mode is active + var result = await SendPlayVmOutputValueAsync(token); - while (!token.IsCancellationRequested) + // process changes for other channels as it's a light or a classic drive + if (result && _outputValues.TryGetChanges(out var changes)) { - if (!await SendOutputValuesAsync(token).ConfigureAwait(false)) + foreach (KeyValuePair change in changes) { - await Task.Delay(10, token).ConfigureAwait(false); + 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 (with servo support) + CHANNEL_C when channelOutputType == ChannelOutputType.ServoMotor => await SendServoValue(change.Key, value, token), + _ => await SendPortOutput_ValueAsync(change.Key, value, token), + }; + + if (!result) + { + return false; + } } + + _outputValues.Commit(); } + + return result; + } + catch + { + return false; + } + } + + protected override bool TryProcessMessageData(byte messageType, ReadOnlySpan data) + { + switch (messageType) + { + case MESSAGE_TYPE_OUTPUT_COMMAND_FEEDBACK: // Port output command feedback + Dump("Output command feedback", data); + // PORT_PLAYVM completion feedback (0x82) signals calibration finished + if (data.Length >= 5 && data[3] == PORT_PLAYVM && (data[4] & 0x02) != 0) + { + _playVmCalibrationTcs?.TrySetResult(true); + return true; + } + break; } - catch { } + return base.TryProcessMessageData(messageType, data); } private async Task SetupChannelForPortInformationAsync(int channel, CancellationToken token) @@ -281,17 +315,16 @@ private async Task ResetServoAsync(int channel, int baseAngle, Cancellatio var calibrateCmd = BuildPortOutput_PlayVm(servoValue: baseAngle, vmCmd: PLAYVM_CALIBRATE_STEERING); await WriteAsync(calibrateCmd, token: token); - // wait for the hub's completion feedback instead of position stability + // wait for the hub's completion feedback try { await _playVmCalibrationTcs.Task.WaitAsync(TimeSpan.FromSeconds(4), token); - Dump("Reset Servo: PLAYVM", ChannelAbsPositions.Get(channel)); + Dump("Command feedback: PLAYVM", ChannelAbsPositions.Get(channel)); } catch (TimeoutException) { // hub did not respond in time, fall back to a short safety delay await Task.Delay(500, token); - Dump("Reset Servo: TIMEOUT", ChannelAbsPositions.Get(channel)); } finally { @@ -304,55 +337,13 @@ private async Task ResetServoAsync(int channel, int baseAngle, Cancellatio var portId = GetPortId(channel); var servoCmd = BuildPortOutput_GotoAbsPosition(portId, _calibratedZeroAngle + baseAngle, servoSpeed: 0x28); await WriteAsync(servoCmd, token: token); - - await AwaitStableAbsolutePositionAsync(channel, TimeSpan.FromSeconds(2), token); - Dump("Reset Servo: PLAYVM", ChannelAbsPositions.Get(channel)); } - return true; - } - catch - { - return false; - } - } + // Wait for position to stabilize before allowing the output loop to start + await AwaitStableAbsolutePositionAsync(channel, TimeSpan.FromSeconds(4), token); + Dump("Reset Servo", ChannelAbsPositions.Get(channel)); - private async Task SendOutputValuesAsync(CancellationToken token) - { - try - { - // conditionally send PLAYVM command if PLAYVM mode is active - var result = await SendPlayVmOutputValueAsync(token); - - // process changes for other channels as it's a light or a classic drive - if (result && _outputValues.TryGetChanges(out var changes)) - { - foreach (KeyValuePair 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 (with servo support) - CHANNEL_C when channelOutputType == ChannelOutputType.ServoMotor => await SendServoValue(change.Key, value, token), - _ => await SendPortOutput_ValueAsync(change.Key, value, token), - }; - - if (!result) - { - return false; - } - } - - _outputValues.Commit(); - } - - return result; + return true; } catch { diff --git a/BrickController2/BrickController2/Diagnostics/Logs.cs b/BrickController2/BrickController2/Diagnostics/Logs.cs index 3b5725af..f5a8eeba 100644 --- a/BrickController2/BrickController2/Diagnostics/Logs.cs +++ b/BrickController2/BrickController2/Diagnostics/Logs.cs @@ -13,7 +13,7 @@ public static void Dump(string label, ReadOnlySpan data) } [Conditional("DEBUG")] - public static void Dump(string label, T data) where T : struct + public static void Dump(string label, T data) { Debug.WriteLine($"{DateTimeOffset.Now:HH:mm:ss.f} {label}: [{data}]"); } diff --git a/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs b/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs index d889cb21..3ee73ea5 100644 --- a/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs +++ b/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs @@ -25,6 +25,7 @@ internal static class LegoWirelessProtocol public const byte MESSAGE_TYPE_HW_NETWORK_COMMANDS = 0x08; public const byte MESSAGE_TYPE_PORT_VALUE = 0x45; public const byte MESSAGE_TYPE_PORT_VALUE_COMBINED = 0x46; + public const byte MESSAGE_TYPE_OUTPUT_COMMAND_FEEDBACK = 0x82; // TechnicMove hub ports public const byte PORT_DRIVE_MOTOR_1 = 0x32; From d7bccda3970343b08ca97078fb759bdeb9db689f Mon Sep 17 00:00:00 2001 From: Vit Nemecky Date: Thu, 30 Apr 2026 23:09:29 +0200 Subject: [PATCH 12/29] nitpicks --- .../DeviceManagement/ControlPlusDevice.cs | 12 ++++++------ .../DeviceManagement/Lego/ChannelAttachmentInfo.cs | 5 ----- .../Lego/WirelessProtocolBasedDevice.cs | 4 +++- 3 files changed, 9 insertions(+), 12 deletions(-) diff --git a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs index 49bd613a..10743f25 100644 --- a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs @@ -430,7 +430,7 @@ protected virtual async Task ResetServoAsync(int channel, int baseAngle, C { baseAngle = Math.Max(-180, Math.Min(179, baseAngle)); - var resetToAngle = NormalizeAngle(ChannelAbsPositions.Get(channel).Current - baseAngle); + var resetToAngle = NormalizeAngle(GetAbsPosition(channel) - baseAngle); var result = true; @@ -444,7 +444,7 @@ protected virtual async Task ResetServoAsync(int channel, int baseAngle, C await Task.Delay(500, token); result = result && await StopAsync(channel, token); - var diff = Math.Abs(NormalizeAngle(ChannelAbsPositions.Get(channel).Current - baseAngle)); + var diff = Math.Abs(NormalizeAngle(GetAbsPosition(channel) - baseAngle)); if (diff > 5) { // Can't reset to base angle, rebase to current position not to stress the plastic @@ -475,17 +475,17 @@ protected virtual async Task ResetServoAsync(int channel, int baseAngle, C await Task.Delay(600, token); result = result && await StopAsync(channel, token); await Task.Delay(500, token); - var absPositionAt0 = ChannelAbsPositions.Get(channel).Current; + var absPositionAt0 = GetAbsPosition(channel); result = result && await TurnAsync(channel, -160, 60, token); await Task.Delay(600, token); result = result && await StopAsync(channel, token); await Task.Delay(500, token); - var absPositionAtMin160 = ChannelAbsPositions.Get(channel).Current; + var absPositionAtMin160 = GetAbsPosition(channel); result = result && await TurnAsync(channel, 160, 60, token); await Task.Delay(600, token); result = result && await StopAsync(channel, token); await Task.Delay(500, token); - var absPositionAt160 = ChannelAbsPositions.Get(channel).Current; + var absPositionAt160 = GetAbsPosition(channel); var midPoint1 = NormalizeAngle((absPositionAtMin160 + absPositionAt160) / 2); var midPoint2 = NormalizeAngle(midPoint1 + 180); @@ -493,7 +493,7 @@ protected virtual async Task ResetServoAsync(int channel, int baseAngle, C var baseAngle = (Math.Abs(NormalizeAngle(midPoint1 - absPositionAt0)) < Math.Abs(NormalizeAngle(midPoint2 - absPositionAt0))) ? RoundAngleToNearest90(midPoint1) : RoundAngleToNearest90(midPoint2); - var resetToAngle = NormalizeAngle(ChannelAbsPositions.Get(channel).Current - baseAngle); + var resetToAngle = NormalizeAngle(GetAbsPosition(channel) - baseAngle); result = result && await ResetAsync(channel, 0, token); result = result && await StopAsync(channel, token); diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/ChannelAttachmentInfo.cs b/BrickController2/BrickController2/DeviceManagement/Lego/ChannelAttachmentInfo.cs index 54c25cc7..ffeec17a 100644 --- a/BrickController2/BrickController2/DeviceManagement/Lego/ChannelAttachmentInfo.cs +++ b/BrickController2/BrickController2/DeviceManagement/Lego/ChannelAttachmentInfo.cs @@ -1,5 +1,4 @@ using System; -using System.Numerics; namespace BrickController2.DeviceManagement.Lego; @@ -19,8 +18,4 @@ internal record struct ChannelAttachmentInfo( /// Returns a new state with an updated device ID and timestamp. public ChannelAttachmentInfo WithDevice(ushort deviceId) => this with { DeviceId = deviceId, IsUpdated = true, UpdateTime = DateTime.Now }; - - /// Clears the IsUpdated flag after the update has been consumed. - public ChannelAttachmentInfo ConsumeUpdate() => - this with { IsUpdated = false }; } diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs index 0a6accde..ae227e8f 100644 --- a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs @@ -311,6 +311,8 @@ protected int GetMaxServoAngle(int channel) return maxServoAngle > 0 ? maxServoAngle : DEFAULT_MAX_SERVO_ANGLE; } + protected int GetAbsPosition(int channel) => ChannelAbsPositions.Get(channel).Current; + protected async ValueTask RequestHubPropertiesAsync(CancellationToken token) { try @@ -398,7 +400,7 @@ protected int CalculateCalibratedTarget(int channel, int targetBaseAngle = 0) int diff = NormalizeAngle(normalizedTarget - normalizedRelative); // Offset the current accumulated position by the physical difference - return ChannelAbsPositions.Get(channel).Current + diff; + return GetAbsPosition(channel) + diff; } protected Task AwaitStableRelativePositionAsync(int channel, TimeSpan timeout, CancellationToken token) From 1935dbd832648164df4c1f00f3c0c00d454d586b Mon Sep 17 00:00:00 2001 From: vicocz Date: Fri, 8 May 2026 22:31:53 +0200 Subject: [PATCH 13/29] finetune + review comments --- .../DeviceManagement/IO/StateStoreTests.cs | 85 +++++++++++++++++++ .../Protocols/LegoWirelessProtocolTests.cs | 43 ++++++++++ .../DeviceManagement/ControlPlusDevice.cs | 10 +-- .../DeviceManagement/IO/ChannelConfig.cs | 4 + .../DeviceManagement/IO/ChannelStateStore.cs | 60 +------------ .../DeviceManagement/IO/StateStore.cs | 49 +++++++++++ .../Lego/ChannelAttachmentInfo.cs | 2 +- .../DeviceManagement/Lego/RemoteControl.cs | 2 +- .../Lego/WirelessProtocolBasedDevice.cs | 68 +++++++-------- .../DeviceManagement/TechnicMoveDevice.cs | 16 ++-- .../Protocols/LegoWirelessProtocol.cs | 14 +-- 11 files changed, 233 insertions(+), 120 deletions(-) create mode 100644 BrickController2/BrickController2.Tests/DeviceManagement/IO/StateStoreTests.cs create mode 100644 BrickController2/BrickController2.Tests/Protocols/LegoWirelessProtocolTests.cs create mode 100644 BrickController2/BrickController2/DeviceManagement/IO/StateStore.cs diff --git a/BrickController2/BrickController2.Tests/DeviceManagement/IO/StateStoreTests.cs b/BrickController2/BrickController2.Tests/DeviceManagement/IO/StateStoreTests.cs new file mode 100644 index 00000000..c93630b6 --- /dev/null +++ b/BrickController2/BrickController2.Tests/DeviceManagement/IO/StateStoreTests.cs @@ -0,0 +1,85 @@ +using BrickController2.DeviceManagement.IO; +using FluentAssertions; +using Xunit; + +namespace BrickController2.Tests.DeviceManagement.IO; + +public class StateStoreTests +{ + private readonly StateStore _sut = new(initialState: 0); + + [Fact] + public void Get_ReturnsDefault_WhenKeyNotPresent() + { + _sut.Get("missing").Should().Be(0); + } + + [Fact] + public void Set_AndGet_ReturnStoredValue() + { + _sut.Set("key", 42); + _sut.Get("key").Should().Be(42); + } + + [Fact] + public void Count_ReflectsNumberOfKeys() + { + _sut.Set("a", 1); + _sut.Set("b", 2); + _sut.Count.Should().Be(2); + } + + [Fact] + public void Remove_ReturnsTrueAndRemovesKey() + { + _sut.Set("key", 10); + _sut.Remove("key").Should().BeTrue(); + _sut.Get("key").Should().Be(0); + } + + [Fact] + public void Remove_ReturnsFalse_WhenKeyNotPresent() + { + _sut.Remove("ghost").Should().BeFalse(); + } + + [Fact] + public void Update_AppliesUpdaterToExistingValue() + { + _sut.Set("key", 5); + var result = _sut.Update("key", v => v + 3); + result.Should().Be(8); + _sut.Get("key").Should().Be(8); + } + + [Fact] + public void Update_UsesDefaultWhenKeyNotPresent() + { + var result = _sut.Update("new", v => v + 7); + result.Should().Be(7); + } + + [Fact] + public void Clear_RemovesAllEntries() + { + _sut.Set("a", 1); + _sut.Set("b", 2); + _sut.Clear(); + _sut.Count.Should().Be(0); + } + + [Fact] + public void Max_ReturnsMaxProjectedValue() + { + _sut.Set("a", 3); + _sut.Set("b", 7); + _sut.Set("c", 1); + _sut.Max(v => v).Should().Be(7); + } + + [Fact] + public void Max_ReturnsDefault_WhenEmpty() + { + _sut.Max(v => v).Should().Be(0); + } +} \ No newline at end of file diff --git a/BrickController2/BrickController2.Tests/Protocols/LegoWirelessProtocolTests.cs b/BrickController2/BrickController2.Tests/Protocols/LegoWirelessProtocolTests.cs new file mode 100644 index 00000000..944d5cc7 --- /dev/null +++ b/BrickController2/BrickController2.Tests/Protocols/LegoWirelessProtocolTests.cs @@ -0,0 +1,43 @@ +using BrickController2.Protocols; +using FluentAssertions; +using Xunit; + +namespace BrickController2.Tests.Protocols; + +public class LegoWirelessProtocolTests +{ + [Theory] + [InlineData(0)] + [InlineData(179)] + [InlineData(-179)] + [InlineData(-180)] + [InlineData(90)] + [InlineData(-90)] + public void NormalizeAngle_WithinRange_ShouldReturnSameValue(int value) + { + var result = LegoWirelessProtocol.NormalizeAngle(value); + result.Should().Be(value); + } + + [Theory] + [InlineData(180, -180)] + [InlineData(360, 0)] + [InlineData(540, -180)] + public void NormalizeAngle_OverPlus180_ShouldReturnNormalizedValue(int input, int expected) + { + var result = LegoWirelessProtocol.NormalizeAngle(input); + result.Should().Be(expected); + } + + [Theory] + [InlineData(-541, 179)] + [InlineData(-540, -180)] + [InlineData(-539, -179)] + [InlineData(-360, 0)] + [InlineData(-181, 179)] + public void NormalizeAngle_BelowMinus180_ShouldReturnNormalizedValue(int input, int expected) + { + var result = LegoWirelessProtocol.NormalizeAngle(input); + result.Should().Be(expected); + } +} \ No newline at end of file diff --git a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs index 10743f25..f53cc7a5 100644 --- a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs @@ -169,7 +169,7 @@ protected override async Task AfterConnectSetupAsync(bool requestDeviceInf try { // Wait until ports finish communicating with the hub - await AwaitForHubConnectedAsync(TimeSpan.FromMilliseconds(1000), token); + await AwaitForPeripheralsAttachedAsync(TimeSpan.FromMilliseconds(1000), token); if (requestDeviceInformation) { @@ -178,7 +178,7 @@ protected override async Task AfterConnectSetupAsync(bool requestDeviceInf for (int channel = 0; channel < NumberOfChannels; channel++) { - var channelConfig = ChannelConfigs[channel]; + var channelConfig = ChannelConfigs.Get(channel); if (channelConfig.OutputType == ChannelOutputType.ServoMotor) { await SetupChannelForPortInformationAsync(channel, token); @@ -203,7 +203,7 @@ protected override async Task SendOutputValuesAsync(CancellationToken toke for (int channel = 0; channel < NumberOfChannels; channel++) { - var outputType = ChannelConfigs[channel].OutputType; + var outputType = GetOutputType(channel); switch (outputType) { case ChannelOutputType.NormalMotor: @@ -316,7 +316,7 @@ private async Task SendServoOutputValueAsync(int channel, CancellationToke if (v != _lastOutputValues[channel] || sendAttemptsLeft > 0) { - var servoValue = ChannelConfigs[channel].MaxServoAngle * v / 100; + var servoValue = GetMaxServoAngle(channel) * v / 100; var servoSpeed = CalculateServoSpeed(channel, servoValue); if (servoSpeed == 0) @@ -359,7 +359,7 @@ private async Task SendStepperOutputValueAsync(int channel, CancellationTo _sendAttemptsLeft[channel] = sendAttemptsLeft > 0 ? sendAttemptsLeft - 1 : 0; } - var stepperAngle = ChannelConfigs[channel].StepperAngle; + var stepperAngle = ChannelConfigs.Get(channel).StepperAngle; _stepperSendBuffer[3] = GetPortId(channel); _stepperSendBuffer[6] = (byte)(stepperAngle & 0xff); _stepperSendBuffer[7] = (byte)((stepperAngle >> 8) & 0xff); diff --git a/BrickController2/BrickController2/DeviceManagement/IO/ChannelConfig.cs b/BrickController2/BrickController2/DeviceManagement/IO/ChannelConfig.cs index 96513b7a..3407956d 100644 --- a/BrickController2/BrickController2/DeviceManagement/IO/ChannelConfig.cs +++ b/BrickController2/BrickController2/DeviceManagement/IO/ChannelConfig.cs @@ -2,6 +2,10 @@ namespace BrickController2.DeviceManagement.IO; +/// +/// Describes configuration of a single channel. +/// This is used to determine how to control the channel. +/// internal readonly record struct ChannelConfig { public ChannelOutputType OutputType { get; init; } diff --git a/BrickController2/BrickController2/DeviceManagement/IO/ChannelStateStore.cs b/BrickController2/BrickController2/DeviceManagement/IO/ChannelStateStore.cs index baf88ea6..a93df4fa 100644 --- a/BrickController2/BrickController2/DeviceManagement/IO/ChannelStateStore.cs +++ b/BrickController2/BrickController2/DeviceManagement/IO/ChannelStateStore.cs @@ -1,63 +1,9 @@ -using System; -using System.Collections.Concurrent; -using System.Collections.Generic; +namespace BrickController2.DeviceManagement.IO; -namespace BrickController2.DeviceManagement.IO; - -/// -/// Thread-safe, key-indexed store for per-key state structs. -/// Supports atomic read, write, and functional update. -/// -internal sealed class ChannelStateStore - where TKey : notnull +internal class ChannelStateStore : StateStore where TValue : struct { - private readonly ConcurrentDictionary _states = new(); - private readonly TValue _default; - - public ChannelStateStore(TValue initialState = default) - { - _states = new(); - _default = initialState; - } - - public int Count => _states.Count; - - /// Returns the current state for the given key or the default state if the key does not exist. - public TValue Get(TKey key) => _states.TryGetValue(key, out var value) ? value : _default; - - /// Removes the current state for the given key. - public bool Remove(TKey key) => _states.TryRemove(key, out var _); - - /// Upsert the state for the given key. - public void Set(TKey key, TValue state = default) - { - _states[key] = state; - } - - /// - /// Atomically updates the state for the given key using the provided updater function. - /// Returns the new state. - /// - public TValue Update(TKey key, Func updater) => _states.AddOrUpdate(key, (k) => updater(_default), (k, o) => updater(o)); - - /// Clears all persisted states. - public void Clear() => _states.Clear(); - - /// Returns the maximum value of a projection over all stored states, or default if empty. - public TResult? Max(Func selector) + public ChannelStateStore(TValue initialState = default) : base(initialState) { - TResult? max = default; - bool first = true; - foreach (var kvp in _states) - { - var val = selector(kvp.Value); - if (first || Comparer.Default.Compare(val, max) > 0) - { - max = val; - first = false; - } - } - return max; } } diff --git a/BrickController2/BrickController2/DeviceManagement/IO/StateStore.cs b/BrickController2/BrickController2/DeviceManagement/IO/StateStore.cs new file mode 100644 index 00000000..41dc3f29 --- /dev/null +++ b/BrickController2/BrickController2/DeviceManagement/IO/StateStore.cs @@ -0,0 +1,49 @@ +using System; +using System.Collections.Concurrent; +using System.Linq; + +namespace BrickController2.DeviceManagement.IO; + +/// +/// Thread-safe, key-indexed store for per-key state structs. +/// Supports atomic read, write, and functional update. +/// +internal class StateStore + where TKey : notnull + where TValue : struct +{ + private readonly ConcurrentDictionary _states = new(); + private readonly TValue _default; + + public StateStore(TValue initialState = default) + { + _states = new(); + _default = initialState; + } + + public int Count => _states.Count; + + /// Returns the current state for the given key or the default state if the key does not exist. + public TValue Get(TKey key) => _states.TryGetValue(key, out var value) ? value : _default; + + /// Removes the current state for the given key. + public bool Remove(TKey key) => _states.TryRemove(key, out var _); + + /// Upsert the state for the given key. + public void Set(TKey key, TValue state = default) + { + _states[key] = state; + } + + /// + /// Atomically updates the state for the given key using the provided updater function. + /// Returns the new state. + /// + public TValue Update(TKey key, Func updater) => _states.AddOrUpdate(key, (k) => updater(_default), (k, o) => updater(o)); + + /// Clears all persisted states. + public void Clear() => _states.Clear(); + + /// Returns the maximum value of a projection over all stored states, or default if empty. + public TResult? Max(Func selector) => _states.Values.Max(x => selector(x)); +} diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/ChannelAttachmentInfo.cs b/BrickController2/BrickController2/DeviceManagement/Lego/ChannelAttachmentInfo.cs index ffeec17a..52758e2c 100644 --- a/BrickController2/BrickController2/DeviceManagement/Lego/ChannelAttachmentInfo.cs +++ b/BrickController2/BrickController2/DeviceManagement/Lego/ChannelAttachmentInfo.cs @@ -3,7 +3,7 @@ namespace BrickController2.DeviceManagement.Lego; /// -/// Immutable snapshot of a motor channel's position feedback. +/// Snapshot of peripheral attachment for a port. /// internal record struct ChannelAttachmentInfo( ushort DeviceId, diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs b/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs index cc50079f..46616fc8 100644 --- a/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs +++ b/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs @@ -62,7 +62,7 @@ internal void ResetEvents() => RaiseButtonEvents( protected override async Task AfterConnectSetupAsync(bool requestDeviceInformation, CancellationToken token) { // wait until ports finish communicating with the hub - await AwaitForHubConnectedAsync(TimeSpan.FromMilliseconds(250), token); + await AwaitForPeripheralsAttachedAsync(TimeSpan.FromMilliseconds(250), token); if (requestDeviceInformation) { diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs index ae227e8f..07f0c224 100644 --- a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs @@ -16,21 +16,21 @@ namespace BrickController2.DeviceManagement.Lego; internal abstract class WirelessProtocolBasedDevice : BluetoothDevice { - protected readonly ChannelConfig[] ChannelConfigs; - protected readonly ChannelStateStore ChannelAbsPositions; - protected readonly ChannelStateStore ChannelRelativePositions; + protected readonly ChannelStateStore ChannelConfigs; + protected readonly ChannelStateStore ChannelAbsPositions; + protected readonly ChannelStateStore ChannelRelativePositions; - protected readonly ChannelStateStore AttachedHubs; + protected readonly ChannelStateStore AttachedPeripherals; protected IGattCharacteristic? Characteristic; protected WirelessProtocolBasedDevice(string name, string address, IDeviceRepository deviceRepository, IBluetoothLEService bleService) : base(name, address, deviceRepository, bleService) { - ChannelConfigs = new ChannelConfig[NumberOfChannels]; + ChannelConfigs = new(); ChannelAbsPositions = new(ChannelPositionState.Initial); ChannelRelativePositions = new(ChannelPositionState.Initial); - AttachedHubs = new(ChannelAttachmentInfo.Initial); + AttachedPeripherals = new(ChannelAttachmentInfo.Initial); } public override string BatteryVoltageSign => "%"; @@ -48,20 +48,23 @@ public override Task ConnectAsync( ResetOutputValues(); ChannelAbsPositions.Clear(); ChannelRelativePositions.Clear(); - AttachedHubs.Clear(); + AttachedPeripherals.Clear(); // Initialize configuration per channel - // build dictionary, but for supported ones only - var configs = channelConfigurations - .Where(c => IsOutputTypeSupported(c.Channel, c.ChannelOutputType)) - .ToDictionary(c => c.Channel, c => c); + // Build dictionary, but for supported ones only + var configs = new Dictionary(); + foreach (var channelConfiguration in channelConfigurations.Where(c => IsOutputTypeSupported(c.Channel, c.ChannelOutputType))) + { + // If multiple configurations target the same channel, keep the last one. + configs[channelConfiguration.Channel] = channelConfiguration; + } for (int i = 0; i < NumberOfChannels; i++) { configs.TryGetValue(i, out var config); - ChannelConfigs[i] = config.ChannelOutputType switch + ChannelConfigs.Set(i, config.ChannelOutputType switch { ChannelOutputType.ServoMotor => new() { @@ -75,7 +78,7 @@ public override Task ConnectAsync( StepperAngle = config.StepperAngle }, _ => new() - }; + }); } return base.ConnectAsync(reconnect, onDeviceDisconnected, channelConfigurations, startOutputProcessing, requestDeviceInformation, token); @@ -93,7 +96,7 @@ protected virtual void ResetOutputValues() // reset output values & positions ChannelAbsPositions.Clear(); ChannelRelativePositions.Clear(); - AttachedHubs.Clear(); + AttachedPeripherals.Clear(); } protected override async Task ProcessOutputsAsync(CancellationToken token) @@ -229,11 +232,11 @@ protected virtual bool TryProcessMessageData(byte messageType, ReadOnlySpan info.WithDevice(deviceId)); // store portId as "position" for simplicity + AttachedPeripherals.Update(channel, info => info.WithDevice(deviceId)); // store portId as "position" for simplicity } else if (eventType == 0x00) { - AttachedHubs.Remove(channel); + AttachedPeripherals.Remove(channel); } } return true; @@ -298,16 +301,11 @@ protected async ValueTask WriteAsync(byte[] data, CancellationToken token protected static Task DelayAsync(CancellationToken token = default) => Task.Delay(20, token); - protected ChannelOutputType GetOutputType(int channel) => (channel < 0 || channel >= NumberOfChannels) - ? ChannelOutputType.NormalMotor // fallback to a default type if out of range - : ChannelConfigs[channel].OutputType; + protected ChannelOutputType GetOutputType(int channel) => ChannelConfigs.Get(channel).OutputType; protected int GetMaxServoAngle(int channel) { - var maxServoAngle = channel >= 0 && channel < NumberOfChannels - ? ChannelConfigs[channel].MaxServoAngle - : default; - + var maxServoAngle = ChannelConfigs.Get(channel).MaxServoAngle; return maxServoAngle > 0 ? maxServoAngle : DEFAULT_MAX_SERVO_ANGLE; } @@ -404,25 +402,24 @@ protected int CalculateCalibratedTarget(int channel, int targetBaseAngle = 0) } protected Task AwaitStableRelativePositionAsync(int channel, TimeSpan timeout, CancellationToken token) - { - return WaitForStablePositionAsync(timeout, () => ChannelRelativePositions.Get(channel), token); - } + => WaitForStablePositionAsync(timeout, () => ChannelRelativePositions.Get(channel), token); protected Task AwaitStableAbsolutePositionAsync(int channel, TimeSpan timeout, CancellationToken token) - { - return WaitForStablePositionAsync(timeout, () => ChannelAbsPositions.Get(channel), token); - } + => WaitForStablePositionAsync(timeout, () => ChannelAbsPositions.Get(channel), token); + + protected Task AwaitForPeripheralsAttachedAsync(TimeSpan timeout, CancellationToken token) + => WaitForTimestampStabilityAsync(timeout, () => AttachedPeripherals.Max(x => x.UpdateTime), token); - protected async Task AwaitForHubConnectedAsync(TimeSpan timeout, CancellationToken token) + private static async Task WaitForTimestampStabilityAsync(TimeSpan timeout, Func getTimestamp, CancellationToken token) { var interval = TimeSpan.FromMilliseconds(50); - var stabilityTimeout = TimeSpan.FromMilliseconds(250); + var stabilityTimeout = TimeSpan.FromMilliseconds(200); using var linkedCts = CancellationTokenSource.CreateLinkedTokenSource(token); linkedCts.CancelAfter(timeout); var stableSince = Stopwatch.StartNew(); - var lastUpdated = AttachedHubs.Max(x => x.UpdateTime); + var lastUpdated = getTimestamp(); try { @@ -430,7 +427,7 @@ protected async Task AwaitForHubConnectedAsync(TimeSpan timeout, CancellationTok { await Task.Delay(interval, linkedCts.Token); - var currentValue = AttachedHubs.Max(x => x.UpdateTime); + var currentValue = getTimestamp(); if (currentValue != lastUpdated) { lastUpdated = currentValue; @@ -438,16 +435,15 @@ protected async Task AwaitForHubConnectedAsync(TimeSpan timeout, CancellationTok } else if (stableSince.Elapsed >= stabilityTimeout) { - Dump("Hub connection: HUBS", AttachedHubs.Count); return; // position stable for the required duration } } - Dump("Hub connection: TIMEOUT", lastUpdated); + Dump("TimestampStability: TIMEOUT", lastUpdated); } catch (OperationCanceledException) when (!token.IsCancellationRequested) { // total timeout elapsed — treat as completed - Dump("Hub connection: CANCELLED", lastUpdated); + Dump("TimestampStability: CANCELLED", lastUpdated); } } diff --git a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs index addf8b6a..d1f6392b 100644 --- a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs @@ -140,7 +140,7 @@ protected override async Task AfterConnectSetupAsync(bool requestDeviceInf try { // wait until ports finish communicating with the hub - await AwaitForHubConnectedAsync(TimeSpan.FromSeconds(1), token); + await AwaitForPeripheralsAttachedAsync(TimeSpan.FromSeconds(1), token); if (requestDeviceInformation) { @@ -161,7 +161,7 @@ protected override async Task AfterConnectSetupAsync(bool requestDeviceInf // port configuration for (int channel = 0; channel < NumberOfChannels; channel++) { - var channelConfig = ChannelConfigs[channel]; + var channelConfig = ChannelConfigs.Get(channel); if (channelConfig.OutputType == ChannelOutputType.ServoMotor) { await SetupChannelForPortInformationAsync(channel, token); @@ -217,7 +217,8 @@ protected override async Task SendOutputValuesAsync(CancellationToken toke // all channels command - use original value int.MaxValue => await SendAllOutputValuesAsync(change.Value, token), // classic output command for A, B, C channels (with servo support) - CHANNEL_C when channelOutputType == ChannelOutputType.ServoMotor => await SendServoValue(change.Key, value, token), + CHANNEL_C when channelOutputType == ChannelOutputType.ServoMotor + => await SendPortOutout_ServoAsync(change.Key, change.Value, token), _ => await SendPortOutput_ValueAsync(change.Key, value, token), }; @@ -394,11 +395,12 @@ private async Task SendPortOutput_ValueAsync(int channel, byte value, Canc return await WriteAsync(cmd, token); } - private async Task SendServoValue(int channel, int value, CancellationToken token) + private async Task SendPortOutout_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 absPosition = _calibratedZeroAngle + ChannelConfigs[channel].ServoBaseAngle + value; //TODO MAX SERVO ANGLE + var servoAngle = (int)value * GetMaxServoAngle(channel) / 100; + var absPosition = _calibratedZeroAngle + ChannelConfigs.Get(channel).ServoBaseAngle + servoAngle; var cmd = BuildPortOutput_GotoAbsPosition(portId, absPosition, servoSpeed: 50); return await WriteAsync(cmd, token); } @@ -412,10 +414,10 @@ private async Task SendAllOutputValuesAsync(Half value, CancellationToken // A, B, C channels foreach (var channel in new[] { CHANNEL_A, CHANNEL_B, CHANNEL_C }) { - var outputType = ChannelConfigs[channel].OutputType; + var outputType = GetOutputType(channel); result = result && outputType switch { - ChannelOutputType.ServoMotor => await SendServoValue(channel, (int)value, token), + ChannelOutputType.ServoMotor => await SendPortOutout_ServoAsync(channel, value, token), _ => await SendPortOutput_ValueAsync(channel, rawValue, token), }; } diff --git a/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs b/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs index 3ee73ea5..531a4736 100644 --- a/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs +++ b/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs @@ -117,19 +117,7 @@ public static void ToBytes(int value, out byte b0, out byte b1, out byte b2, out public static int ToInt32(ReadOnlySpan value) => BinaryPrimitives.ReadInt32LittleEndian(value); public static ushort ToUInt16(ReadOnlySpan value) => BinaryPrimitives.ReadUInt16LittleEndian(value); - public static int NormalizeAngle(int angle) - { - if (angle >= 180) - { - return angle - (360 * ((angle + 180) / 360)); - } - else if (angle < -180) - { - return angle + (360 * ((180 - angle) / 360)); - } - - return angle; - } + public static int NormalizeAngle(int angle) => angle - (360 * ((angle + 180) / 360)); public static string GetVersionString(ReadOnlySpan data) { From c365083e9a5aafe8d13f3a0fc1fb61e74d6612e2 Mon Sep 17 00:00:00 2001 From: vicocz Date: Sat, 9 May 2026 11:32:32 +0200 Subject: [PATCH 14/29] fix unit tests --- .../DeviceManagement/IO/ChannelConfigTests.cs | 61 +++++++++++++++++++ .../DeviceManagement/IO/ChannelConfig.cs | 16 +++++ .../DeviceManagement/IO/StateStore.cs | 4 +- .../Lego/WirelessProtocolBasedDevice.cs | 17 +----- .../Protocols/LegoWirelessProtocol.cs | 2 +- 5 files changed, 82 insertions(+), 18 deletions(-) create mode 100644 BrickController2/BrickController2.Tests/DeviceManagement/IO/ChannelConfigTests.cs diff --git a/BrickController2/BrickController2.Tests/DeviceManagement/IO/ChannelConfigTests.cs b/BrickController2/BrickController2.Tests/DeviceManagement/IO/ChannelConfigTests.cs new file mode 100644 index 00000000..3265b337 --- /dev/null +++ b/BrickController2/BrickController2.Tests/DeviceManagement/IO/ChannelConfigTests.cs @@ -0,0 +1,61 @@ +using BrickController2.CreationManagement; +using BrickController2.DeviceManagement; +using BrickController2.DeviceManagement.IO; +using FluentAssertions; +using Xunit; + +namespace BrickController2.Tests.DeviceManagement.IO; + +public class ChannelConfigTests +{ + [Fact] + public void From_ServoMotor_ReturnsCorrectConfig() + { + var config = new ChannelConfiguration + { + ChannelOutputType = ChannelOutputType.ServoMotor, + MaxServoAngle = 90, + ServoBaseAngle = 45 + }; + + var result = ChannelConfig.From(config); + + result.OutputType.Should().Be(ChannelOutputType.ServoMotor); + result.MaxServoAngle.Should().Be(90); + result.ServoBaseAngle.Should().Be(45); + result.StepperAngle.Should().Be(0); + } + + [Fact] + public void From_StepperMotor_ReturnsCorrectConfig() + { + var config = new ChannelConfiguration + { + ChannelOutputType = ChannelOutputType.StepperMotor, + StepperAngle = 30 + }; + + var result = ChannelConfig.From(config); + + result.OutputType.Should().Be(ChannelOutputType.StepperMotor); + result.StepperAngle.Should().Be(30); + result.MaxServoAngle.Should().Be(0); + result.ServoBaseAngle.Should().Be(0); + } + + [Fact] + public void From_Default_ReturnsEmptyConfig() + { + var config = new ChannelConfiguration + { + ChannelOutputType = ChannelOutputType.NormalMotor + }; + + var result = ChannelConfig.From(config); + + result.OutputType.Should().Be(ChannelOutputType.NormalMotor); + result.MaxServoAngle.Should().Be(0); + result.ServoBaseAngle.Should().Be(0); + result.StepperAngle.Should().Be(0); + } +} \ No newline at end of file diff --git a/BrickController2/BrickController2/DeviceManagement/IO/ChannelConfig.cs b/BrickController2/BrickController2/DeviceManagement/IO/ChannelConfig.cs index 3407956d..93849a39 100644 --- a/BrickController2/BrickController2/DeviceManagement/IO/ChannelConfig.cs +++ b/BrickController2/BrickController2/DeviceManagement/IO/ChannelConfig.cs @@ -12,4 +12,20 @@ internal readonly record struct ChannelConfig public int MaxServoAngle { get; init; } public int ServoBaseAngle { get; init; } public int StepperAngle { get; init; } + + public static ChannelConfig From(ChannelConfiguration config) => config.ChannelOutputType switch + { + ChannelOutputType.ServoMotor => new() + { + OutputType = ChannelOutputType.ServoMotor, + MaxServoAngle = config.MaxServoAngle, + ServoBaseAngle = config.ServoBaseAngle + }, + ChannelOutputType.StepperMotor => new() + { + OutputType = ChannelOutputType.StepperMotor, + StepperAngle = config.StepperAngle + }, + _ => new() + }; } diff --git a/BrickController2/BrickController2/DeviceManagement/IO/StateStore.cs b/BrickController2/BrickController2/DeviceManagement/IO/StateStore.cs index 41dc3f29..dd8d9224 100644 --- a/BrickController2/BrickController2/DeviceManagement/IO/StateStore.cs +++ b/BrickController2/BrickController2/DeviceManagement/IO/StateStore.cs @@ -45,5 +45,7 @@ public void Set(TKey key, TValue state = default) public void Clear() => _states.Clear(); /// Returns the maximum value of a projection over all stored states, or default if empty. - public TResult? Max(Func selector) => _states.Values.Max(x => selector(x)); + public TResult? Max(Func selector) => _states.IsEmpty + ? default + : _states.Values.Max(x => selector(x)); } diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs index 07f0c224..aceed617 100644 --- a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs @@ -63,22 +63,7 @@ public override Task ConnectAsync( for (int i = 0; i < NumberOfChannels; i++) { configs.TryGetValue(i, out var config); - - ChannelConfigs.Set(i, config.ChannelOutputType switch - { - ChannelOutputType.ServoMotor => new() - { - OutputType = ChannelOutputType.ServoMotor, - MaxServoAngle = config.MaxServoAngle, - ServoBaseAngle = config.ServoBaseAngle - }, - ChannelOutputType.StepperMotor => new() - { - OutputType = ChannelOutputType.StepperMotor, - StepperAngle = config.StepperAngle - }, - _ => new() - }); + ChannelConfigs.Set(i, ChannelConfig.From(config)); } return base.ConnectAsync(reconnect, onDeviceDisconnected, channelConfigurations, startOutputProcessing, requestDeviceInformation, token); diff --git a/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs b/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs index 531a4736..8573b740 100644 --- a/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs +++ b/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs @@ -117,7 +117,7 @@ public static void ToBytes(int value, out byte b0, out byte b1, out byte b2, out public static int ToInt32(ReadOnlySpan value) => BinaryPrimitives.ReadInt32LittleEndian(value); public static ushort ToUInt16(ReadOnlySpan value) => BinaryPrimitives.ReadUInt16LittleEndian(value); - public static int NormalizeAngle(int angle) => angle - (360 * ((angle + 180) / 360)); + public static int NormalizeAngle(int angle) => ((angle + 180) % 360 + 360) % 360 - 180; public static string GetVersionString(ReadOnlySpan data) { From 5d2dac86ad0487d0e99988caf13be82013721855 Mon Sep 17 00:00:00 2001 From: vicocz Date: Sat, 9 May 2026 17:35:16 +0200 Subject: [PATCH 15/29] resolve review - part 1 --- .../DeviceManagement/IO/StateStoreTests.cs | 43 +++++++ .../Helpers/AwaitTests.cs | 73 +++++++++++ .../DeviceManagement/ControlPlusDevice.cs | 16 ++- .../DeviceManagement/IO/StateStore.cs | 25 ++++ ...entInfo.cs => PeripheralAttachmentInfo.cs} | 6 +- ...hannelPositionState.cs => PositionInfo.cs} | 8 +- .../DeviceManagement/Lego/RemoteControl.cs | 4 +- .../Lego/WirelessProtocolBasedDevice.cs | 114 ++++-------------- .../DeviceManagement/TechnicMoveDevice.cs | 2 +- .../BrickController2/Helpers/Await.cs | 53 ++++++++ 10 files changed, 234 insertions(+), 110 deletions(-) create mode 100644 BrickController2/BrickController2.Tests/Helpers/AwaitTests.cs rename BrickController2/BrickController2/DeviceManagement/Lego/{ChannelAttachmentInfo.cs => PeripheralAttachmentInfo.cs} (72%) rename BrickController2/BrickController2/DeviceManagement/Lego/{ChannelPositionState.cs => PositionInfo.cs} (73%) create mode 100644 BrickController2/BrickController2/Helpers/Await.cs diff --git a/BrickController2/BrickController2.Tests/DeviceManagement/IO/StateStoreTests.cs b/BrickController2/BrickController2.Tests/DeviceManagement/IO/StateStoreTests.cs index c93630b6..288b34b8 100644 --- a/BrickController2/BrickController2.Tests/DeviceManagement/IO/StateStoreTests.cs +++ b/BrickController2/BrickController2.Tests/DeviceManagement/IO/StateStoreTests.cs @@ -1,5 +1,6 @@ using BrickController2.DeviceManagement.IO; using FluentAssertions; +using System.Threading.Tasks; using Xunit; namespace BrickController2.Tests.DeviceManagement.IO; @@ -82,4 +83,46 @@ public void Max_ReturnsDefault_WhenEmpty() { _sut.Max(v => v).Should().Be(0); } + + [Fact] + public void Exchange_ReturnsOldState_AndAppliesUpdater() + { + var store = new StateStore(); + store.Set("key", new TestState(10, true)); + + var old = store.Exchange("key", s => s with { Value = s.Value + 5, Updated = false }); + + old.Should().BeEquivalentTo(new TestState(10, true)); + var current = store.Get("key"); + current.Should().BeEquivalentTo(new TestState(15)); + } + + [Fact] + public void Exchange_WhenKeyAbsent_ReturnsDefault_AndSetsNext() + { + var store = new StateStore(); + + var old = store.Exchange("missing", s => new TestState(s.Value + 1, true)); + + old.Should().BeEquivalentTo(new TestState()); + var current = store.Get("missing"); + current.Should().BeEquivalentTo(new TestState(1, true)); + } + + [Fact] + public void Exchange_IsSafeUnderConcurrency() + { + var store = new StateStore(); + store.Set("counter", 0); + + const int iterations = 250; + Parallel.For(0, iterations, _ => + { + store.Exchange("counter", v => v + 1); + }); + + Assert.Equal(iterations, store.Get("counter")); + } + + private record struct TestState(int Value = default, bool Updated = false); } \ No newline at end of file diff --git a/BrickController2/BrickController2.Tests/Helpers/AwaitTests.cs b/BrickController2/BrickController2.Tests/Helpers/AwaitTests.cs new file mode 100644 index 00000000..e5971f62 --- /dev/null +++ b/BrickController2/BrickController2.Tests/Helpers/AwaitTests.cs @@ -0,0 +1,73 @@ +using BrickController2.Helpers; +using FluentAssertions; +using System; +using System.Threading; +using System.Threading.Tasks; +using Xunit; + +namespace BrickController2.Tests.Helpers; + +public class AwaitTests +{ + private static Func IsStable => (current, last) => current == last; + + [Fact] + public async Task WaitForStableValueAsync_ReturnsTrue_WhenValueBecomesStable() + { + var callCount = 0; + var values = new[] { 1, 2, 2, 2, 2, 2, 2, 2 }; + int GetValue() => values[Math.Min(callCount++, values.Length - 1)]; + + var result = await Await.WaitForStableValueAsync( + TimeSpan.FromSeconds(500), + GetValue, + IsStable); + + result.Should().BeTrue(); + } + + [Fact] + public async Task WaitForStableValueAsync_ReturnsFalse_WhenTimeoutExpiresWithUnstableValue() + { + var callCount = 0; + int GetValue() => callCount++; // always changing + + var result = await Await.WaitForStableValueAsync( + TimeSpan.FromMilliseconds(500), + GetValue, + IsStable); + + result.Should().BeFalse(); + } + + [Fact] + public async Task WaitForStableValueAsync_ReturnsFalse_WhenCancellationTokenCancelled() + { + using var cts = new CancellationTokenSource(); + var callCount = 0; + int GetValue() => callCount++; + + await cts.CancelAsync(); + + var result = await Await.WaitForStableValueAsync( + TimeSpan.FromSeconds(1), + GetValue, + IsStable, + cts.Token); + + result.Should().BeFalse(); + } + + [Fact] + public async Task WaitForStableValueAsync_ReturnsTrue_WhenValueIsImmediatelyStable() + { + int GetValue() => 42; + + var result = await Await.WaitForStableValueAsync( + TimeSpan.FromSeconds(1), + GetValue, + IsStable); + + result.Should().BeTrue(); + } +} \ No newline at end of file diff --git a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs index f53cc7a5..1a1972ef 100644 --- a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs @@ -169,7 +169,7 @@ protected override async Task AfterConnectSetupAsync(bool requestDeviceInf try { // Wait until ports finish communicating with the hub - await AwaitForPeripheralsAttachedAsync(TimeSpan.FromMilliseconds(1000), token); + await AwaitPeripheralsAttachedAsync(TimeSpan.FromMilliseconds(1000), token); if (requestDeviceInformation) { @@ -525,18 +525,16 @@ private static int RoundAngleToNearest90(int angle) private int CalculateServoSpeed(int channel, int targetAngle) { - var channelPositions = ChannelRelativePositions.Get(channel); - - if (channelPositions.IsUpdated) - { - var diffAngle = Math.Abs(channelPositions.Current - targetAngle); - ChannelRelativePositions.Update(channel, x => x.ConsumeUpdate()); + var position = ChannelRelativePositions.Exchange(channel, x => x.ConsumeUpdate()); + if (position.IsUpdated) + { + var diffAngle = Math.Abs(position.Current - targetAngle); return Math.Max(20, Math.Min(100, diffAngle)); } - if (channelPositions.UpdateTime == DateTime.MinValue || - POSITION_EXPIRATION < DateTime.Now - channelPositions.UpdateTime) + if (position.UpdateTime == DateTime.MinValue || + POSITION_EXPIRATION < DateTime.Now - position.UpdateTime) { // Position update never happened or too old return 50; diff --git a/BrickController2/BrickController2/DeviceManagement/IO/StateStore.cs b/BrickController2/BrickController2/DeviceManagement/IO/StateStore.cs index dd8d9224..f6f609b7 100644 --- a/BrickController2/BrickController2/DeviceManagement/IO/StateStore.cs +++ b/BrickController2/BrickController2/DeviceManagement/IO/StateStore.cs @@ -41,6 +41,31 @@ public void Set(TKey key, TValue state = default) /// public TValue Update(TKey key, Func updater) => _states.AddOrUpdate(key, (k) => updater(_default), (k, o) => updater(o)); + /// + /// Atomically applies to the current state and returns + /// the state that existed before the update (the consumed snapshot). + /// This prevents a race where a concurrent write between a Get() and a separate + /// Update() call causes the new write to be silently cleared. + /// + public TValue Exchange(TKey key, Func updater) + { + while (true) + { + var old = Get(key); + var next = updater(old); + + // If the key is already present, do a compare-and-swap. + if (_states.TryUpdate(key, next, old)) + return old; + + // Key was absent; race to insert the computed next value. + if (_states.TryAdd(key, next)) + return _default; + + // Another thread beat us — retry. + } + } + /// Clears all persisted states. public void Clear() => _states.Clear(); diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/ChannelAttachmentInfo.cs b/BrickController2/BrickController2/DeviceManagement/Lego/PeripheralAttachmentInfo.cs similarity index 72% rename from BrickController2/BrickController2/DeviceManagement/Lego/ChannelAttachmentInfo.cs rename to BrickController2/BrickController2/DeviceManagement/Lego/PeripheralAttachmentInfo.cs index 52758e2c..95342bf8 100644 --- a/BrickController2/BrickController2/DeviceManagement/Lego/ChannelAttachmentInfo.cs +++ b/BrickController2/BrickController2/DeviceManagement/Lego/PeripheralAttachmentInfo.cs @@ -5,17 +5,17 @@ namespace BrickController2.DeviceManagement.Lego; /// /// Snapshot of peripheral attachment for a port. /// -internal record struct ChannelAttachmentInfo( +internal record struct PeripheralAttachmentInfo( ushort DeviceId, bool IsUpdated, DateTime UpdateTime) { - public static readonly ChannelAttachmentInfo Initial = new( + public static readonly PeripheralAttachmentInfo Initial = new( DeviceId: 0, IsUpdated: false, UpdateTime: DateTime.MinValue); /// Returns a new state with an updated device ID and timestamp. - public ChannelAttachmentInfo WithDevice(ushort deviceId) => + public PeripheralAttachmentInfo WithDevice(ushort deviceId) => this with { DeviceId = deviceId, IsUpdated = true, UpdateTime = DateTime.Now }; } diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/ChannelPositionState.cs b/BrickController2/BrickController2/DeviceManagement/Lego/PositionInfo.cs similarity index 73% rename from BrickController2/BrickController2/DeviceManagement/Lego/ChannelPositionState.cs rename to BrickController2/BrickController2/DeviceManagement/Lego/PositionInfo.cs index ae95887e..76da418b 100644 --- a/BrickController2/BrickController2/DeviceManagement/Lego/ChannelPositionState.cs +++ b/BrickController2/BrickController2/DeviceManagement/Lego/PositionInfo.cs @@ -5,21 +5,21 @@ namespace BrickController2.DeviceManagement.Lego; /// /// Immutable snapshot of a motor channel's position feedback. /// -internal record struct ChannelPositionState( +internal record struct PositionInfo( int Current, bool IsUpdated, DateTime UpdateTime) { - public static readonly ChannelPositionState Initial = new( + public static readonly PositionInfo Initial = new( Current: 0, IsUpdated: false, UpdateTime: DateTime.MinValue); /// Returns a new state with an updated position and timestamp. - public ChannelPositionState WithPosition(int position) => + public PositionInfo WithPosition(int position) => this with { Current = position, IsUpdated = true, UpdateTime = DateTime.Now }; /// Clears the IsUpdated flag after the update has been consumed. - public ChannelPositionState ConsumeUpdate() => + public PositionInfo ConsumeUpdate() => this with { IsUpdated = false }; } diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs b/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs index 46616fc8..47cf60e0 100644 --- a/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs +++ b/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs @@ -59,10 +59,12 @@ internal void ResetEvents() => RaiseButtonEvents( protected override Task ProcessOutputsAsync(CancellationToken token) => Task.CompletedTask; + protected override Task SendOutputValuesAsync(CancellationToken token) => throw new InvalidOperationException(); + protected override async Task AfterConnectSetupAsync(bool requestDeviceInformation, CancellationToken token) { // wait until ports finish communicating with the hub - await AwaitForPeripheralsAttachedAsync(TimeSpan.FromMilliseconds(250), token); + await AwaitPeripheralsAttachedAsync(TimeSpan.FromMilliseconds(250), token); if (requestDeviceInformation) { diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs index aceed617..d46c11b1 100644 --- a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs @@ -3,13 +3,13 @@ using BrickController2.PlatformServices.BluetoothLE; using System; using System.Collections.Generic; -using System.Diagnostics; using System.Linq; using System.Threading; using System.Threading.Tasks; using static BrickController2.CreationManagement.ControllerDefaults; using static BrickController2.Diagnostics.Logs; +using static BrickController2.Helpers.Await; using static BrickController2.Protocols.LegoWirelessProtocol; namespace BrickController2.DeviceManagement.Lego; @@ -17,10 +17,10 @@ namespace BrickController2.DeviceManagement.Lego; internal abstract class WirelessProtocolBasedDevice : BluetoothDevice { protected readonly ChannelStateStore ChannelConfigs; - protected readonly ChannelStateStore ChannelAbsPositions; - protected readonly ChannelStateStore ChannelRelativePositions; + protected readonly ChannelStateStore ChannelAbsPositions; + protected readonly ChannelStateStore ChannelRelativePositions; - protected readonly ChannelStateStore AttachedPeripherals; + protected readonly ChannelStateStore AttachedPeripherals; protected IGattCharacteristic? Characteristic; @@ -28,9 +28,9 @@ protected WirelessProtocolBasedDevice(string name, string address, IDeviceReposi : base(name, address, deviceRepository, bleService) { ChannelConfigs = new(); - ChannelAbsPositions = new(ChannelPositionState.Initial); - ChannelRelativePositions = new(ChannelPositionState.Initial); - AttachedPeripherals = new(ChannelAttachmentInfo.Initial); + ChannelAbsPositions = new(PositionInfo.Initial); + ChannelRelativePositions = new(PositionInfo.Initial); + AttachedPeripherals = new(PeripheralAttachmentInfo.Initial); } public override string BatteryVoltageSign => "%"; @@ -46,9 +46,6 @@ public override Task ConnectAsync( { // reset output values & positions ResetOutputValues(); - ChannelAbsPositions.Clear(); - ChannelRelativePositions.Clear(); - AttachedPeripherals.Clear(); // Initialize configuration per channel @@ -105,7 +102,7 @@ protected override async Task ProcessOutputsAsync(CancellationToken token) } } - protected virtual Task SendOutputValuesAsync(CancellationToken token) => throw new InvalidOperationException(nameof(SendOutputValuesAsync)); + protected abstract Task SendOutputValuesAsync(CancellationToken token); protected override async Task ValidateServicesAsync(IEnumerable? services, CancellationToken token) { @@ -176,7 +173,8 @@ protected virtual bool TryProcessMessageData(byte messageType, ReadOnlySpan= 7) { var deviceId = ToUInt16(data.Slice(5)); AttachedPeripherals.Update(channel, info => info.WithDevice(deviceId)); // store portId as "position" for simplicity @@ -386,88 +385,19 @@ protected int CalculateCalibratedTarget(int channel, int targetBaseAngle = 0) return GetAbsPosition(channel) + diff; } - protected Task AwaitStableRelativePositionAsync(int channel, TimeSpan timeout, CancellationToken token) - => WaitForStablePositionAsync(timeout, () => ChannelRelativePositions.Get(channel), token); - protected Task AwaitStableAbsolutePositionAsync(int channel, TimeSpan timeout, CancellationToken token) - => WaitForStablePositionAsync(timeout, () => ChannelAbsPositions.Get(channel), token); + => WaitForStableValueAsync(timeout, + getValue: () => ChannelAbsPositions.Get(channel), + stabilityCheck: (old, value) => value.IsUpdated && value.Current == old.Current, + token); - protected Task AwaitForPeripheralsAttachedAsync(TimeSpan timeout, CancellationToken token) - => WaitForTimestampStabilityAsync(timeout, () => AttachedPeripherals.Max(x => x.UpdateTime), token); - - private static async Task WaitForTimestampStabilityAsync(TimeSpan timeout, Func getTimestamp, CancellationToken token) + protected async Task AwaitPeripheralsAttachedAsync(TimeSpan timeout, CancellationToken token) { - var interval = TimeSpan.FromMilliseconds(50); - var stabilityTimeout = TimeSpan.FromMilliseconds(200); - - using var linkedCts = CancellationTokenSource.CreateLinkedTokenSource(token); - linkedCts.CancelAfter(timeout); - - var stableSince = Stopwatch.StartNew(); - var lastUpdated = getTimestamp(); - - try - { - while (!linkedCts.Token.IsCancellationRequested) - { - await Task.Delay(interval, linkedCts.Token); + var result = await WaitForStableValueAsync(timeout, + getValue: () => AttachedPeripherals.Max(x => x.UpdateTime), + stabilityCheck: (old, value) => value != default && old == value, + token); - var currentValue = getTimestamp(); - if (currentValue != lastUpdated) - { - lastUpdated = currentValue; - stableSince.Restart(); - } - else if (stableSince.Elapsed >= stabilityTimeout) - { - return; // position stable for the required duration - } - } - Dump("TimestampStability: TIMEOUT", lastUpdated); - } - catch (OperationCanceledException) when (!token.IsCancellationRequested) - { - // total timeout elapsed — treat as completed - Dump("TimestampStability: CANCELLED", lastUpdated); - } + return result || AttachedPeripherals.Count > 0; } - - private static async Task WaitForStablePositionAsync(TimeSpan timeout, Func getPosition, CancellationToken token) - { - var interval = TimeSpan.FromMilliseconds(50); - var stabilityTimeout = TimeSpan.FromMilliseconds(500); - - using var linkedCts = CancellationTokenSource.CreateLinkedTokenSource(token); - linkedCts.CancelAfter(timeout); - - var lastPosition = getPosition().Current; - var stableSince = Stopwatch.StartNew(); - - try - { - while (!linkedCts.Token.IsCancellationRequested) - { - await Task.Delay(interval, linkedCts.Token); - - var currentPosition = getPosition(); - if (!currentPosition.IsUpdated || currentPosition.Current != lastPosition) - { - lastPosition = currentPosition.Current; - stableSince.Restart(); - } - else if (stableSince.Elapsed >= stabilityTimeout) - { - Dump("Servo position: STABLE", lastPosition); - return; // position stable for the required duration - } - } - Dump("Servo position: TIMEOUT", lastPosition); - } - catch (OperationCanceledException) when (!token.IsCancellationRequested) - { - // total timeout elapsed — treat as completed - Dump("Servo position: CANCELLED", lastPosition); - } - } - } diff --git a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs index d1f6392b..df02312f 100644 --- a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs @@ -140,7 +140,7 @@ protected override async Task AfterConnectSetupAsync(bool requestDeviceInf try { // wait until ports finish communicating with the hub - await AwaitForPeripheralsAttachedAsync(TimeSpan.FromSeconds(1), token); + await AwaitPeripheralsAttachedAsync(TimeSpan.FromSeconds(1), token); if (requestDeviceInformation) { diff --git a/BrickController2/BrickController2/Helpers/Await.cs b/BrickController2/BrickController2/Helpers/Await.cs new file mode 100644 index 00000000..edcc70c7 --- /dev/null +++ b/BrickController2/BrickController2/Helpers/Await.cs @@ -0,0 +1,53 @@ +using System; +using System.Diagnostics; +using System.Threading; +using System.Threading.Tasks; + +using static BrickController2.Diagnostics.Logs; + +namespace BrickController2.Helpers; + +internal static class Await +{ + internal static async Task WaitForStableValueAsync(TimeSpan timeout, + Func getValue, + Func stabilityCheck, + CancellationToken token = default) + where TValue : struct + { + var interval = TimeSpan.FromMilliseconds(50); + var stabilityTimeout = TimeSpan.FromMilliseconds(200); + + using var linkedCts = CancellationTokenSource.CreateLinkedTokenSource(token); + linkedCts.CancelAfter(timeout); + + var stableSince = Stopwatch.StartNew(); + var lastValue = getValue(); + + try + { + while (!linkedCts.Token.IsCancellationRequested) + { + await Task.Delay(interval, linkedCts.Token); + + var currentValue = getValue(); + if (!stabilityCheck(currentValue, lastValue)) + { + lastValue = currentValue; + stableSince.Restart(); + } + else if (stableSince.Elapsed >= stabilityTimeout) + { + return true; // position stable for the required duration + } + } + Dump("TimestampStability: TIMEOUT", lastValue); + } + catch (OperationCanceledException) when (!token.IsCancellationRequested) + { + // total timeout elapsed — treat as completed + Dump("TimestampStability: CANCELLED", lastValue); + } + return false; + } +} From 5c95c1df0fe6e32d5cfd57fc976e5d444b02bc3f Mon Sep 17 00:00:00 2001 From: vicocz Date: Sat, 9 May 2026 22:59:50 +0200 Subject: [PATCH 16/29] . --- .../Helpers/AwaitTests.cs | 17 ++++---- .../DeviceManagement/IO/StateStore.cs | 4 +- .../Lego/WirelessProtocolBasedDevice.cs | 35 +++++++++++----- .../DeviceManagement/TechnicMoveDevice.cs | 40 ++++++++++++------- .../BrickController2/Helpers/Await.cs | 22 +++++++--- .../Protocols/LegoWirelessProtocol.cs | 7 ++++ 6 files changed, 82 insertions(+), 43 deletions(-) diff --git a/BrickController2/BrickController2.Tests/Helpers/AwaitTests.cs b/BrickController2/BrickController2.Tests/Helpers/AwaitTests.cs index e5971f62..3ef3c333 100644 --- a/BrickController2/BrickController2.Tests/Helpers/AwaitTests.cs +++ b/BrickController2/BrickController2.Tests/Helpers/AwaitTests.cs @@ -18,10 +18,9 @@ public async Task WaitForStableValueAsync_ReturnsTrue_WhenValueBecomesStable() var values = new[] { 1, 2, 2, 2, 2, 2, 2, 2 }; int GetValue() => values[Math.Min(callCount++, values.Length - 1)]; - var result = await Await.WaitForStableValueAsync( - TimeSpan.FromSeconds(500), - GetValue, - IsStable); + var result = await Await.WaitForStableValueAsync(GetValue, + IsStable, + TimeSpan.FromSeconds(500)); result.Should().BeTrue(); } @@ -33,9 +32,9 @@ public async Task WaitForStableValueAsync_ReturnsFalse_WhenTimeoutExpiresWithUns int GetValue() => callCount++; // always changing var result = await Await.WaitForStableValueAsync( - TimeSpan.FromMilliseconds(500), GetValue, - IsStable); + IsStable, + TimeSpan.FromMilliseconds(500)); result.Should().BeFalse(); } @@ -50,9 +49,9 @@ public async Task WaitForStableValueAsync_ReturnsFalse_WhenCancellationTokenCanc await cts.CancelAsync(); var result = await Await.WaitForStableValueAsync( - TimeSpan.FromSeconds(1), GetValue, IsStable, + TimeSpan.FromSeconds(1), cts.Token); result.Should().BeFalse(); @@ -64,9 +63,9 @@ public async Task WaitForStableValueAsync_ReturnsTrue_WhenValueIsImmediatelyStab int GetValue() => 42; var result = await Await.WaitForStableValueAsync( - TimeSpan.FromSeconds(1), GetValue, - IsStable); + IsStable, + TimeSpan.FromSeconds(1)); result.Should().BeTrue(); } diff --git a/BrickController2/BrickController2/DeviceManagement/IO/StateStore.cs b/BrickController2/BrickController2/DeviceManagement/IO/StateStore.cs index f6f609b7..0a67abb2 100644 --- a/BrickController2/BrickController2/DeviceManagement/IO/StateStore.cs +++ b/BrickController2/BrickController2/DeviceManagement/IO/StateStore.cs @@ -62,7 +62,7 @@ public TValue Exchange(TKey key, Func updater) if (_states.TryAdd(key, next)) return _default; - // Another thread beat us — retry. + // Another thread beat us - retry. } } @@ -71,6 +71,6 @@ public TValue Exchange(TKey key, Func updater) /// Returns the maximum value of a projection over all stored states, or default if empty. public TResult? Max(Func selector) => _states.IsEmpty - ? default + ? selector(_default) : _states.Values.Max(x => selector(x)); } diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs index d46c11b1..4c318a1e 100644 --- a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs @@ -210,15 +210,15 @@ protected virtual bool TryProcessMessageData(byte messageType, ReadOnlySpan= 7) + if (eventType == HUB_EVENT_ATTACHED || eventType == HUB_EVENT_ATTACHED_VIRTUAL) { + // Get Unique type identification of the attached I/O device var deviceId = ToUInt16(data.Slice(5)); AttachedPeripherals.Update(channel, info => info.WithDevice(deviceId)); // store portId as "position" for simplicity } - else if (eventType == 0x00) + else if (eventType == HUB_EVENT_DETACHED) { AttachedPeripherals.Remove(channel); } @@ -385,17 +385,30 @@ protected int CalculateCalibratedTarget(int channel, int targetBaseAngle = 0) return GetAbsPosition(channel) + diff; } - protected Task AwaitStableAbsolutePositionAsync(int channel, TimeSpan timeout, CancellationToken token) - => WaitForStableValueAsync(timeout, - getValue: () => ChannelAbsPositions.Get(channel), - stabilityCheck: (old, value) => value.IsUpdated && value.Current == old.Current, + protected static ValueTask AwaitStablePositionAsync(Func getValue, TimeSpan timeout, CancellationToken token) + => WaitForStableValueAsync(getValue, + stabilityCheck: (value, last) => + (value.IsUpdated && value.Current == last.Current) || + (!value.IsUpdated && value.UpdateTime == last.UpdateTime), + timeout, token); - protected async Task AwaitPeripheralsAttachedAsync(TimeSpan timeout, CancellationToken token) + protected static ValueTask AwaitPositionChangeAsync(Func getValue, TimeSpan timeout, CancellationToken token) { - var result = await WaitForStableValueAsync(timeout, + var initialValue = getValue(); + return WaitForStableValueAsync(getValue: getValue, + stabilityCheck: (value, last) => value.IsUpdated, + timeout, + stabilityTimeout: TimeSpan.FromMilliseconds(10), //TODO + token); + } + + protected async ValueTask AwaitPeripheralsAttachedAsync(TimeSpan timeout, CancellationToken token) + { + var result = await WaitForStableValueAsync( getValue: () => AttachedPeripherals.Max(x => x.UpdateTime), - stabilityCheck: (old, value) => value != default && old == value, + stabilityCheck: (value, last) => value != default && last == value, + timeout, token); return result || AttachedPeripherals.Count > 0; diff --git a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs index df02312f..603cc1ea 100644 --- a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs @@ -203,8 +203,13 @@ protected override async Task SendOutputValuesAsync(CancellationToken toke var result = await SendPlayVmOutputValueAsync(token); // process changes for other channels as it's a light or a classic drive - if (result && _outputValues.TryGetChanges(out var changes)) + if (result) { + if (!_outputValues.TryGetChanges(out var changes)) + { + return false; + } + foreach (KeyValuePair change in changes) { var value = ToByte(change.Value); @@ -256,7 +261,7 @@ protected override bool TryProcessMessageData(byte messageType, ReadOnlySpan SetupChannelForPortInformationAsync(int channel, CancellationToken token) + private async ValueTask SetupChannelForPortInformationAsync(int channel, CancellationToken token) { try { @@ -278,12 +283,14 @@ private async Task SetupChannelForPortInformationAsync(int channel, Cancel // query current APOS await WriteAsync([0x05, 0x00, 0x21, portId, 0x00], token); - await Task.Delay(250, token); //TODO wait for change + await AwaitPositionChangeAsync(() => ChannelAbsPositions.Exchange(channel, x => x.ConsumeUpdate()), + TimeSpan.FromMilliseconds(250), token); // setup channel to report POS position regularly var inputFormatForRelAngle = BuildPortInputFormatSetup(portId, PORT_MODE_2); await WriteAsync(inputFormatForRelAngle, token); - await Task.Delay(250, token); //TODO wait for change + await AwaitPositionChangeAsync(() => ChannelRelativePositions.Exchange(channel, x => x.ConsumeUpdate()), + TimeSpan.FromMilliseconds(250), token); // need to recalculate zero angle to support ABS POS commands _calibratedZeroAngle = CalculateCalibratedTarget(channel); @@ -296,7 +303,7 @@ private async Task SetupChannelForPortInformationAsync(int channel, Cancel } } - private async Task ResetServoAsync(int channel, int baseAngle, CancellationToken token) + private async ValueTask ResetServoAsync(int channel, int baseAngle, CancellationToken token) { try { @@ -331,6 +338,9 @@ private async Task ResetServoAsync(int channel, int baseAngle, Cancellatio { _playVmCalibrationTcs = null; } + // Wait for position to stabilize before allowing the output loop to start + await AwaitStablePositionAsync(() => ChannelAbsPositions.Get(channel), TimeSpan.FromSeconds(4), token); + Dump("Reset Servo", ChannelAbsPositions.Get(channel)); } else { @@ -338,11 +348,11 @@ private async Task ResetServoAsync(int channel, int baseAngle, Cancellatio var portId = GetPortId(channel); var servoCmd = BuildPortOutput_GotoAbsPosition(portId, _calibratedZeroAngle + baseAngle, servoSpeed: 0x28); await WriteAsync(servoCmd, token: token); - } - // Wait for position to stabilize before allowing the output loop to start - await AwaitStableAbsolutePositionAsync(channel, TimeSpan.FromSeconds(4), token); - Dump("Reset Servo", ChannelAbsPositions.Get(channel)); + // Wait for position to stabilize before allowing the output loop to start + await AwaitStablePositionAsync(() => ChannelAbsPositions.Get(channel), TimeSpan.FromSeconds(2), token); + Dump("Reset Servo", ChannelAbsPositions.Get(channel)); + } return true; } @@ -352,7 +362,7 @@ private async Task ResetServoAsync(int channel, int baseAngle, Cancellatio } } - private async Task SendPlayVmOutputValueAsync(CancellationToken token) + private async ValueTask SendPlayVmOutputValueAsync(CancellationToken token) { try { @@ -380,22 +390,22 @@ private async Task SendPlayVmOutputValueAsync(CancellationToken token) } } - private Task SendPortOutput_6LedAsync(int ledIndex, byte value, CancellationToken token) + private ValueTask SendPortOutput_6LedAsync(int ledIndex, byte value, CancellationToken token) => SendPortOutput_6LedMaskAsync(ToByte(1 << ledIndex), value, token); - private async Task SendPortOutput_6LedMaskAsync(byte lightMask, byte value, CancellationToken token) + private async ValueTask SendPortOutput_6LedMaskAsync(byte lightMask, byte value, CancellationToken token) { var cmd = BuildPortOutput_LedMask(PORT_6LEDS, PORT_MODE_0, lightMask, value); return await WriteAsync(cmd, token); } - private async Task SendPortOutput_ValueAsync(int channel, byte value, CancellationToken token) + private async ValueTask SendPortOutput_ValueAsync(int channel, byte value, CancellationToken token) { byte[] cmd = [8, 0x00, 0x81, GetPortId(channel), 0x11, 0x51, 0x00, value]; return await WriteAsync(cmd, token); } - private async Task SendPortOutout_ServoAsync(int channel, Half value, CancellationToken token) + private async ValueTask SendPortOutout_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 @@ -405,7 +415,7 @@ private async Task SendPortOutout_ServoAsync(int channel, Half value, Canc return await WriteAsync(cmd, token); } - private async Task SendAllOutputValuesAsync(Half value, CancellationToken token) + private async ValueTask SendAllOutputValuesAsync(Half value, CancellationToken token) { var rawValue = ToByte(value); // all LEDs at once diff --git a/BrickController2/BrickController2/Helpers/Await.cs b/BrickController2/BrickController2/Helpers/Await.cs index edcc70c7..59d47af1 100644 --- a/BrickController2/BrickController2/Helpers/Await.cs +++ b/BrickController2/BrickController2/Helpers/Await.cs @@ -9,14 +9,24 @@ namespace BrickController2.Helpers; internal static class Await { - internal static async Task WaitForStableValueAsync(TimeSpan timeout, - Func getValue, + private static readonly TimeSpan DefaultInterval = TimeSpan.FromMilliseconds(50); + private static readonly TimeSpan DefaultStabilityTimeout = TimeSpan.FromMilliseconds(200); + + internal static ValueTask WaitForStableValueAsync(Func getValue, + Func stabilityCheck, + TimeSpan timeout, + CancellationToken token = default) + where TValue : struct + => WaitForStableValueAsync(getValue, stabilityCheck, timeout, DefaultStabilityTimeout, token); + + internal static async ValueTask WaitForStableValueAsync(Func getValue, Func stabilityCheck, + TimeSpan timeout, + TimeSpan stabilityTimeout, CancellationToken token = default) where TValue : struct { - var interval = TimeSpan.FromMilliseconds(50); - var stabilityTimeout = TimeSpan.FromMilliseconds(200); + var interval = DefaultInterval; using var linkedCts = CancellationTokenSource.CreateLinkedTokenSource(token); linkedCts.CancelAfter(timeout); @@ -41,12 +51,12 @@ internal static async Task WaitForStableValueAsync(TimeSpan timeou return true; // position stable for the required duration } } - Dump("TimestampStability: TIMEOUT", lastValue); + Dump("Stable Value: TIMEOUT", lastValue); } catch (OperationCanceledException) when (!token.IsCancellationRequested) { // total timeout elapsed — treat as completed - Dump("TimestampStability: CANCELLED", lastValue); + Dump("Stable Value: CANCELLED", lastValue); } return false; } diff --git a/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs b/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs index 8573b740..ef92f737 100644 --- a/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs +++ b/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs @@ -93,6 +93,10 @@ internal static class LegoWirelessProtocol public const byte FEEDBACK_ACTION_ACTION_START = 0x10; public const byte FEEDBACK_ACTION_BOTH = 0x11; + public const byte HUB_EVENT_DETACHED = 0x00; + public const byte HUB_EVENT_ATTACHED = 0x01; + public const byte HUB_EVENT_ATTACHED_VIRTUAL = 0x01; + // remote controller public const byte REMOTE_MODE_KEYS = 0x04; public const byte REMOTE_BUTTONS_LEFT = 0x00; @@ -117,6 +121,9 @@ public static void ToBytes(int value, out byte b0, out byte b1, out byte b2, out public static int ToInt32(ReadOnlySpan value) => BinaryPrimitives.ReadInt32LittleEndian(value); public static ushort ToUInt16(ReadOnlySpan value) => BinaryPrimitives.ReadUInt16LittleEndian(value); + /// + /// Normalize the angle to the range [-179, 180] degrees. + /// public static int NormalizeAngle(int angle) => ((angle + 180) % 360 + 360) % 360 - 180; public static string GetVersionString(ReadOnlySpan data) From 5e32828a2dcea52f0af475bd91fa1ac83ec89e93 Mon Sep 17 00:00:00 2001 From: vicocz Date: Sat, 9 May 2026 23:29:01 +0200 Subject: [PATCH 17/29] fix typos --- .../BrickController2.Tests/Helpers/AwaitTests.cs | 2 +- .../DeviceManagement/Lego/WirelessProtocolBasedDevice.cs | 6 +----- .../BrickController2/DeviceManagement/TechnicMoveDevice.cs | 6 +++--- .../BrickController2/Protocols/LegoWirelessProtocol.cs | 4 ++-- 4 files changed, 7 insertions(+), 11 deletions(-) diff --git a/BrickController2/BrickController2.Tests/Helpers/AwaitTests.cs b/BrickController2/BrickController2.Tests/Helpers/AwaitTests.cs index 3ef3c333..594173e0 100644 --- a/BrickController2/BrickController2.Tests/Helpers/AwaitTests.cs +++ b/BrickController2/BrickController2.Tests/Helpers/AwaitTests.cs @@ -20,7 +20,7 @@ public async Task WaitForStableValueAsync_ReturnsTrue_WhenValueBecomesStable() var result = await Await.WaitForStableValueAsync(GetValue, IsStable, - TimeSpan.FromSeconds(500)); + TimeSpan.FromMilliseconds(500)); result.Should().BeTrue(); } diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs index 4c318a1e..495f1141 100644 --- a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs @@ -331,7 +331,6 @@ protected void ProcessHubPropertyData(ReadOnlySpan data) return; } - var dataLength = data[0]; var messageId = data[2]; var propertyId = data[3]; var propertyOperation = data[4]; @@ -394,14 +393,11 @@ protected static ValueTask AwaitStablePositionAsync(Func get token); protected static ValueTask AwaitPositionChangeAsync(Func getValue, TimeSpan timeout, CancellationToken token) - { - var initialValue = getValue(); - return WaitForStableValueAsync(getValue: getValue, + => WaitForStableValueAsync(getValue: getValue, stabilityCheck: (value, last) => value.IsUpdated, timeout, stabilityTimeout: TimeSpan.FromMilliseconds(10), //TODO token); - } protected async ValueTask AwaitPeripheralsAttachedAsync(TimeSpan timeout, CancellationToken token) { diff --git a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs index 603cc1ea..e2e4b6e8 100644 --- a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs @@ -223,7 +223,7 @@ protected override async Task SendOutputValuesAsync(CancellationToken toke int.MaxValue => await SendAllOutputValuesAsync(change.Value, token), // classic output command for A, B, C channels (with servo support) CHANNEL_C when channelOutputType == ChannelOutputType.ServoMotor - => await SendPortOutout_ServoAsync(change.Key, change.Value, token), + => await SendPortOutput_ServoAsync(change.Key, change.Value, token), _ => await SendPortOutput_ValueAsync(change.Key, value, token), }; @@ -405,7 +405,7 @@ private async ValueTask SendPortOutput_ValueAsync(int channel, byte value, return await WriteAsync(cmd, token); } - private async ValueTask SendPortOutout_ServoAsync(int channel, Half value, CancellationToken token) + private async 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 @@ -427,7 +427,7 @@ private async ValueTask SendAllOutputValuesAsync(Half value, CancellationT var outputType = GetOutputType(channel); result = result && outputType switch { - ChannelOutputType.ServoMotor => await SendPortOutout_ServoAsync(channel, value, token), + ChannelOutputType.ServoMotor => await SendPortOutput_ServoAsync(channel, value, token), _ => await SendPortOutput_ValueAsync(channel, rawValue, token), }; } diff --git a/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs b/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs index ef92f737..d157a7c5 100644 --- a/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs +++ b/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs @@ -95,7 +95,7 @@ internal static class LegoWirelessProtocol public const byte HUB_EVENT_DETACHED = 0x00; public const byte HUB_EVENT_ATTACHED = 0x01; - public const byte HUB_EVENT_ATTACHED_VIRTUAL = 0x01; + public const byte HUB_EVENT_ATTACHED_VIRTUAL = 0x02; // remote controller public const byte REMOTE_MODE_KEYS = 0x04; @@ -122,7 +122,7 @@ public static void ToBytes(int value, out byte b0, out byte b1, out byte b2, out public static ushort ToUInt16(ReadOnlySpan value) => BinaryPrimitives.ReadUInt16LittleEndian(value); /// - /// Normalize the angle to the range [-179, 180] degrees. + /// Normalize the angle to the range [-180, 179] degrees. /// public static int NormalizeAngle(int angle) => ((angle + 180) % 360 + 360) % 360 - 180; From a00621d1ac38eeae8f4245f95bc25205543e4115 Mon Sep 17 00:00:00 2001 From: vicocz Date: Sun, 10 May 2026 18:06:02 +0200 Subject: [PATCH 18/29] fix servo reset - properly combine target angle and current ABS / REL pos --- .../Lego/WirelessProtocolBasedDevice.cs | 16 ------------ .../DeviceManagement/TechnicMoveDevice.cs | 26 +++++++++++++++---- 2 files changed, 21 insertions(+), 21 deletions(-) diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs index 495f1141..d6b3ecbc 100644 --- a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs @@ -368,22 +368,6 @@ protected void ProcessHubPropertyData(ReadOnlySpan data) catch { } } - protected int CalculateCalibratedTarget(int channel, int targetBaseAngle = 0) - { - var position = ChannelRelativePositions.Get(channel).Current; - - // Normalize the hardware relative angle to a clean 0-359 range - // (Crucial if your motor firmware reports APOS as -180 to 179) - int normalizedRelative = ((position % 360) + 360) % 360; - int normalizedTarget = ((targetBaseAngle % 360) + 360) % 360; - - // Calculate the raw difference + normalize - int diff = NormalizeAngle(normalizedTarget - normalizedRelative); - - // Offset the current accumulated position by the physical difference - return GetAbsPosition(channel) + diff; - } - protected static ValueTask AwaitStablePositionAsync(Func getValue, TimeSpan timeout, CancellationToken token) => WaitForStableValueAsync(getValue, stabilityCheck: (value, last) => diff --git a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs index e2e4b6e8..ed699de4 100644 --- a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs @@ -269,19 +269,19 @@ private async ValueTask SetupChannelForPortInformationAsync(int channel, C if (_applyPlayVmMode) { - // setup channel to report APOS position + // setup channel to report Mode 3: GOPOS (Absolute Position) var inputFormatForAbsAngle = BuildPortInputFormatSetup(portId, PORT_MODE_3); await WriteAsync(inputFormatForAbsAngle, token); await Task.Delay(300, token); return true; } - // setup channel to for APOS, but no notifications + // setup channel to for Mode 3: GOPOS (Absolute Position), but no notifications var inputFormatForAbsAngleDisabled = BuildPortInputFormatSetup(portId, PORT_MODE_3, notification: PORT_VALUE_NOTIFICATION_DISABLED); await WriteAsync(inputFormatForAbsAngleDisabled, token); await Task.Delay(50, token); - // query current APOS + // query current GOPOS await WriteAsync([0x05, 0x00, 0x21, portId, 0x00], token); await AwaitPositionChangeAsync(() => ChannelAbsPositions.Exchange(channel, x => x.ConsumeUpdate()), TimeSpan.FromMilliseconds(250), token); @@ -296,6 +296,22 @@ await AwaitPositionChangeAsync(() => ChannelRelativePositions.Exchange(channel, _calibratedZeroAngle = CalculateCalibratedTarget(channel); return true; + + int CalculateCalibratedTarget(int channel, int targetBaseAngle = 0) + { + int currentGopos = GetAbsPosition(channel); // True physical angle (e.g., 90) + int position = ChannelRelativePositions.Get(channel).Current; // Accumulated hub angle (e.g., 1080) + + // Calculate the shortest physical distance to your target + // We use GOPOS here because it represents the actual hardware marker + int diffToTarget = NormalizeAngle(targetBaseAngle - currentGopos); + + // Apply that physical difference to the Hub's accumulated POS + // If POS is 1080 and we need to move -90 degrees, the target is 990. + int targetHubPos = position + diffToTarget; + + return targetHubPos; + } } catch { @@ -350,8 +366,8 @@ private async ValueTask ResetServoAsync(int channel, int baseAngle, Cancel await WriteAsync(servoCmd, token: token); // Wait for position to stabilize before allowing the output loop to start - await AwaitStablePositionAsync(() => ChannelAbsPositions.Get(channel), TimeSpan.FromSeconds(2), token); - Dump("Reset Servo", ChannelAbsPositions.Get(channel)); + await AwaitStablePositionAsync(() => ChannelRelativePositions.Get(channel), TimeSpan.FromSeconds(2), token); + Dump("Reset Servo", ChannelRelativePositions.Get(channel)); } return true; From 512570b8caf71f0258703c3d8b053359b9dd75a5 Mon Sep 17 00:00:00 2001 From: vicocz Date: Sun, 10 May 2026 18:55:22 +0200 Subject: [PATCH 19/29] minor fixes, finetune --- .../BrickController2/DeviceManagement/IO/StateStore.cs | 3 ++- .../DeviceManagement/Lego/WirelessProtocolBasedDevice.cs | 6 ++++-- .../DeviceManagement/TechnicMoveDevice.cs | 8 ++++---- .../BrickController2/Protocols/LegoWirelessProtocol.cs | 1 + 4 files changed, 11 insertions(+), 7 deletions(-) diff --git a/BrickController2/BrickController2/DeviceManagement/IO/StateStore.cs b/BrickController2/BrickController2/DeviceManagement/IO/StateStore.cs index 0a67abb2..e0be2882 100644 --- a/BrickController2/BrickController2/DeviceManagement/IO/StateStore.cs +++ b/BrickController2/BrickController2/DeviceManagement/IO/StateStore.cs @@ -70,7 +70,8 @@ public TValue Exchange(TKey key, Func updater) public void Clear() => _states.Clear(); /// Returns the maximum value of a projection over all stored states, or default if empty. - public TResult? Max(Func selector) => _states.IsEmpty + public TResult Max(Func selector) where TResult : struct + => _states.IsEmpty ? selector(_default) : _states.Values.Max(x => selector(x)); } diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs index d6b3ecbc..6e4afbe4 100644 --- a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs @@ -387,10 +387,12 @@ protected async ValueTask AwaitPeripheralsAttachedAsync(TimeSpan timeout, { var result = await WaitForStableValueAsync( getValue: () => AttachedPeripherals.Max(x => x.UpdateTime), - stabilityCheck: (value, last) => value != default && last == value, + stabilityCheck: (value, last) => AttachedPeripherals.Count > 0 && + value != DateTime.MinValue && + last == value, timeout, token); - return result || AttachedPeripherals.Count > 0; + return result && AttachedPeripherals.Count > 0; } } diff --git a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs index ed699de4..0f1e179d 100644 --- a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs @@ -282,7 +282,7 @@ private async ValueTask SetupChannelForPortInformationAsync(int channel, C await Task.Delay(50, token); // query current GOPOS - await WriteAsync([0x05, 0x00, 0x21, portId, 0x00], token); + await WriteAsync([0x05, 0x00, MESSAGE_TYPE_PORT_INFORMATION_REQUEST, portId, 0x00], token); await AwaitPositionChangeAsync(() => ChannelAbsPositions.Exchange(channel, x => x.ConsumeUpdate()), TimeSpan.FromMilliseconds(250), token); @@ -299,8 +299,8 @@ await AwaitPositionChangeAsync(() => ChannelRelativePositions.Exchange(channel, int CalculateCalibratedTarget(int channel, int targetBaseAngle = 0) { - int currentGopos = GetAbsPosition(channel); // True physical angle (e.g., 90) - int position = ChannelRelativePositions.Get(channel).Current; // Accumulated hub angle (e.g., 1080) + int currentGopos = GetAbsPosition(channel); // True physical angle (e.g., 90) + var position = ChannelRelativePositions.Get(channel); // Accumulated hub angle (e.g., 1080) // Calculate the shortest physical distance to your target // We use GOPOS here because it represents the actual hardware marker @@ -308,7 +308,7 @@ int CalculateCalibratedTarget(int channel, int targetBaseAngle = 0) // Apply that physical difference to the Hub's accumulated POS // If POS is 1080 and we need to move -90 degrees, the target is 990. - int targetHubPos = position + diffToTarget; + int targetHubPos = position.Current + diffToTarget; return targetHubPos; } diff --git a/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs b/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs index d157a7c5..80567e8a 100644 --- a/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs +++ b/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs @@ -23,6 +23,7 @@ internal static class LegoWirelessProtocol public const byte MESSAGE_TYPE_HUB_ACTIONS = 0x02; public const byte MESSAGE_TYPE_HUB_ATTACHED_IO = 0x04; public const byte MESSAGE_TYPE_HW_NETWORK_COMMANDS = 0x08; + public const byte MESSAGE_TYPE_PORT_INFORMATION_REQUEST = 0x21; public const byte MESSAGE_TYPE_PORT_VALUE = 0x45; public const byte MESSAGE_TYPE_PORT_VALUE_COMBINED = 0x46; public const byte MESSAGE_TYPE_OUTPUT_COMMAND_FEEDBACK = 0x82; From ef35140dd38ef249daa3484edd35a26cef2299c4 Mon Sep 17 00:00:00 2001 From: vicocz Date: Sun, 10 May 2026 19:03:44 +0200 Subject: [PATCH 20/29] . --- .../DeviceManagement/Lego/WirelessProtocolBasedDevice.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs index 6e4afbe4..062cee68 100644 --- a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs @@ -216,7 +216,7 @@ protected virtual bool TryProcessMessageData(byte messageType, ReadOnlySpan info.WithDevice(deviceId)); // store portId as "position" for simplicity + AttachedPeripherals.Update(channel, info => info.WithDevice(deviceId)); } else if (eventType == HUB_EVENT_DETACHED) { From 0af0184c037dcb61eac5255db04ab29eef51066a Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Sun, 10 May 2026 18:21:14 +0000 Subject: [PATCH 21/29] Fix typo in comment: "post value format" -> "port value format" Agent-Logs-Url: https://github.com/vicocz/brickcontroller2/sessions/391dbddf-e12f-4d5f-bf17-72fec9d7ff6b Co-authored-by: vicocz <11756608+vicocz@users.noreply.github.com> --- .../DeviceManagement/Lego/WirelessProtocolBasedDevice.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs index 6e4afbe4..2dd16c45 100644 --- a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs @@ -192,7 +192,7 @@ protected virtual bool TryProcessMessageData(byte messageType, ReadOnlySpan= 4 => ToInt32(currentData), From 867e74138b4177f97c4525753b3850e67343c8b8 Mon Sep 17 00:00:00 2001 From: vicocz Date: Sun, 10 May 2026 21:01:40 +0200 Subject: [PATCH 22/29] nitpicks --- .../DeviceManagement/Lego/ChannelPositionStore.cs | 12 ++++++++++++ .../Lego/WirelessProtocolBasedDevice.cs | 12 ++++++------ .../DeviceManagement/TechnicMoveDevice.cs | 8 ++++---- 3 files changed, 22 insertions(+), 10 deletions(-) create mode 100644 BrickController2/BrickController2/DeviceManagement/Lego/ChannelPositionStore.cs diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/ChannelPositionStore.cs b/BrickController2/BrickController2/DeviceManagement/Lego/ChannelPositionStore.cs new file mode 100644 index 00000000..9cd13a9a --- /dev/null +++ b/BrickController2/BrickController2/DeviceManagement/Lego/ChannelPositionStore.cs @@ -0,0 +1,12 @@ +using BrickController2.DeviceManagement.IO; + +namespace BrickController2.DeviceManagement.Lego; + +internal class ChannelPositionStore : ChannelStateStore +{ + public ChannelPositionStore() : base(PositionInfo.Initial) + { + } + + internal PositionInfo ConsumeUpdate(int channel) => Exchange(channel, x => x.ConsumeUpdate()); +} diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs index e41ba1e7..d25ed782 100644 --- a/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs @@ -17,9 +17,8 @@ namespace BrickController2.DeviceManagement.Lego; internal abstract class WirelessProtocolBasedDevice : BluetoothDevice { protected readonly ChannelStateStore ChannelConfigs; - protected readonly ChannelStateStore ChannelAbsPositions; - protected readonly ChannelStateStore ChannelRelativePositions; - + protected readonly ChannelPositionStore ChannelAbsPositions; + protected readonly ChannelPositionStore ChannelRelativePositions; protected readonly ChannelStateStore AttachedPeripherals; protected IGattCharacteristic? Characteristic; @@ -28,8 +27,8 @@ protected WirelessProtocolBasedDevice(string name, string address, IDeviceReposi : base(name, address, deviceRepository, bleService) { ChannelConfigs = new(); - ChannelAbsPositions = new(PositionInfo.Initial); - ChannelRelativePositions = new(PositionInfo.Initial); + ChannelAbsPositions = new(); + ChannelRelativePositions = new(); AttachedPeripherals = new(PeripheralAttachmentInfo.Initial); } @@ -46,6 +45,7 @@ public override Task ConnectAsync( { // reset output values & positions ResetOutputValues(); + ChannelConfigs.Clear(); // Initialize configuration per channel @@ -75,7 +75,7 @@ protected virtual bool TryGetChannelIndex(byte portId, out int channelIndex) protected virtual void ResetOutputValues() { - // reset output values & positions + // reset status values & positions ChannelAbsPositions.Clear(); ChannelRelativePositions.Clear(); AttachedPeripherals.Clear(); diff --git a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs index 0f1e179d..ec2fa098 100644 --- a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs @@ -249,10 +249,10 @@ protected override bool TryProcessMessageData(byte messageType, ReadOnlySpan= 5 && data[3] == PORT_PLAYVM && (data[4] & 0x02) != 0) { + Dump("Output command feedback", data); _playVmCalibrationTcs?.TrySetResult(true); return true; } @@ -283,13 +283,13 @@ private async ValueTask SetupChannelForPortInformationAsync(int channel, C // query current GOPOS await WriteAsync([0x05, 0x00, MESSAGE_TYPE_PORT_INFORMATION_REQUEST, portId, 0x00], token); - await AwaitPositionChangeAsync(() => ChannelAbsPositions.Exchange(channel, x => x.ConsumeUpdate()), + await AwaitPositionChangeAsync(() => ChannelAbsPositions.ConsumeUpdate(channel), TimeSpan.FromMilliseconds(250), token); // setup channel to report POS position regularly var inputFormatForRelAngle = BuildPortInputFormatSetup(portId, PORT_MODE_2); await WriteAsync(inputFormatForRelAngle, token); - await AwaitPositionChangeAsync(() => ChannelRelativePositions.Exchange(channel, x => x.ConsumeUpdate()), + await AwaitPositionChangeAsync(() => ChannelRelativePositions.ConsumeUpdate(channel), TimeSpan.FromMilliseconds(250), token); // need to recalculate zero angle to support ABS POS commands @@ -417,7 +417,7 @@ private async ValueTask SendPortOutput_6LedMaskAsync(byte lightMask, byte private async ValueTask SendPortOutput_ValueAsync(int channel, byte value, CancellationToken token) { - byte[] cmd = [8, 0x00, 0x81, GetPortId(channel), 0x11, 0x51, 0x00, value]; + byte[] cmd = [8, 0x00, PORT_OUTPUT_COMMAND, GetPortId(channel), FEEDBACK_ACTION_BOTH, PORT_OUTPUT_SUBCOMMAND_WRITE_DIRECT, 0x00, value]; return await WriteAsync(cmd, token); } From 3d7c02f8a1bdcbcfb670bad9a89c1c24798ac6d1 Mon Sep 17 00:00:00 2001 From: vicocz Date: Mon, 11 May 2026 21:45:10 +0200 Subject: [PATCH 23/29] try enable servo --- .../DeviceManagement/TechnicMoveDevice.cs | 34 +++++++++++++++++-- 1 file changed, 31 insertions(+), 3 deletions(-) diff --git a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs index 6ce65719..dd047a90 100644 --- a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs @@ -62,6 +62,9 @@ public override bool IsOutputTypeSupported(int channel, ChannelOutputType output ChannelOutputType.NormalMotor => !EnablePlayVmMode || channel != CHANNEL_C, // servo for both PLAYVM and normal mode but C channel only ChannelOutputType.ServoMotor => channel == CHANNEL_C, + // stepper for normal mode but C channel only + ChannelOutputType.StepperMotor => !EnablePlayVmMode && channel == CHANNEL_C, + // other types (such as stepper) are not supported at all _ => false, }; @@ -168,6 +171,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; @@ -223,8 +231,8 @@ protected override async Task SendOutputValuesAsync(CancellationToken toke // all channels command - use original value int.MaxValue => await SendAllOutputValuesAsync(change.Value, token), // classic output command for A, B, C channels (with servo support) - CHANNEL_C when channelOutputType == ChannelOutputType.ServoMotor - => await SendPortOutput_ServoAsync(change.Key, change.Value, token), + CHANNEL_C when channelOutputType == ChannelOutputType.ServoMotor => await SendPortOutput_ServoAsync(change.Key, change.Value, token), + CHANNEL_C when channelOutputType == ChannelOutputType.StepperMotor => await SendPortOutput_StepperAsync(change.Key, change.Value, token), _ => await SendPortOutput_ValueAsync(change.Key, value, token), }; @@ -424,13 +432,32 @@ private ValueTask SendPortOutput_ValueAsync(int channel, byte value, Cance return WriteAsync(cmd, token); } - private ValueTask SendPortOutput_ServoAsync(int channel, Half value, CancellationToken token) + private async 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 = _calibratedZeroAngle + ChannelConfigs.Get(channel).ServoBaseAngle + servoAngle; var cmd = BuildPortOutput_GotoAbsPosition(portId, absPosition, servoSpeed: 50); + return await WriteAsync(cmd, token); + } + + private ValueTask SendPortOutput_StepperAsync(int channel, Half value, CancellationToken token) + { + // get current position to calculate the new target position based on the stepper angle configuration and the value + var position = ChannelRelativePositions.Get(channel); + + if (value == Half.Zero || position.UpdateTime.AddMilliseconds(250) > DateTime.Now) + { + // if value is zero or position was updated recently, skip to avoid flooding the hub with commands when the slider is not yet stabilized + return new ValueTask(true); + } + + var portId = GetPortId(channel); + + // in non PLAYVM mode, need to apply calibrated base angle as offset to reach correct position + var absPosition = position.Current + (int)value * ChannelConfigs.Get(channel).StepperAngle / 100; + var cmd = BuildPortOutput_GotoAbsPosition(portId, absPosition, servoSpeed: 50); return WriteAsync(cmd, token); } @@ -447,6 +474,7 @@ private async ValueTask SendAllOutputValuesAsync(Half value, CancellationT result = result && outputType switch { ChannelOutputType.ServoMotor => await SendPortOutput_ServoAsync(channel, value, token), + ChannelOutputType.StepperMotor => await SendPortOutput_StepperAsync(channel, value, token), _ => await SendPortOutput_ValueAsync(channel, rawValue, token), }; } From 3a280d094d63ce38bf618481bbc956fb8ef27dfe Mon Sep 17 00:00:00 2001 From: Vit Nemecky Date: Wed, 20 May 2026 21:29:50 +0200 Subject: [PATCH 24/29] try support servo for C + stepper for A-C --- .../DeviceManagement/TechnicMoveDevice.cs | 59 +++++++++++-------- 1 file changed, 34 insertions(+), 25 deletions(-) diff --git a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs index dd047a90..a2db9be5 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,21 +51,27 @@ public TechnicMoveDevice(string name, public bool EnablePlayVmMode => GetSettingValue(EnablePlayVmSettingName, true); public override bool CanAutoCalibrateOutput(int channel) => false; - public override bool CanResetOutput(int channel) => channel == CHANNEL_C; + public override bool CanResetOutput(int channel) => + (EnablePlayVmMode && channel == CHANNEL_C) || // PLAYVM - only C channel supports reset + (!EnablePlayVmMode && channel == CHANNEL_C); // TODO standard mode - all A, B and C channels - public override bool CanChangeMaxServoAngle(int channel) => !EnablePlayVmMode && channel == CHANNEL_C; + public override bool CanChangeMaxServoAngle(int channel) + => !EnablePlayVmMode && channel == CHANNEL_C; // TODO standard mode - all A, B and C channels 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 for both PLAYVM and normal mode but C channel only - ChannelOutputType.ServoMotor => channel == CHANNEL_C, - // stepper for normal mode but C channel only - ChannelOutputType.StepperMotor => !EnablePlayVmMode && channel == CHANNEL_C, - - // other types (such as stepper) are not supported at all + // servo for - PLAYVM and C channel only + // - standard mode but A,B,C channels only + ChannelOutputType.ServoMotor => + (EnablePlayVmMode && channel == CHANNEL_C) || + (!EnablePlayVmMode && 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, }; @@ -133,7 +139,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); } } @@ -201,7 +207,7 @@ protected override void ResetOutputValues() // otherwise all channels to be initialized _outputValues.Initialize(); } - _calibratedZeroAngle = default; + _calibratedZeroAngles.AsSpan().Clear(); } protected override async Task SendOutputValuesAsync(CancellationToken token) @@ -231,8 +237,8 @@ protected override async Task SendOutputValuesAsync(CancellationToken toke // all channels command - use original value int.MaxValue => await SendAllOutputValuesAsync(change.Value, token), // classic output command for A, B, C channels (with servo support) - CHANNEL_C when channelOutputType == ChannelOutputType.ServoMotor => await SendPortOutput_ServoAsync(change.Key, change.Value, token), - CHANNEL_C when channelOutputType == ChannelOutputType.StepperMotor => await SendPortOutput_StepperAsync(change.Key, change.Value, token), + <= CHANNEL_C when channelOutputType == ChannelOutputType.ServoMotor => await SendPortOutput_ServoAsync(change.Key, change.Value, token), + <= CHANNEL_C when channelOutputType == ChannelOutputType.StepperMotor => await SendPortOutput_StepperAsync(change.Key, change.Value, token), _ => await SendPortOutput_ValueAsync(change.Key, value, token), }; @@ -304,7 +310,7 @@ 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; @@ -373,7 +379,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 @@ -432,14 +439,14 @@ private ValueTask SendPortOutput_ValueAsync(int channel, byte value, Cance return WriteAsync(cmd, token); } - private async ValueTask SendPortOutput_ServoAsync(int channel, Half value, CancellationToken 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 = _calibratedZeroAngle + ChannelConfigs.Get(channel).ServoBaseAngle + servoAngle; + var absPosition = _calibratedZeroAngles[channel] + ChannelConfigs.Get(channel).ServoBaseAngle + servoAngle; var cmd = BuildPortOutput_GotoAbsPosition(portId, absPosition, servoSpeed: 50); - return await WriteAsync(cmd, token); + return WriteAsync(cmd, token); } private ValueTask SendPortOutput_StepperAsync(int channel, Half value, CancellationToken token) @@ -456,8 +463,9 @@ private ValueTask SendPortOutput_StepperAsync(int channel, Half value, Can var portId = GetPortId(channel); // in non PLAYVM mode, need to apply calibrated base angle as offset to reach correct position - var absPosition = position.Current + (int)value * ChannelConfigs.Get(channel).StepperAngle / 100; - var cmd = BuildPortOutput_GotoAbsPosition(portId, absPosition, servoSpeed: 50); + var servoValue = position.Current + (int)value * ChannelConfigs.Get(channel).StepperAngle / 100; + var servoSpeed = channel == CHANNEL_C ? (byte)50 : (byte)30; + var cmd = BuildPortOutput_GotoAbsPosition(portId, servoValue, servoSpeed); return WriteAsync(cmd, token); } @@ -471,12 +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 { - ChannelOutputType.ServoMotor => await SendPortOutput_ServoAsync(channel, value, token), - ChannelOutputType.StepperMotor => await SendPortOutput_StepperAsync(channel, value, token), - _ => 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; From e9c83d45f072936ee8520c483761dbb94c815adf Mon Sep 17 00:00:00 2001 From: Vit Nemecky Date: Wed, 27 May 2026 22:42:00 +0200 Subject: [PATCH 25/29] stepper for C channel only --- .../DeviceManagement/TechnicMoveDevice.cs | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs index a2db9be5..9d557387 100644 --- a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs @@ -51,23 +51,18 @@ 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) || // PLAYVM - only C channel supports reset - (!EnablePlayVmMode && channel == CHANNEL_C); // TODO standard mode - all A, B and C channels + public override bool CanResetOutput(int channel) => channel == CHANNEL_C; // only C channel supports reset public override bool CanChangeMaxServoAngle(int channel) - => !EnablePlayVmMode && channel == CHANNEL_C; // TODO standard mode - all A, B and C channels + => !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 for - PLAYVM and C channel only - // - standard mode but A,B,C channels only - ChannelOutputType.ServoMotor => - (EnablePlayVmMode && channel == CHANNEL_C) || - (!EnablePlayVmMode && channel == CHANNEL_C), + // 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, @@ -236,8 +231,8 @@ 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 (with servo support) - <= CHANNEL_C when channelOutputType == ChannelOutputType.ServoMotor => await SendPortOutput_ServoAsync(change.Key, change.Value, token), + 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), }; From 3b1c797361591187e11654548a41459102735cf7 Mon Sep 17 00:00:00 2001 From: Vit Nemecky Date: Wed, 27 May 2026 22:54:48 +0200 Subject: [PATCH 26/29] try relative stepper via accumulated counter --- .../DeviceManagement/IO/OutputValuesGroup.cs | 19 ++++++++++++++++ .../DeviceManagement/TechnicMoveDevice.cs | 22 ++++++++----------- 2 files changed, 28 insertions(+), 13 deletions(-) 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/TechnicMoveDevice.cs b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs index 9d557387..425df804 100644 --- a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs @@ -81,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 { @@ -91,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) }; @@ -446,21 +450,13 @@ private ValueTask SendPortOutput_ServoAsync(int channel, Half value, Cance private ValueTask SendPortOutput_StepperAsync(int channel, Half value, CancellationToken token) { - // get current position to calculate the new target position based on the stepper angle configuration and the value - var position = ChannelRelativePositions.Get(channel); - - if (value == Half.Zero || position.UpdateTime.AddMilliseconds(250) > DateTime.Now) - { - // if value is zero or position was updated recently, skip to avoid flooding the hub with commands when the slider is not yet stabilized - return new ValueTask(true); - } + // value is the accumulated step coefficient from SetOutput + var targetPosition = _calibratedZeroAngles[channel] + + (int)value * ChannelConfigs.Get(channel).StepperAngle; var portId = GetPortId(channel); - - // in non PLAYVM mode, need to apply calibrated base angle as offset to reach correct position - var servoValue = position.Current + (int)value * ChannelConfigs.Get(channel).StepperAngle / 100; var servoSpeed = channel == CHANNEL_C ? (byte)50 : (byte)30; - var cmd = BuildPortOutput_GotoAbsPosition(portId, servoValue, servoSpeed); + var cmd = BuildPortOutput_GotoAbsPosition(portId, targetPosition, servoSpeed); return WriteAsync(cmd, token); } From 891226c41571f0069089fdb111deed913bdeb292 Mon Sep 17 00:00:00 2001 From: Vit Nemecky Date: Mon, 13 Jul 2026 21:16:57 +0200 Subject: [PATCH 27/29] unit tests for AccumulateOutput --- .../IO/OutputValuesGroupTests.cs | 74 +++++++++++++++++++ 1 file changed, 74 insertions(+) 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"); + } } From 5cd1883af1522cd9b90f998016caf59b8e60be8e Mon Sep 17 00:00:00 2001 From: Vit Nemecky Date: Mon, 13 Jul 2026 22:54:08 +0200 Subject: [PATCH 28/29] fix relative pos reporting for reset --- .../DeviceManagement/Lego/WirelessProtocolBasedDevice.cs | 8 +++++++- .../DeviceManagement/TechnicMoveDevice.cs | 9 +++++++++ 2 files changed, 16 insertions(+), 1 deletion(-) 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 425df804..a85664d7 100644 --- a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs @@ -206,6 +206,11 @@ protected override void ResetOutputValues() // otherwise all channels to be initialized _outputValues.Initialize(); } + } + + protected override void InitializeChannelInfo() + { + base.InitializeChannelInfo(); _calibratedZeroAngles.AsSpan().Clear(); } @@ -305,6 +310,10 @@ 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); From 89d2b64c38197eba0eba4d1038588a7412902ff5 Mon Sep 17 00:00:00 2001 From: Vit Nemecky Date: Mon, 13 Jul 2026 23:57:39 +0200 Subject: [PATCH 29/29] update button availability after e.g. device setting change --- .../UI/ViewModels/DevicePageViewModel.cs | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) 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