From 532bbf394778cb4c32cd6240d514c1c859298791 Mon Sep 17 00:00:00 2001 From: Vit Nemecky Date: Sat, 2 Nov 2024 13:01:37 +0100 Subject: [PATCH 1/3] Reset send attemp for channel if write was successful [Control+] --- .../DeviceManagement/ControlPlusDevice.cs | 134 +++++++++--------- 1 file changed, 68 insertions(+), 66 deletions(-) diff --git a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs index 299ed558..75556ca9 100644 --- a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs @@ -15,7 +15,7 @@ internal abstract class ControlPlusDevice : BluetoothDevice private static readonly Guid SERVICE_UUID = new Guid("00001623-1212-efde-1623-785feabcd123"); private static readonly Guid CHARACTERISTIC_UUID = new Guid("00001624-1212-efde-1623-785feabcd123"); - private static readonly TimeSpan SEND_DELAY = TimeSpan.FromMilliseconds(60); + private static readonly TimeSpan SEND_DELAY = TimeSpan.FromMilliseconds(10); private static readonly TimeSpan POSITION_EXPIRATION = TimeSpan.FromMilliseconds(200); private readonly byte[] _sendBuffer = new byte[] { 8, 0x00, 0x81, 0x00, 0x11, 0x51, 0x00, 0x00 }; @@ -282,14 +282,15 @@ protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[] break; case 0x82: // Port output command feedback + DumpData("Output command feedback", data); break; } } private void DumpData(string header, byte[] data) { - //var s = BitConverter.ToString(data); - //Console.WriteLine(header + " - " + s); + var s = BitConverter.ToString(data); + Console.WriteLine(header + " - " + s); } protected override async Task ProcessOutputsAsync(CancellationToken token) @@ -350,6 +351,41 @@ protected override async Task AfterConnectSetupAsync(bool requestDeviceInf } } + protected virtual async Task SetupServoAsync(int channel, int baseAngle, CancellationToken token = default) + { + await SetupChannelForPortInformationAsync(channel, token); + await Task.Delay(300, token); + await ResetServoAsync(channel, baseAngle, token); + } + + private void GetOutputValue(int channel, out int value, out bool sendAttemptsLeft) + { + lock (_outputLock) + { + value = _outputValues[channel]; + var attemptsLeft = _sendAttemptsLeft[channel]; + sendAttemptsLeft = attemptsLeft > 0; + _sendAttemptsLeft[channel] = sendAttemptsLeft ? attemptsLeft - 1 : 0; + } + } + + private void OnOutputValueApplied(int channel, int value) + { + _lastOutputValues[channel] = value; + // reset send attemps due to success + lock (_outputLock) + { + _sendAttemptsLeft[channel] = MAX_SEND_ATTEMPTS; + } + } + + private Task OnOutputValueAppliedAsync(int channel, int value, CancellationToken token) + { + OnOutputValueApplied(channel, value); + + return Task.Delay(SEND_DELAY, token); + } + private async Task SendOutputValuesAsync(CancellationToken token) { try @@ -387,30 +423,18 @@ private async Task SendOutputValueAsync(int channel, CancellationToken tok { try { - int v, sendAttemptsLeft; - - lock (_outputLock) - { - v = _outputValues[channel]; - sendAttemptsLeft = _sendAttemptsLeft[channel]; - _sendAttemptsLeft[channel] = sendAttemptsLeft > 0 ? sendAttemptsLeft - 1 : 0; - } + // get output value (under lock) + GetOutputValue(channel, out var v, out var sendAttemptsLeft); - if (v != _lastOutputValues[channel] || sendAttemptsLeft > 0) + if (v != _lastOutputValues[channel] || sendAttemptsLeft) { _sendBuffer[3] = (byte)channel; _sendBuffer[7] = (byte)(v < 0 ? (255 + v) : v); - - if (await _bleDevice!.WriteNoResponseAsync(_characteristic!, _sendBuffer, token)) - { - _lastOutputValues[channel] = v; - await Task.Delay(SEND_DELAY, token); - return true; - } - else + if (!await _bleDevice!.WriteNoResponseAsync(_characteristic!, _sendBuffer, token)) { return false; } + await OnOutputValueAppliedAsync(channel, v, token); } return true; @@ -457,16 +481,10 @@ private async Task SendServoOutputValueAsync(int channel, CancellationToke { try { - int v, sendAttemptsLeft; + // get output value (under lock) + GetOutputValue(channel, out var v, out var sendAttemptsLeft); - lock (_outputLock) - { - v = _outputValues[channel]; - sendAttemptsLeft = _sendAttemptsLeft[channel]; - _sendAttemptsLeft[channel] = sendAttemptsLeft > 0 ? sendAttemptsLeft - 1 : 0; - } - - if (v != _lastOutputValues[channel] || sendAttemptsLeft > 0) + if (v != _lastOutputValues[channel] || sendAttemptsLeft) { var servoValue = _maxServoAngles[channel] * v / 100; var servoSpeed = CalculateServoSpeed(channel, servoValue); @@ -483,16 +501,11 @@ private async Task SendServoOutputValueAsync(int channel, CancellationToke _servoSendBuffer[9] = (byte)((servoValue >> 24) & 0xff); _servoSendBuffer[10] = (byte)servoSpeed; - if (await _bleDevice!.WriteNoResponseAsync(_characteristic!, _servoSendBuffer, token)) - { - _lastOutputValues[channel] = v; - await Task.Delay(SEND_DELAY, token); - return true; - } - else + if (!await _bleDevice!.WriteNoResponseAsync(_characteristic!, _servoSendBuffer, token)) { return false; } + await OnOutputValueAppliedAsync(channel, v, token); } return true; @@ -507,14 +520,8 @@ private async Task SendStepperOutputValueAsync(int channel, CancellationTo { try { - int v, sendAttemptsLeft; - - lock (_outputLock) - { - v = _outputValues[channel]; - sendAttemptsLeft = _sendAttemptsLeft[channel]; - _sendAttemptsLeft[channel] = sendAttemptsLeft > 0 ? sendAttemptsLeft - 1 : 0; - } + // get output value (under lock) + GetOutputValue(channel, out var v, out var sendAttemptsLeft); var stepperAngle = _stepperAngles[channel]; _stepperSendBuffer[3] = (byte)channel; @@ -526,20 +533,15 @@ private async Task SendStepperOutputValueAsync(int channel, CancellationTo if (v != _lastOutputValues[channel] && Math.Abs(v) == 100) { - if (await _bleDevice!.WriteNoResponseAsync(_characteristic!, _stepperSendBuffer, token)) - { - _lastOutputValues[channel] = v; - await Task.Delay(SEND_DELAY, token); - return true; - } - else + if (!await _bleDevice!.WriteNoResponseAsync(_characteristic!, _stepperSendBuffer, token)) { return false; } + await OnOutputValueAppliedAsync(channel, v, token); } else { - _lastOutputValues[channel] = v; + OnOutputValueApplied(channel, v); } return true; @@ -592,11 +594,11 @@ private async Task ResetServoAsync(int channel, int baseAngle, Cancellatio result = result && await ResetAsync(channel, 0, token); result = result && await StopAsync(channel, token); result = result && await TurnAsync(channel, 0, 40, token); - await Task.Delay(50); + await Task.Delay(50, token); result = result && await StopAsync(channel, token); result = result && await ResetAsync(channel, resetToAngle, token); result = result && await TurnAsync(channel, 0, 40, token); - await Task.Delay(500); + await Task.Delay(500, token); result = result && await StopAsync(channel, token); var diff = Math.Abs(NormalizeAngle(_absolutePositions[channel] - baseAngle)); @@ -606,7 +608,7 @@ private async Task ResetServoAsync(int channel, int baseAngle, Cancellatio result = result && await ResetAsync(channel, 0, token); result = result && await StopAsync(channel, token); result = result && await TurnAsync(channel, 0, 40, token); - await Task.Delay(50); + await Task.Delay(50, token); result = result && await StopAsync(channel, token); } @@ -627,19 +629,19 @@ private async Task ResetServoAsync(int channel, int baseAngle, Cancellatio result = result && await ResetAsync(channel, 0, token); result = result && await StopAsync(channel, token); result = result && await TurnAsync(channel, 0, 50, token); - await Task.Delay(600); + await Task.Delay(600, token); result = result && await StopAsync(channel, token); - await Task.Delay(500); + await Task.Delay(500, token); var absPositionAt0 = _absolutePositions[channel]; result = result && await TurnAsync(channel, -160, 60, token); - await Task.Delay(600); + await Task.Delay(600, token); result = result && await StopAsync(channel, token); - await Task.Delay(500); + await Task.Delay(500, token); var absPositionAtMin160 = _absolutePositions[channel]; result = result && await TurnAsync(channel, 160, 60, token); - await Task.Delay(600); + await Task.Delay(600, token); result = result && await StopAsync(channel, token); - await Task.Delay(500); + await Task.Delay(500, token); var absPositionAt160 = _absolutePositions[channel]; var midPoint1 = NormalizeAngle((absPositionAtMin160 + absPositionAt160) / 2); @@ -653,11 +655,11 @@ private async Task ResetServoAsync(int channel, int baseAngle, Cancellatio result = result && await ResetAsync(channel, 0, token); result = result && await StopAsync(channel, token); result = result && await TurnAsync(channel, 0, 40, token); - await Task.Delay(50); + await Task.Delay(50, token); result = result && await StopAsync(channel, token); result = result && await ResetAsync(channel, resetToAngle, token); result = result && await TurnAsync(channel, 0, 40, token); - await Task.Delay(600); + await Task.Delay(600, token); result = result && await StopAsync(channel, token); return (result, baseAngle / 180F); @@ -750,19 +752,19 @@ private async Task RequestHubPropertiesAsync(CancellationToken token) try { // Request firmware version - await Task.Delay(TimeSpan.FromMilliseconds(300)); + 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)); + 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)); + 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); From 61ab42ec25273f08c01d1042cc655f97206aaa4e Mon Sep 17 00:00:00 2001 From: Vit Nemecky Date: Sat, 2 Nov 2024 13:52:35 +0100 Subject: [PATCH 2/3] Conditionally reset attemp count --- .../DeviceManagement/ControlPlusDevice.cs | 52 ++++++++----------- 1 file changed, 23 insertions(+), 29 deletions(-) diff --git a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs index 75556ca9..63ba195f 100644 --- a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs @@ -358,34 +358,29 @@ protected virtual async Task SetupServoAsync(int channel, int baseAngle, Cancell await ResetServoAsync(channel, baseAngle, token); } - private void GetOutputValue(int channel, out int value, out bool sendAttemptsLeft) + private void GetOutputValue(int channel, out int value, out int sendAttemptsLeft) { lock (_outputLock) { value = _outputValues[channel]; - var attemptsLeft = _sendAttemptsLeft[channel]; - sendAttemptsLeft = attemptsLeft > 0; - _sendAttemptsLeft[channel] = sendAttemptsLeft ? attemptsLeft - 1 : 0; + sendAttemptsLeft = --_sendAttemptsLeft[channel]; + _sendAttemptsLeft[channel] = Math.Max(sendAttemptsLeft, 0); } } - private void OnOutputValueApplied(int channel, int value) + private void SetLastOutputValue(int channel, int value, int sendAttemptsLeft) { _lastOutputValues[channel] = value; - // reset send attemps due to success lock (_outputLock) { - _sendAttemptsLeft[channel] = MAX_SEND_ATTEMPTS; + // conditionally reset send attemps due to success + if (_sendAttemptsLeft[channel] == sendAttemptsLeft) + { + _sendAttemptsLeft[channel] = 0; + } } } - private Task OnOutputValueAppliedAsync(int channel, int value, CancellationToken token) - { - OnOutputValueApplied(channel, value); - - return Task.Delay(SEND_DELAY, token); - } - private async Task SendOutputValuesAsync(CancellationToken token) { try @@ -426,7 +421,7 @@ private async Task SendOutputValueAsync(int channel, CancellationToken tok // get output value (under lock) GetOutputValue(channel, out var v, out var sendAttemptsLeft); - if (v != _lastOutputValues[channel] || sendAttemptsLeft) + if (v != _lastOutputValues[channel] || sendAttemptsLeft >= 0) { _sendBuffer[3] = (byte)channel; _sendBuffer[7] = (byte)(v < 0 ? (255 + v) : v); @@ -434,7 +429,8 @@ private async Task SendOutputValueAsync(int channel, CancellationToken tok { return false; } - await OnOutputValueAppliedAsync(channel, v, token); + SetLastOutputValue(channel, v, sendAttemptsLeft); + await Task.Delay(SEND_DELAY, token); } return true; @@ -455,18 +451,14 @@ 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)) - { - _lastOutputValues[channel1] = value1; - _lastOutputValues[channel2] = value2; - - await Task.Delay(SEND_DELAY, token); - return true; - } - else + if (!await _bleDevice!.WriteNoResponseAsync(_characteristic!, _virtualPortSendBuffer, token)) { return false; } + _lastOutputValues[channel1] = value1; + _lastOutputValues[channel2] = value2; + + await Task.Delay(SEND_DELAY, token); } return true; @@ -484,7 +476,7 @@ private async Task SendServoOutputValueAsync(int channel, CancellationToke // get output value (under lock) GetOutputValue(channel, out var v, out var sendAttemptsLeft); - if (v != _lastOutputValues[channel] || sendAttemptsLeft) + if (v != _lastOutputValues[channel] || sendAttemptsLeft >= 0) { var servoValue = _maxServoAngles[channel] * v / 100; var servoSpeed = CalculateServoSpeed(channel, servoValue); @@ -505,7 +497,8 @@ private async Task SendServoOutputValueAsync(int channel, CancellationToke { return false; } - await OnOutputValueAppliedAsync(channel, v, token); + SetLastOutputValue(channel, v, sendAttemptsLeft); + await Task.Delay(SEND_DELAY, token); } return true; @@ -537,11 +530,12 @@ private async Task SendStepperOutputValueAsync(int channel, CancellationTo { return false; } - await OnOutputValueAppliedAsync(channel, v, token); + SetLastOutputValue(channel, v, sendAttemptsLeft); + await Task.Delay(SEND_DELAY, token); } else { - OnOutputValueApplied(channel, v); + SetLastOutputValue(channel, v, sendAttemptsLeft); } return true; From 05ec2d2e6e820ce7bd58445aa988579c7ce74235 Mon Sep 17 00:00:00 2001 From: Vit Nemecky Date: Sat, 2 Nov 2024 14:06:45 +0100 Subject: [PATCH 3/3] cleanup --- .../DeviceManagement/ControlPlusDevice.cs | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs index 63ba195f..0022f85c 100644 --- a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs @@ -289,8 +289,8 @@ protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[] private void DumpData(string header, byte[] data) { - var s = BitConverter.ToString(data); - Console.WriteLine(header + " - " + s); + //var s = BitConverter.ToString(data); + //Console.WriteLine(header + " - " + s); } protected override async Task ProcessOutputsAsync(CancellationToken token) @@ -351,13 +351,6 @@ protected override async Task AfterConnectSetupAsync(bool requestDeviceInf } } - protected virtual async Task SetupServoAsync(int channel, int baseAngle, CancellationToken token = default) - { - await SetupChannelForPortInformationAsync(channel, token); - await Task.Delay(300, token); - await ResetServoAsync(channel, baseAngle, token); - } - private void GetOutputValue(int channel, out int value, out int sendAttemptsLeft) { lock (_outputLock)