diff --git a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs index f7bf38df..7a9607fb 100644 --- a/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/ControlPlusDevice.cs @@ -1,9 +1,9 @@ using BrickController2.CreationManagement; +using BrickController2.DeviceManagement.Lego; using BrickController2.PlatformServices.BluetoothLE; using System; using System.Collections.Generic; using System.Diagnostics; -using System.Linq; using System.Threading; using System.Threading.Tasks; @@ -11,7 +11,7 @@ namespace BrickController2.DeviceManagement { - internal abstract class ControlPlusDevice : BluetoothDevice + internal abstract class ControlPlusDevice : ControlPlusDeviceBase { private const int MAX_SEND_ATTEMPTS = 10; @@ -30,7 +30,7 @@ internal abstract class ControlPlusDevice : BluetoothDevice private readonly ChannelOutputType[] _channelOutputTypes; private readonly int[] _maxServoAngles; - private readonly int[] _servoBaseAngles; + protected readonly int[] _servoBaseAngles; private readonly int[] _stepperAngles; private readonly int[] _absolutePositions; @@ -40,8 +40,6 @@ internal abstract class ControlPlusDevice : BluetoothDevice private readonly object _positionLock = new object(); private readonly Stopwatch _lastSent_NormalMotor = new Stopwatch(); - private IGattCharacteristic? _characteristic; - public ControlPlusDevice(string name, string address, IDeviceRepository deviceRepository, IBluetoothLEService bleService) : base(name, address, deviceRepository, bleService) { @@ -60,8 +58,6 @@ public ControlPlusDevice(string name, string address, IDeviceRepository deviceRe _positionUpdateTimes = new DateTime[NumberOfChannels]; } - public override string BatteryVoltageSign => "%"; - public override bool IsOutputTypeSupported(int channel, ChannelOutputType outputType) // support all output types on all channels => true; @@ -125,22 +121,6 @@ public async override Task ConnectAsync( return await base.ConnectAsync(reconnect, onDeviceDisconnected, channelConfigurations, startOutputProcessing, requestDeviceInformation, token); } - public override void SetOutput(int channel, float value) - { - CheckChannel(channel); - value = CutOutputValue(value); - - var intValue = (int)(100 * value); - - lock (_outputLock) - { - if (_outputValues[channel] != intValue) - { - _outputValues[channel] = intValue; - _sendAttemptsLeft[channel] = MAX_SEND_ATTEMPTS; - } - } - } public override bool CanResetOutput(int channel) => true; @@ -166,54 +146,6 @@ public override async Task ResetOutputAsync(int channel, float value, Cancellati return await AutoCalibrateServoAsync(channel, token); } - protected override async Task ValidateServicesAsync(IEnumerable? services, CancellationToken token) - { - var service = services?.FirstOrDefault(s => s.Uuid == ServiceUuid); - _characteristic = service?.Characteristics?.FirstOrDefault(c => c.Uuid == CharacteristicUuid); - - if (_characteristic is not null) - { - return await _bleDevice!.EnableNotificationAsync(_characteristic, token); - } - - return false; - } - - protected override async ValueTask BeforeDisconnectAsync(CancellationToken token) - { - // reset notifications (if possible) - if (_characteristic != null && _bleDevice != null) - { - await _bleDevice.DisableNotificationAsync(_characteristic, token); - } - } - - protected override void BeforeDisconnectCleanup() - { - _characteristic = null; - } - - protected async Task WriteNoResponseAsync(byte[] data, bool withSendDelay = false, CancellationToken token = default) - { - var result = await _bleDevice!.WriteNoResponseAsync(_characteristic!, data, token); - - if (withSendDelay) - { - await Task.Delay(SEND_DELAY, token); - } - return result; - } - - protected Task WriteAsync(byte[] data, CancellationToken token = default) - => _bleDevice!.WriteAsync(_characteristic!, data, token); - - protected virtual byte GetPortId(int channelIndex) => (byte)channelIndex; - protected virtual bool TryGetChannelIndex(byte portId, out int channelIndex) - { - channelIndex = portId; - return portId < NumberOfChannels; - } - protected virtual byte GetChannelValue(int value) // calculate raw motor value => (byte)(value < 0 ? (255 + value) : value); @@ -262,7 +194,7 @@ protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[] switch (messageCode) { - case 0x01: // Hub properties + case MESSAGE_TYPE_HUB_PROPERTIES: // Hub properties ProcessHubPropertyData(data); break; @@ -334,44 +266,32 @@ protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[] } var modeMask = data[5]; - var dataIndex = 6; + var currentData = data.AsSpan(6); // start at index 6 if ((modeMask & 0x01) != 0) { - var absPosBuffer = BitConverter.IsLittleEndian ? - new byte[] { data[dataIndex + 0], data[dataIndex + 1] } : - new byte[] { data[dataIndex + 1], data[dataIndex + 0] }; - - var absPosition = BitConverter.ToInt16(absPosBuffer, 0); + var absPosition = ToInt16(currentData); _absolutePositions[channel] = absPosition; - dataIndex += 2; + currentData = currentData.Slice(2); } if ((modeMask & 0x02) != 0) { // TODO: Read the post value format response and determine the value length accordingly - if ((dataIndex + 3) < data.Length) + if (currentData.Length >= 4) { - var relPosBuffer = BitConverter.IsLittleEndian ? - new byte[] { data[dataIndex + 0], data[dataIndex + 1], data[dataIndex + 2], data[dataIndex + 3] } : - new byte[] { data[dataIndex + 3], data[dataIndex + 2], data[dataIndex + 1], data[dataIndex + 0] }; - - var relPosition = BitConverter.ToInt32(relPosBuffer, 0); + var relPosition = ToInt32(currentData); _relativePositions[channel] = relPosition; } - else if ((dataIndex + 1) < data.Length) + else if (currentData.Length >= 2) { - var relPosBuffer = BitConverter.IsLittleEndian ? - new byte[] { data[dataIndex + 0], data[dataIndex + 1] } : - new byte[] { data[dataIndex + 1], data[dataIndex + 0] }; - - var relPosition = BitConverter.ToInt16(relPosBuffer, 0); + var relPosition = ToInt16(currentData); _relativePositions[channel] = relPosition; } else { - _relativePositions[channel] = data[dataIndex]; + _relativePositions[channel] = currentData[0]; } _positionsUpdated[channel] = true; @@ -523,7 +443,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 +476,7 @@ private async Task SendOutputValueVirtualAsync(int virtualChannel, int cha _virtualPortSendBuffer[6] = (byte)(value1 < 0 ? (255 + value1) : value1); _virtualPortSendBuffer[7] = (byte)(value2 < 0 ? (255 + value2) : value2); - if (await _bleDevice!.WriteNoResponseAsync(_characteristic!, _virtualPortSendBuffer, token)) + if (await WriteAsync(_virtualPortSendBuffer, token)) { _lastOutputValues[channel1] = value1; _lastOutputValues[channel2] = value2; @@ -602,7 +522,7 @@ private async Task SendServoOutputValueAsync(int channel, CancellationToke } var servoCmd = GetServoCommand(channel, servoValue, servoSpeed); - if (await _bleDevice!.WriteAsync(_characteristic!, servoCmd, token)) + if (await WriteAsync(servoCmd, token)) { _lastOutputValues[channel] = v; ResetSendAttemps(channel, 0); @@ -646,7 +566,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 +603,15 @@ protected virtual async Task SetupChannelForPortInformationAsync(int chann var unlockAndEnableBuffer = new byte[] { 0x05, 0x00, 0x42, portId, 0x03 }; var result = true; - result = result && await _bleDevice!.WriteAsync(_characteristic!, lockBuffer, token); + result = result && await WriteAsync(lockBuffer, token); await Task.Delay(20, token); - result = result && await _bleDevice!.WriteAsync(_characteristic!, inputFormatForAbsAngleBuffer, token); + result = result && await WriteAsync(inputFormatForAbsAngleBuffer, token); await Task.Delay(20, token); - result = result && await _bleDevice!.WriteAsync(_characteristic!, inputFormatForRelAngleBuffer, token); + result = result && await WriteAsync(inputFormatForRelAngleBuffer, token); await Task.Delay(20, token); - result = result && await _bleDevice!.WriteAsync(_characteristic!, modeAndDataSetBuffer, token); + result = result && await WriteAsync(modeAndDataSetBuffer, token); await Task.Delay(20, token); - result = result && await _bleDevice!.WriteAsync(_characteristic!, unlockAndEnableBuffer, token); + result = result && await WriteAsync(unlockAndEnableBuffer, token); return result; } @@ -724,7 +644,7 @@ protected virtual async Task ResetServoAsync(int channel, int baseAngle, C var diff = Math.Abs(NormalizeAngle(_absolutePositions[channel] - baseAngle)); if (diff > 5) { - // Can't reset to base angle, rebease to current position not to stress the plastic + // Can't reset to base angle, rebase to current position not to stress the plastic result = result && await ResetAsync(channel, 0, token); result = result && await StopAsync(channel, token); result = result && await TurnAsync(channel, 0, 40, token); @@ -740,15 +660,38 @@ protected virtual async Task ResetServoAsync(int channel, int baseAngle, C } } - protected Task AwaitStableAbsPositionAsync(int channel, TimeSpan timeout, CancellationToken token) + protected int CalculateCalibratedTarget(int channel, int targetBaseAngle = 0) + { + int currentAbsPos; + int currentRelativeAngle; + + lock (_positionLock) + { + currentAbsPos = _absolutePositions[channel]; + currentRelativeAngle = _relativePositions[channel]; + } + + // Normalize the hardware relative angle to a clean 0-359 range + // (Crucial if your motor firmware reports APOS as -180 to 179) + int normalizedRelative = ((currentRelativeAngle % 360) + 360) % 360; + int normalizedTarget = ((targetBaseAngle % 360) + 360) % 360; + + // Calculate the raw difference + normalize + int diff = NormalizeAngle(normalizedTarget - normalizedRelative); + + // Offset the current accumulated position by the physical difference + return currentAbsPos + diff; + } + + protected Task AwaitStableRelativePositionAsync(int channel, TimeSpan timeout, CancellationToken token) { - return WaitForStablePositionAsync(timeout, GetCurrentAbsPosition, token); + return WaitForStablePositionAsync(timeout, GetCurrentRelativePosition, token); - int GetCurrentAbsPosition() + int GetCurrentRelativePosition() { lock (_positionLock) { - return _absolutePositions[channel]; + return _relativePositions[channel]; } } } @@ -838,21 +781,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; @@ -889,7 +818,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 +826,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 +835,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..6ae1a8cf --- /dev/null +++ b/BrickController2/BrickController2/DeviceManagement/IO/ChannelConfig.cs @@ -0,0 +1,11 @@ +using BrickController2.CreationManagement; + +namespace BrickController2.DeviceManagement.IO; + +internal readonly record struct ChannelConfig +{ + public ChannelOutputType ChannelOutputType { get; init; } + public int MaxServoAngle { get; init; } + public int ServoBaseAngle { get; init; } + public int StepperAngle { get; init; } +} diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/ControlPlusDeviceBase.cs b/BrickController2/BrickController2/DeviceManagement/Lego/ControlPlusDeviceBase.cs new file mode 100644 index 00000000..0e9775a1 --- /dev/null +++ b/BrickController2/BrickController2/DeviceManagement/Lego/ControlPlusDeviceBase.cs @@ -0,0 +1,201 @@ +using BrickController2.CreationManagement; +using BrickController2.DeviceManagement.IO; +using BrickController2.PlatformServices.BluetoothLE; +using System; +using System.Collections.Generic; +using System.Linq; +using System.Threading; +using System.Threading.Tasks; + + +using static BrickController2.Protocols.LegoWirelessProtocol; + +namespace BrickController2.DeviceManagement.Lego; + +internal abstract class ControlPlusDeviceBase : BluetoothDevice +{ + protected readonly OutputValuesGroup OutputValues; + protected readonly ChannelConfig[] ChannelConfigs; + + protected IGattCharacteristic? Characteristic; + + protected ControlPlusDeviceBase(string name, string address, IDeviceRepository deviceRepository, IBluetoothLEService bleService) + : base(name, address, deviceRepository, bleService) + { + OutputValues = new(NumberOfChannels); + ChannelConfigs = new ChannelConfig[NumberOfChannels]; + } + + public override string BatteryVoltageSign => "%"; + + public override void SetOutput(int channel, float value) + { + CheckChannel(channel); + + var percentValue = (Half)(100 * CutOutputValue(value)); + OutputValues.SetOutput(channel, percentValue); + } + + public override Task ConnectAsync( + bool reconnect, + Action onDeviceDisconnected, + IEnumerable channelConfigurations, + bool startOutputProcessing, + bool requestDeviceInformation, + CancellationToken token) + { + // reset output values + OutputValues.Clear(); + + //TODO angles + + // Initialize configuration per channel + + // build dictionary + var configs = channelConfigurations.ToDictionary(c => c.Channel, c => c); + + for (int i = 0; i < NumberOfChannels; i++) + { + configs.TryGetValue(i, out var config); + + ChannelConfigs[i] = config.ChannelOutputType switch + { + ChannelOutputType.ServoMotor => new() + { + ChannelOutputType = ChannelOutputType.ServoMotor, + MaxServoAngle = config.MaxServoAngle, + ServoBaseAngle = config.ServoBaseAngle + }, + ChannelOutputType.StepperMotor => new() + { + ChannelOutputType = ChannelOutputType.StepperMotor, + StepperAngle = config.StepperAngle + }, + _ => new() + }; + } + + return base.ConnectAsync(reconnect, onDeviceDisconnected, channelConfigurations, startOutputProcessing, requestDeviceInformation, token); + } + + protected virtual byte GetPortId(int channelIndex) => (byte)channelIndex; + protected virtual bool TryGetChannelIndex(byte portId, out int channelIndex) + { + channelIndex = portId; + return portId < NumberOfChannels; + } + + protected override async Task ValidateServicesAsync(IEnumerable? services, CancellationToken token) + { + var service = services?.FirstOrDefault(s => s.Uuid == ServiceUuid); + Characteristic = service?.Characteristics?.FirstOrDefault(c => c.Uuid == CharacteristicUuid); + + if (Characteristic is not null) + { + return await _bleDevice!.EnableNotificationAsync(Characteristic, token); + } + + return false; + } + + protected override async ValueTask BeforeDisconnectAsync(CancellationToken token) + { + // reset notifications (if possible) + if (Characteristic != null && _bleDevice != null) + { + await _bleDevice.DisableNotificationAsync(Characteristic, token); + } + } + + protected override void BeforeDisconnectCleanup() + { + Characteristic = null; + } + + protected async ValueTask WriteNoResponseAsync(byte[] data, TimeSpan sentDelay, CancellationToken token = default) + { + var result = await _bleDevice!.WriteNoResponseAsync(Characteristic!, data, token); + await Task.Delay(sentDelay, token); + return result; + } + + protected async ValueTask WriteNoResponseAsync(byte[] data, CancellationToken token = default) + => await _bleDevice!.WriteNoResponseAsync(Characteristic!, data, token); + + protected async ValueTask WriteAsync(byte[] data, CancellationToken token = default) + => await _bleDevice!.WriteAsync(Characteristic!, data, token); + + protected async ValueTask RequestHubPropertiesAsync(CancellationToken token) + { + try + { + // Request firmware version + await RequestHubPropertyAsync(HUB_PROPERTY_FW_VERSION, token); + // Request hardware version + await RequestHubPropertyAsync(HUB_PROPERTY_HW_VERSION, token); + // Request battery voltage + await RequestHubPropertyAsync(HUB_PROPERTY_VOLTAGE, token); + } + catch { } + } + + protected async ValueTask RequestHubPropertyAsync(byte propertyId, CancellationToken token) + { + try + { + // Request firmware version + await Task.Delay(TimeSpan.FromMilliseconds(100), token); + await _bleDevice!.WriteAsync(Characteristic!, [0x05, 0x00, 0x01, propertyId, 0x05], token); + var data = await _bleDevice!.ReadAsync(Characteristic!, token); + ProcessHubPropertyData(data); + } + catch { } + } + + protected void ProcessHubPropertyData(ReadOnlySpan data) + { + try + { + if (data.Length < 6) + { + return; + } + + var dataLength = data[0]; + var messageId = data[2]; + var propertyId = data[3]; + var propertyOperation = data[4]; + + if (messageId != MESSAGE_TYPE_HUB_PROPERTIES || propertyOperation != HUB_PROPERTY_OPERATION_UPDATE) + { + // Operation is not 'update' + return; + } + + switch (propertyId) + { + case HUB_PROPERTY_FW_VERSION: // FW version + var firmwareVersion = GetVersionString(data.Slice(5)); + if (!string.IsNullOrEmpty(firmwareVersion)) + { + FirmwareVersion = firmwareVersion; + } + break; + + case HUB_PROPERTY_HW_VERSION: // HW version + var hardwareVersion = GetVersionString(data.Slice(5)); + if (!string.IsNullOrEmpty(hardwareVersion)) + { + HardwareVersion = hardwareVersion; + } + break; + + case HUB_PROPERTY_VOLTAGE: // Battery voltage + var voltage = data[5]; + BatteryVoltage = voltage.ToString("F0"); + break; + } + } + catch { } + } +} diff --git a/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs b/BrickController2/BrickController2/DeviceManagement/Lego/RemoteControl.cs index 9891d966..aac682cb 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 : ControlPlusDeviceBase { private const string ENABLED_SETTING_NAME = "RemoteControlEnabled"; private const bool DEFAULT_ENABLED = false; - private IGattCharacteristic? _characteristic; private InputDeviceBase? _inputController; public RemoteControl(string name, string address, IEnumerable settings, IDeviceRepository deviceRepository, IBluetoothLEService bleService) @@ -33,8 +32,6 @@ public RemoteControl(string name, string address, IEnumerable sett public override int NumberOfChannels => 0; - public override string BatteryVoltageSign => "%"; - public bool IsEnabled => GetSettingValue(ENABLED_SETTING_NAME, DEFAULT_ENABLED); protected override bool AutoConnectOnFirstConnect => false; @@ -64,19 +61,6 @@ internal void ResetEvents() => RaiseButtonEvents( protected override Task ProcessOutputsAsync(CancellationToken token) => Task.CompletedTask; - protected override async Task ValidateServicesAsync(IEnumerable? services, CancellationToken token) - { - var service = services?.FirstOrDefault(s => s.Uuid == ServiceUuid); - _characteristic = service?.Characteristics?.FirstOrDefault(c => c.Uuid == CharacteristicUuid); - - if (_characteristic is not null) - { - return await _bleDevice!.EnableNotificationAsync(_characteristic, token); - } - - return false; - } - protected override async Task AfterConnectSetupAsync(bool requestDeviceInformation, CancellationToken token) { // wait until ports finish communicating with the hub @@ -84,22 +68,15 @@ protected override async Task AfterConnectSetupAsync(bool requestDeviceInf if (requestDeviceInformation) { - // Request battery voltage - await _bleDevice!.WriteAsync(_characteristic!, [0x05, 0x00, 0x01, 0x06, 0x05], token); - await Task.Delay(TimeSpan.FromMilliseconds(50), token); + await RequestHubPropertiesAsync(token); } // setup ports - 0x04 - REMOTE_MODE_KEYS var remoteButtonA = BuildPortInputFormatSetup(REMOTE_BUTTONS_LEFT, REMOTE_MODE_KEYS, interval: 1); - await _bleDevice!.WriteAsync(_characteristic!, remoteButtonA, token); + await WriteAsync(remoteButtonA, token); var remoteButtonB = BuildPortInputFormatSetup(REMOTE_BUTTONS_RIGHT, REMOTE_MODE_KEYS, interval: 1); - return await _bleDevice!.WriteAsync(_characteristic!, remoteButtonB, token); - } - - protected override void BeforeDisconnectCleanup() - { - _characteristic = null; + return await WriteAsync(remoteButtonB, token); } protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[] data) @@ -114,12 +91,7 @@ protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[] switch (messageCode) { case MESSAGE_TYPE_HUB_PROPERTIES: // Hub properties - if (data.Length >= 6 && - data[3] == HUB_PROPERTY_VOLTAGE && - data[4] == HUB_PROPERTY_OPERATION_UPDATE) - { - BatteryVoltage = data[5].ToString("F0"); - } + ProcessHubPropertyData(data); break; case MESSAGE_TYPE_HW_NETWORK_COMMANDS: // HW network commands diff --git a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs index fbe36cff..3441cbd3 100644 --- a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs +++ b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs @@ -19,6 +19,7 @@ internal class TechnicMoveDevice : ControlPlusDevice private bool _applyPlayVmMode; private volatile byte _virtualMotorValue; + private int _calibratedZeroAngle; // zero ABS angle for steering C channel in non PLAYVM mode public TechnicMoveDevice(string name, string address, @@ -37,17 +38,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, }; @@ -145,7 +146,22 @@ protected override byte[] GetServoCommand(int channel, int servoValue, int servo } var portId = GetPortId(channel); - return BuildPortOutput_GotoAbsPosition(portId, servoValue, (byte)servoSpeed); + // in non PLAYVM mode, need to apply calibrated base angle as offset to reach correct position + var value = _calibratedZeroAngle + _servoBaseAngles[channel] + servoValue; + return BuildPortOutput_GotoAbsPosition(portId, value, (byte)servoSpeed); + } + + protected override async ValueTask BeforeDisconnectAsync(CancellationToken token) + { + await base.BeforeDisconnectAsync(token); + + if (_applyPlayVmMode) + { + // reset hub LED + var ledCmd = BuildPortOutput_DirectMode(PORT_HUB_LED, HUB_LED_MODE_COLOR, HUB_LED_COLOR_WHITE); + await WriteAsync(ledCmd, token: token); + await Task.Delay(20, token); + } } protected override async Task AfterConnectSetupAsync(bool requestDeviceInformation, CancellationToken token) @@ -179,10 +195,32 @@ protected override async Task SetupChannelForPortInformationAsync(int chan { try { - // setup channel to report ABS position var portId = GetPortId(channel); - var inputFormatForAbsAngle = BuildPortInputFormatSetup(portId, PORT_MODE_3); - return await WriteAsync(inputFormatForAbsAngle, token); + var inputFormatForRelAngle = BuildPortInputFormatSetup(portId, PORT_MODE_2); + + if (_applyPlayVmMode) + { + // setup channel to report POS position regularly + return await WriteAsync(inputFormatForRelAngle, token); + } + + // setup channel to for APOS, but no notifications + var inputFormatForAbsAngle = BuildPortInputFormatSetup(portId, PORT_MODE_3, notification: PORT_VALUE_NOTIFICATION_DISABLED); + await WriteAsync(inputFormatForAbsAngle, token); + await Task.Delay(50, token); + + // query current APOS + await WriteAsync([0x05, 0x00, 0x21, portId, 0x00], token); + await Task.Delay(250, token); //TODO wait for change + + // setup channel to report POS position regularly + await WriteAsync(inputFormatForRelAngle, token); + await Task.Delay(250, token); //TODO wait for change + + // need to recalculate zero angle to support ABS POS commands + _calibratedZeroAngle = CalculateCalibratedTarget(channel); + + return true; } catch { @@ -210,12 +248,12 @@ protected override async Task ResetServoAsync(int channel, int baseAngle, { // use simple Goto ABS position var portId = GetPortId(channel); - var servoCmd = BuildPortOutput_GotoAbsPosition(portId, baseAngle, servoSpeed: 0x28); + var servoCmd = BuildPortOutput_GotoAbsPosition(portId, _calibratedZeroAngle + baseAngle, servoSpeed: 0x28); await WriteAsync(servoCmd, token: token); } // need to wait till it completes - await AwaitStableAbsPositionAsync(channel, TimeSpan.FromSeconds(4), token); + await AwaitStableRelativePositionAsync(channel, TimeSpan.FromSeconds(4), token); return true; } diff --git a/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs b/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs index fec3dcab..6b9fd247 100644 --- a/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs +++ b/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs @@ -112,6 +112,20 @@ public static void ToBytes(int value, out byte b0, out byte b1, out byte b2, out public static short ToInt16(ReadOnlySpan value) => BinaryPrimitives.ReadInt16LittleEndian(value); public static int ToInt32(ReadOnlySpan value) => BinaryPrimitives.ReadInt32LittleEndian(value); + public static int NormalizeAngle(int angle) + { + if (angle >= 180) + { + return angle - (360 * ((angle + 180) / 360)); + } + else if (angle < -180) + { + return angle + (360 * ((180 - angle) / 360)); + } + + return angle; + } + public static string GetVersionString(ReadOnlySpan data) { if (data.Length < 4)