Skip to content

Reset send attemp for a channel if write was successful [Control+] #127

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
vicocz wants to merge 3 commits into from
Closed

Reset send attemp for a channel if write was successful [Control+] #127

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
532bbf3
Reset send attemp for channel if write was successful [Control+]
Nov 2, 2024
61ab42e
Conditionally reset attemp count
Nov 2, 2024
05ec2d2
cleanup
Nov 2, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
135 changes: 62 additions & 73 deletions BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 };
Expand Down Expand Up @@ -282,6 +282,7 @@ protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[]
break;

case 0x82: // Port output command feedback
DumpData("Output command feedback", data);
break;
}
}
Expand Down Expand Up @@ -350,6 +351,29 @@ protected override async Task<bool> 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<bool> SendOutputValuesAsync(CancellationToken token)
{
try
Expand Down Expand Up @@ -387,30 +411,19 @@ private async Task<bool> 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;
Expand All @@ -431,18 +444,14 @@ private async Task<bool> 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;
Expand All @@ -457,16 +466,10 @@ private async Task<bool> 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);
Expand All @@ -483,16 +486,12 @@ private async Task<bool> 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;
Expand All @@ -507,14 +506,8 @@ private async Task<bool> 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;
Expand All @@ -526,20 +519,16 @@ private async Task<bool> 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;
Expand Down Expand Up @@ -592,11 +581,11 @@ private async Task<bool> 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));
Expand All @@ -606,7 +595,7 @@ private async Task<bool> 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);
}

Expand All @@ -627,19 +616,19 @@ private async Task<bool> 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);
Expand All @@ -653,11 +642,11 @@ private async Task<bool> 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);
Expand Down Expand Up @@ -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);
Expand Down