diff --git a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs index f7bf38df..10743f25 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 : WirelessProtocolBasedDevice { private const int MAX_SEND_ATTEMPTS = 10; @@ -28,40 +28,16 @@ internal abstract class ControlPlusDevice : BluetoothDevice private readonly int[] _sendAttemptsLeft; private readonly object _outputLock = new object(); - private readonly ChannelOutputType[] _channelOutputTypes; - private readonly int[] _maxServoAngles; - private 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(); - private IGattCharacteristic? _characteristic; - public ControlPlusDevice(string name, string address, IDeviceRepository deviceRepository, IBluetoothLEService bleService) : base(name, address, deviceRepository, bleService) { _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 string BatteryVoltageSign => "%"; - public override bool IsOutputTypeSupported(int channel, ChannelOutputType outputType) // support all output types on all channels => true; @@ -83,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); } @@ -166,54 +111,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); @@ -251,195 +148,20 @@ protected virtual byte[] GetServoCommand(int channel, int servoValue, int servoS return _servoSendBuffer; } - protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[] data) + protected override void ResetOutputValues() { - if (characteristicGuid != CharacteristicUuid || data.Length < 4) - { - return; - } - - var messageCode = data[2]; - - switch (messageCode) - { - case 0x01: // Hub properties - ProcessHubPropertyData(data); - break; - - 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; + base.ResetOutputValues(); - 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 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 dataIndex = 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); - _absolutePositions[channel] = absPosition; - - dataIndex += 2; - } - - if ((modeMask & 0x02) != 0) - { - // TODO: Read the post value format response and determine the value length accordingly - if ((dataIndex + 3) < data.Length) - { - 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); - _relativePositions[channel] = relPosition; - } - else if ((dataIndex + 1) < data.Length) - { - 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); - _relativePositions[channel] = relPosition; - } - else - { - _relativePositions[channel] = data[dataIndex]; - } - - _positionsUpdated[channel] = true; - _positionUpdateTimes[channel] = DateTime.Now; - } - } - - 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; - } - } - - private static void DumpData(string header, byte[] data) - { -#if DEBUG - var s = BitConverter.ToString(data); - Debug.WriteLine($"{DateTimeOffset.Now:HH:mm:ss.f} {header}-{s}"); -#endif - } - - protected override async Task ProcessOutputsAsync(CancellationToken token) - { - try + lock (_outputLock) { - lock (_outputLock) - lock (_positionLock) - { - for (int channel = 0; channel < NumberOfChannels; channel++) - { - InitializeChannelInfo(channel); - } - } - _lastSent_NormalMotor.Reset(); - - while (!token.IsCancellationRequested) + 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; - _positionsUpdated[channel] = false; - _positionUpdateTimes[channel] = DateTime.MinValue; + _lastSent_NormalMotor.Reset(); } protected override async Task AfterConnectSetupAsync(bool requestDeviceInformation, CancellationToken token) @@ -447,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) { @@ -456,11 +178,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); } } @@ -472,7 +195,7 @@ protected override async Task AfterConnectSetupAsync(bool requestDeviceInf } } - private async Task SendOutputValuesAsync(CancellationToken token) + protected override async Task SendOutputValuesAsync(CancellationToken token) { try { @@ -480,14 +203,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; @@ -523,7 +246,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 +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 _bleDevice!.WriteNoResponseAsync(_characteristic!, _virtualPortSendBuffer, token)) + if (await WriteNoResponseAsync(_virtualPortSendBuffer, token)) { _lastOutputValues[channel1] = value1; _lastOutputValues[channel2] = value2; @@ -593,7 +316,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) @@ -602,7 +325,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); @@ -636,7 +359,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); @@ -646,7 +369,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 +406,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; } @@ -707,7 +430,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(GetAbsPosition(channel) - baseAngle); var result = true; @@ -721,10 +444,10 @@ 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(GetAbsPosition(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); @@ -740,54 +463,6 @@ protected virtual async Task ResetServoAsync(int channel, int baseAngle, C } } - protected Task AwaitStableAbsPositionAsync(int channel, TimeSpan timeout, CancellationToken token) - { - return WaitForStablePositionAsync(timeout, GetCurrentAbsPosition, token); - - int GetCurrentAbsPosition() - { - lock (_positionLock) - { - return _absolutePositions[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 @@ -800,17 +475,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 = 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 = _absolutePositions[channel]; + 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 = _absolutePositions[channel]; + var absPositionAt160 = GetAbsPosition(channel); var midPoint1 = NormalizeAngle((absPositionAtMin160 + absPositionAt160) / 2); var midPoint2 = NormalizeAngle(midPoint1 + 180); @@ -818,7 +493,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(GetAbsPosition(channel) - baseAngle); result = result && await ResetAsync(channel, 0, token); result = result && await StopAsync(channel, token); @@ -838,21 +513,7 @@ private static async Task WaitForStablePositionAsync(TimeSpan timeout, Func } } - private 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; @@ -864,23 +525,21 @@ private int RoundAngleToNearest90(int angle) private int CalculateServoSpeed(int channel, int targetAngle) { - lock (_positionLock) + var channelPositions = ChannelRelativePositions.Get(channel); + + if (channelPositions.IsUpdated) { - if (_positionsUpdated[channel]) - { - var diffAngle = Math.Abs(_relativePositions[channel] - targetAngle); - _positionsUpdated[channel] = false; + var diffAngle = Math.Abs(channelPositions.Current - targetAngle); + ChannelRelativePositions.Update(channel, x => x.ConsumeUpdate()); - 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; @@ -889,7 +548,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) @@ -897,12 +556,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) @@ -910,84 +565,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); - } - - 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 { } + 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/IO/ChannelConfig.cs b/BrickController2/BrickController2/DeviceManagement/IO/ChannelConfig.cs new file mode 100644 index 00000000..96513b7a --- /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 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..baf88ea6 --- /dev/null +++ b/BrickController2/BrickController2/DeviceManagement/IO/ChannelStateStore.cs @@ -0,0 +1,63 @@ +using System; +using System.Collections.Concurrent; +using System.Collections.Generic; + +namespace BrickController2.DeviceManagement.IO; + +/// +/// Thread-safe, key-indexed store for per-key state structs. +/// Supports atomic read, write, and functional update. +/// +internal sealed class ChannelStateStore + where TKey : notnull + where TValue : struct +{ + private readonly ConcurrentDictionary _states = new(); + private readonly TValue _default; + + public ChannelStateStore(TValue initialState = default) + { + _states = new(); + _default = initialState; + } + + public int Count => _states.Count; + + /// Returns the current state for the given key or the default state if the key does not exist. + public TValue Get(TKey key) => _states.TryGetValue(key, out var value) ? value : _default; + + /// Removes the current state for the given key. + public bool Remove(TKey key) => _states.TryRemove(key, out var _); + + /// Upsert the state for the given key. + public void Set(TKey key, TValue state = default) + { + _states[key] = state; + } + + /// + /// Atomically updates the state for the given key using the provided updater function. + /// Returns the new state. + /// + public TValue Update(TKey key, Func updater) => _states.AddOrUpdate(key, (k) => updater(_default), (k, o) => updater(o)); + + /// Clears all persisted states. + public void Clear() => _states.Clear(); + + /// Returns the maximum value of a projection over all stored states, or default if empty. + public TResult? Max(Func selector) + { + TResult? max = default; + bool first = true; + foreach (var kvp in _states) + { + var val = selector(kvp.Value); + if (first || Comparer.Default.Compare(val, max) > 0) + { + max = val; + first = false; + } + } + return max; + } +} diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/ChannelAttachmentInfo.cs b/BrickController2/BrickController2/DeviceManagement/Lego/ChannelAttachmentInfo.cs new file mode 100644 index 00000000..ffeec17a --- /dev/null +++ b/BrickController2/BrickController2/DeviceManagement/Lego/ChannelAttachmentInfo.cs @@ -0,0 +1,21 @@ +using System; + +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 }; +} diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/ChannelPositionState.cs b/BrickController2/BrickController2/DeviceManagement/Lego/ChannelPositionState.cs new file mode 100644 index 00000000..ae95887e --- /dev/null +++ b/BrickController2/BrickController2/DeviceManagement/Lego/ChannelPositionState.cs @@ -0,0 +1,25 @@ +using System; + +namespace BrickController2.DeviceManagement.Lego; + +/// +/// Immutable snapshot of a motor channel's position feedback. +/// +internal record struct ChannelPositionState( + int Current, + bool IsUpdated, + DateTime UpdateTime) +{ + public static readonly ChannelPositionState Initial = new( + Current: 0, + IsUpdated: false, + UpdateTime: DateTime.MinValue); + + /// 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() => + this with { IsUpdated = false }; +} diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs b/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs index 9891d966..cc50079f 100644 --- a/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs +++ b/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs @@ -15,12 +15,11 @@ namespace BrickController2.DeviceManagement.Lego; /// /// Represents a LEGO® Powered Up 88010 Remote Control /// -internal class RemoteControl : BluetoothDevice +internal class RemoteControl : WirelessProtocolBasedDevice { 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,12 +32,8 @@ 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; - public override void SetOutput(int channel, float value) => throw new InvalidOperationException(); internal void ConnectInputController(TController inputController) where TController : InputDeviceBase @@ -64,70 +59,34 @@ 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 - await Task.Delay(250, token); + await AwaitForHubConnectedAsync(TimeSpan.FromMilliseconds(250), token); 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) + protected override bool TryProcessMessageData(byte messageType, ReadOnlySpan data) { - if (data.Length < 4) + switch (messageType) { - return; - } - - var messageCode = data[2]; - - 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"); - } - 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 @@ -136,19 +95,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..ae227e8f --- /dev/null +++ b/BrickController2/BrickController2/DeviceManagement/Lego/WirelessProtocolBasedDevice.cs @@ -0,0 +1,492 @@ +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.CreationManagement.ControllerDefaults; +using static BrickController2.Diagnostics.Logs; +using static BrickController2.Protocols.LegoWirelessProtocol; + +namespace BrickController2.DeviceManagement.Lego; + +internal abstract class WirelessProtocolBasedDevice : BluetoothDevice +{ + protected readonly ChannelConfig[] ChannelConfigs; + protected readonly ChannelStateStore ChannelAbsPositions; + protected readonly ChannelStateStore ChannelRelativePositions; + + protected readonly ChannelStateStore AttachedHubs; + + protected IGattCharacteristic? Characteristic; + + protected WirelessProtocolBasedDevice(string name, string address, IDeviceRepository deviceRepository, IBluetoothLEService bleService) + : base(name, address, deviceRepository, bleService) + { + ChannelConfigs = new ChannelConfig[NumberOfChannels]; + ChannelAbsPositions = new(ChannelPositionState.Initial); + ChannelRelativePositions = new(ChannelPositionState.Initial); + AttachedHubs = new(ChannelAttachmentInfo.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(); + ChannelAbsPositions.Clear(); + ChannelRelativePositions.Clear(); + AttachedHubs.Clear(); + + // Initialize configuration per channel + + // build dictionary, but for supported ones only + var configs = channelConfigurations + .Where(c => IsOutputTypeSupported(c.Channel, c.ChannelOutputType)) + .ToDictionary(c => c.Channel, c => c); + + 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 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) + { + 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)); + ChannelAbsPositions.Update(channel, pos => pos.WithPosition(absPosition)); + } + else if (data.Length == 8) + { + // assume 32 bit data is REL + var relPosition = ToInt32(data.Slice(4)); + ChannelRelativePositions.Update(channel, pos => pos.WithPosition(relPosition)); + } + Dump("PORT_VALUE", data); + 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); + ChannelAbsPositions.Update(channel, pos => pos.WithPosition(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] + }; + ChannelRelativePositions.Update(channel, pos => pos.WithPosition(relPosition)); + } + 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 + Dump("Hub actions", data); + break; + + case 0x03: // Hub alerts + Dump("Hub alerts", data); + break; + + case 0x05: // Generic error messages + Dump("Generic error messages", data); + break; + + case 0x08: // HW network commands + Dump("HW network commands", data); + break; + + case 0x13: // FW lock status + Dump("FW lock status", data); + break; + + case 0x43: // Port information + Dump("Port information", data); + break; + + case 0x44: // Port mode information + Dump("Port mode information", data); + break; + + case 0x47: // Port input format (Single mode) + Dump("Port input format (single)", data); + break; + + case 0x48: // Port input format (Combined mode) + Dump("Port input format (combined)", data); + break; + + case MESSAGE_TYPE_OUTPUT_COMMAND_FEEDBACK: // Port output command feedback + Dump("Output command feedback", data); + 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 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 int GetAbsPosition(int channel) => ChannelAbsPositions.Get(channel).Current; + + 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 = ChannelRelativePositions.Get(channel).Current; + + // Normalize the hardware relative angle to a clean 0-359 range + // (Crucial if your motor firmware reports APOS as -180 to 179) + int normalizedRelative = ((position % 360) + 360) % 360; + int normalizedTarget = ((targetBaseAngle % 360) + 360) % 360; + + // Calculate the raw difference + normalize + int diff = NormalizeAngle(normalizedTarget - normalizedRelative); + + // Offset the current accumulated position by the physical difference + return GetAbsPosition(channel) + diff; + } + + protected Task AwaitStableRelativePositionAsync(int channel, TimeSpan timeout, CancellationToken token) + { + return WaitForStablePositionAsync(timeout, () => ChannelRelativePositions.Get(channel), token); + } + + protected Task AwaitStableAbsolutePositionAsync(int channel, TimeSpan timeout, CancellationToken token) + { + 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); + var stabilityTimeout = TimeSpan.FromMilliseconds(500); + + using var linkedCts = CancellationTokenSource.CreateLinkedTokenSource(token); + linkedCts.CancelAfter(timeout); + + var lastPosition = getPosition().Current; + var stableSince = Stopwatch.StartNew(); + + try + { + while (!linkedCts.Token.IsCancellationRequested) + { + await Task.Delay(interval, linkedCts.Token); + + var currentPosition = getPosition(); + if (!currentPosition.IsUpdated || currentPosition.Current != lastPosition) + { + lastPosition = currentPosition.Current; + stableSince.Restart(); + } + else if (stableSince.Elapsed >= stabilityTimeout) + { + Dump("Servo position: STABLE", lastPosition); + return; // position stable for the required duration + } + } + Dump("Servo position: TIMEOUT", lastPosition); + } + catch (OperationCanceledException) when (!token.IsCancellationRequested) + { + // total timeout elapsed — treat as completed + Dump("Servo position: CANCELLED", lastPosition); + } + } + +} diff --git a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs index fbe36cff..addf8b6a 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,31 @@ using System.Linq; using System.Threading; using System.Threading.Tasks; + +using static BrickController2.Diagnostics.Logs; 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 + private TaskCompletionSource? _playVmCalibrationTcs; public TechnicMoveDevice(string name, string address, @@ -37,17 +51,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, }; @@ -58,29 +72,32 @@ 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 ResetServoAsync(channel, Convert.ToInt32(value * 180), token); } protected override byte GetPortId(int channelIndex) => channelIndex switch @@ -99,90 +116,178 @@ 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) + protected override async ValueTask BeforeDisconnectAsync(CancellationToken token) { - // 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) + await base.BeforeDisconnectAsync(token); + + if (_applyPlayVmMode) { - // no need to update lights - lastOutputValue = 0; - sendAttemptsLeft = 0; + // reset hub LED + var ledCmd = BuildPortOutput_DirectMode(PORT_HUB_LED, HUB_LED_MODE_COLOR, HUB_LED_COLOR_WHITE); + await WriteAsync(ledCmd, token: token); + await DelayAsync(token); } - base.InitializeChannelInfo(channel, lastOutputValue, sendAttemptsLeft); } - protected override byte[] GetOutputCommand(int channel, int value) + protected override async Task AfterConnectSetupAsync(bool requestDeviceInformation, CancellationToken token) { - // 6LED - var ledIndex = channel - 3; - if (ledIndex >= 0) + try { - var rawValue = ToByte(Math.Abs(value)); - var ledMask = ToByte(1 << ledIndex); - return BuildPortOutput_LedMask(PORT_6LEDS, PORT_MODE_0, ledMask, rawValue); + // wait until ports finish communicating with the hub + await AwaitForHubConnectedAsync(TimeSpan.FromSeconds(1), token); + + 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); + 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); + + // port configuration + for (int channel = 0; channel < NumberOfChannels; channel++) + { + var channelConfig = ChannelConfigs[channel]; + if (channelConfig.OutputType == ChannelOutputType.ServoMotor) + { + await SetupChannelForPortInformationAsync(channel, token); + await ResetServoAsync(channel, channelConfig.ServoBaseAngle, token); + } + } + + return result; + } + catch + { + return false; } - return base.GetOutputCommand(channel, value); } - protected override byte[] GetServoCommand(int channel, int servoValue, int servoSpeed) + protected override void ResetOutputValues() { + base.ResetOutputValues(); if (_applyPlayVmMode) { - return BuildPortOutput_PlayVm(speedValue: _virtualMotorValue, servoValue: servoValue); + _playVmValues.Initialize(); + // output values - clear always lights — suppress initial burst to avoid flooding the hub + _outputValues.Clear(); } - - var portId = GetPortId(channel); - return BuildPortOutput_GotoAbsPosition(portId, servoValue, (byte)servoSpeed); + else + { + _playVmValues.Clear(); + // otherwise all channels to be initialized + _outputValues.Initialize(); + } + _calibratedZeroAngle = default; } - protected override async Task AfterConnectSetupAsync(bool requestDeviceInformation, CancellationToken token) + protected override async Task SendOutputValuesAsync(CancellationToken token) { - if (await base.AfterConnectSetupAsync(requestDeviceInformation, token)) + try { - try - { - // 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 Task.Delay(20, 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); - - return result; - } - catch + // 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; + } + catch + { + return false; } + } - 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; + } + return base.TryProcessMessageData(messageType, data); } - protected override async Task SetupChannelForPortInformationAsync(int channel, CancellationToken token) + private async Task SetupChannelForPortInformationAsync(int channel, CancellationToken token) { try { - // setup channel to report ABS position var portId = GetPortId(channel); - var inputFormatForAbsAngle = BuildPortInputFormatSetup(portId, PORT_MODE_3); - return await WriteAsync(inputFormatForAbsAngle, token); + + if (_applyPlayVmMode) + { + // setup channel to report APOS position + var inputFormatForAbsAngle = BuildPortInputFormatSetup(portId, PORT_MODE_3); + await WriteAsync(inputFormatForAbsAngle, token); + await Task.Delay(300, token); + return true; + } + + // setup channel to for APOS, but no notifications + var inputFormatForAbsAngleDisabled = BuildPortInputFormatSetup(portId, PORT_MODE_3, notification: PORT_VALUE_NOTIFICATION_DISABLED); + await WriteAsync(inputFormatForAbsAngleDisabled, 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 + var inputFormatForRelAngle = BuildPortInputFormatSetup(portId, PORT_MODE_2); + await WriteAsync(inputFormatForRelAngle, token); + await Task.Delay(250, token); //TODO wait for change + + // need to recalculate zero angle to support ABS POS commands + _calibratedZeroAngle = CalculateCalibratedTarget(channel); + + return true; } catch { @@ -190,7 +295,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 { @@ -202,20 +307,41 @@ protected override async Task ResetServoAsync(int channel, int baseAngle, 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); + + // wait for the hub's completion feedback + try + { + await _playVmCalibrationTcs.Task.WaitAsync(TimeSpan.FromSeconds(4), token); + 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); + } + finally + { + _playVmCalibrationTcs = null; + } } else { // 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); + // 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)); return true; } @@ -224,5 +350,77 @@ protected override async Task ResetServoAsync(int channel, int baseAngle, return false; } } + + private async Task SendPlayVmOutputValueAsync(CancellationToken token) + { + try + { + if (_applyPlayVmMode && _playVmValues.TryGetValues(out var values)) + { + var maxServoAngle = GetMaxServoAngle(CHANNEL_C); + var speed = ToByte(values[PLAYVM_CHANNEL_DRIVE]); + var servoValue = maxServoAngle * (int)values[PLAYVM_CHANNEL_STEER] / 100; + var playVmCmd = BuildPortOutput_PlayVm(speed, servoValue); + + if (!await WriteAsync(playVmCmd, token)) + { + await DelayAsync(token); + 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; //TODO MAX SERVO ANGLE + var cmd = BuildPortOutput_GotoAbsPosition(portId, absPosition, servoSpeed: 50); + return await WriteAsync(cmd, 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, rawValue, 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, (int)value, token), + _ => await SendPortOutput_ValueAsync(channel, rawValue, token), + }; + } + + return result; + } } } diff --git a/BrickController2/BrickController2/Diagnostics/Logs.cs b/BrickController2/BrickController2/Diagnostics/Logs.cs new file mode 100644 index 00000000..f5a8eeba --- /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) + { + 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 fec3dcab..3ee73ea5 100644 --- a/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs +++ b/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs @@ -20,8 +20,12 @@ 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; + public const byte MESSAGE_TYPE_OUTPUT_COMMAND_FEEDBACK = 0x82; // TechnicMove hub ports public const byte PORT_DRIVE_MOTOR_1 = 0x32; @@ -111,6 +115,21 @@ 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) + { + 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) {