diff --git a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs index 299ed558..0022f85c 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,6 +282,7 @@ protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[] break; case 0x82: // Port output command feedback + DumpData("Output command feedback", data); break; } } @@ -350,6 +351,29 @@ protected override async Task AfterConnectSetupAsync(bool requestDeviceInf } } + private void GetOutputValue(int channel, out int value, out int sendAttemptsLeft) + { + lock (_outputLock) + { + value = _outputValues[channel]; + sendAttemptsLeft = --_sendAttemptsLeft[channel]; + _sendAttemptsLeft[channel] = Math.Max(sendAttemptsLeft, 0); + } + } + + private void SetLastOutputValue(int channel, int value, int sendAttemptsLeft) + { + _lastOutputValues[channel] = value; + lock (_outputLock) + { + // conditionally reset send attemps due to success + if (_sendAttemptsLeft[channel] == sendAttemptsLeft) + { + _sendAttemptsLeft[channel] = 0; + } + } + } + private async Task SendOutputValuesAsync(CancellationToken token) { try @@ -387,30 +411,19 @@ 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 >= 0) { _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; } + SetLastOutputValue(channel, v, sendAttemptsLeft); + await Task.Delay(SEND_DELAY, token); } return true; @@ -431,18 +444,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; @@ -457,16 +466,10 @@ private async Task SendServoOutputValueAsync(int channel, CancellationToke { 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 >= 0) { var servoValue = _maxServoAngles[channel] * v / 100; var servoSpeed = CalculateServoSpeed(channel, servoValue); @@ -483,16 +486,12 @@ 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; } + SetLastOutputValue(channel, v, sendAttemptsLeft); + await Task.Delay(SEND_DELAY, token); } return true; @@ -507,14 +506,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 +519,16 @@ 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; } + SetLastOutputValue(channel, v, sendAttemptsLeft); + await Task.Delay(SEND_DELAY, token); } else { - _lastOutputValues[channel] = v; + SetLastOutputValue(channel, v, sendAttemptsLeft); } return true; @@ -592,11 +581,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 +595,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 +616,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 +642,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 +739,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);