From 4d66de2c41105dc252ed51546e22b2648b6e819e Mon Sep 17 00:00:00 2001 From: Vit Nemecky Date: Wed, 1 Apr 2026 23:43:52 +0200 Subject: [PATCH 01/12] 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/12] 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/12] 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/12] 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/12] 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/12] 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/12] 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/12] 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/12] 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/12] 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/12] 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/12] 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)