diff --git a/BrickController2/BrickController2/BrickController2.csproj b/BrickController2/BrickController2/BrickController2.csproj
index 46b4cca3..ce027e5c 100644
--- a/BrickController2/BrickController2/BrickController2.csproj
+++ b/BrickController2/BrickController2/BrickController2.csproj
@@ -46,6 +46,8 @@
+
+
diff --git a/BrickController2/BrickController2/DeviceManagement/BluetoothDeviceManager.cs b/BrickController2/BrickController2/DeviceManagement/BluetoothDeviceManager.cs
index 821959b4..2f496f9c 100644
--- a/BrickController2/BrickController2/DeviceManagement/BluetoothDeviceManager.cs
+++ b/BrickController2/BrickController2/DeviceManagement/BluetoothDeviceManager.cs
@@ -96,6 +96,7 @@ public async Task ScanAsync(Func ValidateServicesAsync(IEnumerable 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 int GetChannelIndex(byte portId) => portId;
+
+ protected virtual byte GetChannelValue(int value)
+ // calculate raw motor value
+ => (byte)(value < 0 ? (255 + value) : value);
+
+ protected void ResetSendAttemps(int channel, int attemps = MAX_SEND_ATTEMPTS)
+ {
+ lock (_outputLock)
+ {
+ // do it conditionally
+ if (_sendAttemptsLeft[channel] != MAX_SEND_ATTEMPTS)
+ {
+ _sendAttemptsLeft[channel] = attemps;
+ }
+ }
+ }
+
+ protected virtual byte[] GetOutputCommand(int channel, int value)
+ {
+ // send base motor value (-100 .. 100 %)
+ _sendBuffer[3] = GetPortId(channel);
+ _sendBuffer[7] = GetChannelValue(value);
+
+ return _sendBuffer;
+ }
+
+ protected virtual byte[] GetServoCommand(int channel, int servoValue, int servoSpeed)
+ {
+ _servoSendBuffer[3] = GetPortId(channel);
+ _servoSendBuffer[6] = (byte)(servoValue & 0xff);
+ _servoSendBuffer[7] = (byte)((servoValue >> 8) & 0xff);
+ _servoSendBuffer[8] = (byte)((servoValue >> 16) & 0xff);
+ _servoSendBuffer[9] = (byte)((servoValue >> 24) & 0xff);
+ _servoSendBuffer[10] = (byte)servoSpeed;
+
+ return _servoSendBuffer;
+ }
+
protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[] data)
{
if (characteristicGuid != CHARACTERISTIC_UUID || data.Length < 4)
@@ -218,13 +274,32 @@ protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[]
break;
case 0x45: // Port value (single mode)
- DumpData("Port value (single)", data);
+ lock (_positionLock)
+ {
+ if (data.Length == 6)
+ {
+ // assume 16bit data is ABS
+ var channel = GetChannelIndex(data[3]);
+ var absPosition = ToInt16(data, 4);
+ _absolutePositions[channel] = absPosition;
+ }
+ else if (data.Length == 8)
+ {
+ // assume 32 bit data is REL
+ var channel = GetChannelIndex(data[3]);
+ var relPosition = ToInt32(data, 4);
+ _relativePositions[channel] = relPosition;
+
+ _positionsUpdated[channel] = true;
+ _positionUpdateTimes[channel] = DateTime.Now;
+ }
+ }
break;
case 0x46: // Port value (combined mode)
lock (_positionLock)
{
- var portId = data[3];
+ var channel = GetChannelIndex(data[3]);
var modeMask = data[5];
var dataIndex = 6;
@@ -235,7 +310,7 @@ protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[]
new byte[] { data[dataIndex + 1], data[dataIndex + 0] };
var absPosition = BitConverter.ToInt16(absPosBuffer, 0);
- _absolutePositions[portId] = absPosition;
+ _absolutePositions[channel] = absPosition;
dataIndex += 2;
}
@@ -250,7 +325,7 @@ protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[]
new byte[] { data[dataIndex + 3], data[dataIndex + 2], data[dataIndex + 1], data[dataIndex + 0] };
var relPosition = BitConverter.ToInt32(relPosBuffer, 0);
- _relativePositions[portId] = relPosition;
+ _relativePositions[channel] = relPosition;
}
else if ((dataIndex + 1) < data.Length)
{
@@ -259,15 +334,15 @@ protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[]
new byte[] { data[dataIndex + 1], data[dataIndex + 0] };
var relPosition = BitConverter.ToInt16(relPosBuffer, 0);
- _relativePositions[portId] = relPosition;
+ _relativePositions[channel] = relPosition;
}
else
{
- _relativePositions[portId] = data[dataIndex];
+ _relativePositions[channel] = data[dataIndex];
}
- _positionsUpdated[portId] = true;
- _positionUpdateTimes[portId] = DateTime.Now;
+ _positionsUpdated[channel] = true;
+ _positionUpdateTimes[channel] = DateTime.Now;
}
}
@@ -282,6 +357,7 @@ protected override void OnCharacteristicChanged(Guid characteristicGuid, byte[]
break;
case 0x82: // Port output command feedback
+ DumpData("Output command feedback", data);
break;
}
}
@@ -301,11 +377,7 @@ protected override async Task ProcessOutputsAsync(CancellationToken token)
{
for (int channel = 0; channel < NumberOfChannels; channel++)
{
- _outputValues[channel] = 0;
- _lastOutputValues[channel] = 1;
- _sendAttemptsLeft[channel] = MAX_SEND_ATTEMPTS;
- _positionsUpdated[channel] = false;
- _positionUpdateTimes[channel] = DateTime.MinValue;
+ InitializeChannelInfo(channel);
}
}
@@ -320,6 +392,20 @@ protected override async Task ProcessOutputsAsync(CancellationToken token)
catch { }
}
+ ///
+ /// Initialize channel data when output processing is going to be started
+ ///
+ protected virtual void InitializeChannelInfo(int channel,
+ int lastOutputValue = 1,
+ int sendAttempsLeft = MAX_SEND_ATTEMPTS)
+ {
+ _outputValues[channel] = 0;
+ _lastOutputValues[channel] = lastOutputValue;
+ _sendAttemptsLeft[channel] = sendAttempsLeft;
+ _positionsUpdated[channel] = false;
+ _positionUpdateTimes[channel] = DateTime.MinValue;
+ }
+
protected override async Task AfterConnectSetupAsync(bool requestDeviceInformation, CancellationToken token)
{
try
@@ -398,12 +484,11 @@ private async Task SendOutputValueAsync(int channel, CancellationToken tok
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))
+ var outputCmd = GetOutputCommand(channel, v);
+ if (await _bleDevice!.WriteNoResponseAsync(_characteristic!, outputCmd, token))
{
_lastOutputValues[channel] = v;
+ ResetSendAttemps(channel, 0);
await Task.Delay(SEND_DELAY, token);
return true;
}
@@ -476,16 +561,11 @@ private async Task SendServoOutputValueAsync(int channel, CancellationToke
return true;
}
- _servoSendBuffer[3] = (byte)channel;
- _servoSendBuffer[6] = (byte)(servoValue & 0xff);
- _servoSendBuffer[7] = (byte)((servoValue >> 8) & 0xff);
- _servoSendBuffer[8] = (byte)((servoValue >> 16) & 0xff);
- _servoSendBuffer[9] = (byte)((servoValue >> 24) & 0xff);
- _servoSendBuffer[10] = (byte)servoSpeed;
-
- if (await _bleDevice!.WriteNoResponseAsync(_characteristic!, _servoSendBuffer, token))
+ var servoCmd = GetServoCommand(channel, servoValue, servoSpeed);
+ if (await _bleDevice!.WriteNoResponseAsync(_characteristic!, servoCmd, token))
{
_lastOutputValues[channel] = v;
+ ResetSendAttemps(channel, 0);
await Task.Delay(SEND_DELAY, token);
return true;
}
@@ -517,7 +597,7 @@ private async Task SendStepperOutputValueAsync(int channel, CancellationTo
}
var stepperAngle = _stepperAngles[channel];
- _stepperSendBuffer[3] = (byte)channel;
+ _stepperSendBuffer[3] = GetPortId(channel);
_stepperSendBuffer[6] = (byte)(stepperAngle & 0xff);
_stepperSendBuffer[7] = (byte)((stepperAngle >> 8) & 0xff);
_stepperSendBuffer[8] = (byte)((stepperAngle >> 16) & 0xff);
@@ -529,6 +609,7 @@ private async Task SendStepperOutputValueAsync(int channel, CancellationTo
if (await _bleDevice!.WriteNoResponseAsync(_characteristic!, _stepperSendBuffer, token))
{
_lastOutputValues[channel] = v;
+ ResetSendAttemps(channel, 0);
await Task.Delay(SEND_DELAY, token);
return true;
}
@@ -550,25 +631,26 @@ private async Task SendStepperOutputValueAsync(int channel, CancellationTo
}
}
- private async Task SetupChannelForPortInformationAsync(int channel, CancellationToken token)
+ protected virtual async Task SetupChannelForPortInformationAsync(int channel, CancellationToken token)
{
try
{
- var lockBuffer = new byte[] { 0x05, 0x00, 0x42, (byte)channel, 0x02 };
- var inputFormatForAbsAngleBuffer = new byte[] { 0x0a, 0x00, 0x41, (byte)channel, 0x03, 0x02, 0x00, 0x00, 0x00, 0x01 };
- var inputFormatForRelAngleBuffer = new byte[] { 0x0a, 0x00, 0x41, (byte)channel, 0x02, 0x02, 0x00, 0x00, 0x00, 0x01 };
- var modeAndDataSetBuffer = new byte[] { 0x08, 0x00, 0x42, (byte)channel, 0x01, 0x00, 0x30, 0x20 };
- var unlockAndEnableBuffer = new byte[] { 0x05, 0x00, 0x42, (byte)channel, 0x03 };
+ var portId = GetPortId(channel);
+ var lockBuffer = new byte[] { 0x05, 0x00, 0x42, portId, 0x02 };
+ var inputFormatForAbsAngleBuffer = new byte[] { 0x0a, 0x00, 0x41, portId, 0x03, 0x02, 0x00, 0x00, 0x00, 0x01 };
+ var inputFormatForRelAngleBuffer = new byte[] { 0x0a, 0x00, 0x41, portId, 0x02, 0x02, 0x00, 0x00, 0x00, 0x01 };
+ var modeAndDataSetBuffer = new byte[] { 0x08, 0x00, 0x42, portId, 0x01, 0x00, 0x30, 0x20 };
+ var unlockAndEnableBuffer = new byte[] { 0x05, 0x00, 0x42, portId, 0x03 };
var result = true;
result = result && await _bleDevice!.WriteAsync(_characteristic!, lockBuffer, token);
- await Task.Delay(20);
+ await Task.Delay(20, token);
result = result && await _bleDevice!.WriteAsync(_characteristic!, inputFormatForAbsAngleBuffer, token);
- await Task.Delay(20);
+ await Task.Delay(20, token);
result = result && await _bleDevice!.WriteAsync(_characteristic!, inputFormatForRelAngleBuffer, token);
- await Task.Delay(20);
+ await Task.Delay(20, token);
result = result && await _bleDevice!.WriteAsync(_characteristic!, modeAndDataSetBuffer, token);
- await Task.Delay(20);
+ await Task.Delay(20, token);
result = result && await _bleDevice!.WriteAsync(_characteristic!, unlockAndEnableBuffer, token);
return result;
@@ -579,7 +661,7 @@ private async Task SetupChannelForPortInformationAsync(int channel, Cancel
}
}
- private async Task ResetServoAsync(int channel, int baseAngle, CancellationToken token)
+ protected virtual async Task ResetServoAsync(int channel, int baseAngle, CancellationToken token)
{
try
{
@@ -592,11 +674,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 +688,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 +709,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 +735,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);
@@ -718,31 +800,34 @@ private int CalculateServoSpeed(int channel, int targetAngle)
private Task StopAsync(int channel, CancellationToken token)
{
- return _bleDevice!.WriteAsync(_characteristic!, new byte[] { 0x08, 0x00, 0x81, (byte)channel, 0x11, 0x51, 0x00, 0x00 }, token);
+ var portId = GetPortId(channel);
+ return _bleDevice!.WriteAsync(_characteristic!, new byte[] { 0x08, 0x00, 0x81, portId, 0x11, 0x51, 0x00, 0x00 }, token);
}
private Task TurnAsync(int channel, int angle, int speed, 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[] { 0x0e, 0x00, 0x81, (byte)channel, 0x11, 0x0d, a0, a1, a2, a3, (byte)speed, 0x64, 0x7e, 0x00 }, token);
+ return _bleDevice!.WriteAsync(_characteristic!, new byte[] { 0x0e, 0x00, 0x81, portId, 0x11, 0x0d, a0, a1, a2, a3, (byte)speed, 0x64, 0x7e, 0x00 }, token);
}
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, (byte)channel, 0x11, 0x51, 0x02, a0, a1, a2, a3 }, token);
+ return _bleDevice!.WriteAsync(_characteristic!, new byte[] { 0x0b, 0x00, 0x81, portId, 0x11, 0x51, 0x02, a0, a1, a2, a3 }, token);
}
private async Task RequestHubPropertiesAsync(CancellationToken token)
@@ -750,19 +835,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);
diff --git a/BrickController2/BrickController2/DeviceManagement/DI/DeviceManagementModule.cs b/BrickController2/BrickController2/DeviceManagement/DI/DeviceManagementModule.cs
index 2c21dbc7..cf00591f 100644
--- a/BrickController2/BrickController2/DeviceManagement/DI/DeviceManagementModule.cs
+++ b/BrickController2/BrickController2/DeviceManagement/DI/DeviceManagementModule.cs
@@ -23,6 +23,7 @@ protected override void Load(ContainerBuilder builder)
builder.RegisterType().Keyed(DeviceType.DuploTrainHub);
builder.RegisterType().Keyed(DeviceType.CircuitCubes);
builder.RegisterType().Keyed(DeviceType.WeDo2);
+ builder.RegisterType().Keyed(DeviceType.TechnicMove);
builder.Register(c =>
{
diff --git a/BrickController2/BrickController2/DeviceManagement/DeviceType.cs b/BrickController2/BrickController2/DeviceManagement/DeviceType.cs
index 64d3ca5e..c4749c5d 100644
--- a/BrickController2/BrickController2/DeviceManagement/DeviceType.cs
+++ b/BrickController2/BrickController2/DeviceManagement/DeviceType.cs
@@ -14,5 +14,6 @@ public enum DeviceType
BuWizz3,
CircuitCubes,
WeDo2,
+ TechnicMove
}
}
diff --git a/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs
new file mode 100644
index 00000000..63896f29
--- /dev/null
+++ b/BrickController2/BrickController2/DeviceManagement/TechnicMoveDevice.cs
@@ -0,0 +1,206 @@
+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
+{
+ internal class TechnicMoveDevice : ControlPlusDevice
+ {
+ 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 bool _applyPlayVmMode;
+ private byte _virtualMotorValue;
+
+ public TechnicMoveDevice(string name,
+ string address,
+ byte[] deviceData,
+ IDeviceRepository deviceRepository,
+ IBluetoothLEService bleService)
+ : base(name, address, deviceRepository, bleService)
+ {
+ }
+
+ public override DeviceType DeviceType => DeviceType.TechnicMove;
+ public override int NumberOfChannels => 9;
+
+ // This is now mandatory as the hub does not support generic servo / stepper commands (yet)
+ public bool EnablePlayVmMode => true;
+
+ public override bool CanAutoCalibrateOutput(int channel) => false;
+ public override bool CanResetOutput(int channel) => channel == CHANNEL_C;
+ public override bool CanChangeOutputType(int channel) => channel == CHANNEL_C;
+
+
+ public override Task ConnectAsync(bool reconnect, Action onDeviceDisconnected, IEnumerable channelConfigurations, bool startOutputProcessing, bool requestDeviceInformation, CancellationToken token)
+ {
+ // autodetect PLAYVM mode for A / B channels (as testing page should not be affected)
+ _applyPlayVmMode = startOutputProcessing &&
+ channelConfigurations.Any(c => c.Channel == CHANNEL_VM) &&
+ channelConfigurations.Any(c => c.Channel == CHANNEL_C && c.ChannelOutputType == CreationManagement.ChannelOutputType.ServoMotor);
+
+ // filter out non standard channels
+ var filteredConfigurtions = channelConfigurations
+ .Where(c => c.Channel != CHANNEL_VM);
+
+ return base.ConnectAsync(reconnect, onDeviceDisconnected, filteredConfigurtions, startOutputProcessing, requestDeviceInformation, token);
+ }
+
+ public override void SetOutput(int channel, float value)
+ {
+ if (channel == CHANNEL_VM)
+ {
+ // for PLAYVM mode
+ if (_applyPlayVmMode)
+ {
+ // 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
+ {
+ // user somehow defined this VM channel without servo setup for C channel
+ base.SetOutput(CHANNEL_A, -value);
+ base.SetOutput(CHANNEL_B, value);
+ }
+ }
+ else
+ {
+ base.SetOutput(channel, value);
+ }
+ }
+
+ protected override byte GetPortId(int channelIndex) => channelIndex switch
+ {
+ 0 => PORT_DRIVE_MOTOR_1,
+ 1 => PORT_DRIVE_MOTOR_2,
+ 2 => PORT_STEERING_MOTOR,
+ 3 or 4 or 5 or 6 or 7 or 8 => PORT_6LEDS,
+ _ => throw new ArgumentException($"Value of channel '{channelIndex}' is out of supported range.", nameof(channelIndex))
+ };
+
+ protected override int GetChannelIndex(byte portId) => portId switch
+ {
+ PORT_DRIVE_MOTOR_1 => 0,
+ PORT_DRIVE_MOTOR_2 => 1,
+ PORT_STEERING_MOTOR => 2,
+ // PORT_6LEDS is not supported
+ _ => throw new ArgumentException($"Value of port ID '{portId}' is out of supported ranges.", nameof(portId))
+ };
+
+ protected override byte GetChannelValue(int value) => ToByte(value);
+
+ protected override void InitializeChannelInfo(int channel, int lastOutputValue = 1, int sendAttempsLeft = 10)
+ {
+ // if PLAYVM enabled, reset A / B channels diffrently in order to avoid output writes
+ if (_applyPlayVmMode && channel < CHANNEL_C)
+ {
+ lastOutputValue = 0;
+ sendAttempsLeft = 0;
+ }
+ base.InitializeChannelInfo(channel, lastOutputValue, sendAttempsLeft);
+ }
+
+ protected override byte[] GetOutputCommand(int channel, int value)
+ {
+ // 6LED
+ var ledIndex = channel - 3;
+ if (ledIndex >= 0)
+ {
+ var rawValue = ToByte(Math.Abs(value));
+ var ledMask = ToByte(1 << ledIndex);
+ return BuildPortOutput_LedMask(PORT_6LEDS, PORT_MODE_0, ledMask, rawValue);
+ }
+ return base.GetOutputCommand(channel, value);
+ }
+
+ protected override byte[] GetServoCommand(int channel, int servoValue, int servoSpeed)
+ {
+ if (_applyPlayVmMode)
+ {
+ return BuildPortOutput_PlayVm(speedValue: _virtualMotorValue, servoValue: servoValue);
+ }
+ return base.GetServoCommand(channel, servoValue, servoSpeed);
+ }
+
+ protected override async Task AfterConnectSetupAsync(bool requestDeviceInformation, CancellationToken token)
+ {
+ if (await base.AfterConnectSetupAsync(requestDeviceInformation, token))
+ {
+ try
+ {
+ // hub LED
+ var color = _applyPlayVmMode ? HUB_LED_COLOR_MAGENTA : HUB_LED_COLOR_WHITE;
+ var ledCmd = BuildPortOutput_HubLed(PORT_HUB_LED, HUB_LED_MODE_COLOR, color);
+ await WriteNoResponseAsync(ledCmd, withSendDelay: true, token: token);
+
+ // switch lights off
+ var lightsOffCmd = BuildPortOutput_LedMask(PORT_6LEDS, PORT_MODE_0, 0xff, 0x00);
+ return await WriteNoResponseAsync(lightsOffCmd, withSendDelay: true, token: token);
+ }
+ catch
+ {
+ }
+ }
+
+ return false;
+ }
+
+ protected override async Task SetupChannelForPortInformationAsync(int channel, CancellationToken token)
+ {
+ if (!EnablePlayVmMode)
+ {
+ return await base.SetupChannelForPortInformationAsync(channel, token);
+ }
+
+ try
+ {
+ // setup channel to report ABS position
+ var portId = GetPortId(channel);
+ var inputFormatForAbsAngle = BuildPortInputFormatSetup(portId, PORT_MODE_3);
+ return await WriteAsync(inputFormatForAbsAngle, token);
+ }
+ catch
+ {
+ return false;
+ }
+ }
+
+ protected override async Task ResetServoAsync(int channel, int baseAngle, CancellationToken token)
+ {
+ if (!EnablePlayVmMode)
+ {
+ return await base.ResetServoAsync(channel, baseAngle, token);
+ }
+
+ try
+ {
+ // reset servo via PLAYVM
+ // PLAYVM cmd supports only servo on C channel
+ var servoCmd = BuildPortOutput_PlayVm(servoValue: baseAngle, vmCmd: PLAYVM_COMMAND);
+ await WriteNoResponseAsync(servoCmd, token: token);
+ await Task.Delay(100, token);
+
+ // do calibration
+ var calibrateCmd = BuildPortOutput_PlayVm(servoValue: baseAngle, vmCmd: PLAYVM_CALIBRATE_STEERING);
+ await WriteNoResponseAsync(calibrateCmd, token: token);
+ await Task.Delay(750, token);
+
+ return true;
+ }
+ catch
+ {
+ return false;
+ }
+ }
+ }
+}
diff --git a/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs b/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs
new file mode 100644
index 00000000..e6d7ae6e
--- /dev/null
+++ b/BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs
@@ -0,0 +1,107 @@
+using System;
+using System.Buffers.Binary;
+
+namespace BrickController2.Protocols;
+
+///
+/// Contains implementation of Lego Wireless Protocol
+/// Inspired by
+///
+internal static class LegoWirelessProtocol
+{
+ // TechnicMove hub ports
+ public const byte PORT_DRIVE_MOTOR_1 = 0x32;
+ public const byte PORT_DRIVE_MOTOR_2 = 0x33;
+ public const byte PORT_STEERING_MOTOR = 0x34;
+ public const byte PORT_6LEDS = 0x35;
+ public const byte PORT_HUB_LED = 0x3F;
+
+ // port modes
+ public const byte PORT_MODE_0 = 0x00;
+ public const byte PORT_MODE_1 = 0x01;
+ public const byte PORT_MODE_2 = 0x02;
+ public const byte PORT_MODE_3 = 0x03;
+ public const byte PORT_MODE_4 = 0x04;
+
+ // output command
+ public const byte PORT_OUTPUT_COMMAND = 0x81;
+
+ public const byte PORT_OUTPUT_SUBCOMMAND_WRITE_DIRECT = 0x51;
+
+ // - output / playvm command
+ public const byte PORT_PLAYVM = 0x36;
+
+ public const byte PLAYVM_LIGHTS_OFF_OFF = 0x04;
+ public const byte PLAYVM_CALIBRATE_STEERING = 0x08;
+ public const byte PLAYVM_COMMAND = 0x10;
+
+ // - output / HUB LED colors
+ public const byte HUB_LED_MODE_COLOR = 0x00;
+ public const byte HUB_LED_MODE_RGB = 0x01;
+
+ public const byte HUB_LED_COLOR_NONE = 0x00;
+ public const byte HUB_LED_COLOR_PINK = 0x01;
+ public const byte HUB_LED_COLOR_MAGENTA = 0x02;
+ public const byte HUB_LED_COLOR_BLUE = 0x03;
+ public const byte HUB_LED_COLOR_LIGHT_BLUE = 0x04;
+ public const byte HUB_LED_COLOR_CYAN = 0x05;
+ public const byte HUB_LED_COLOR_GREEN = 0x06;
+ public const byte HUB_LED_COLOR_YELLOW = 0x07;
+ public const byte HUB_LED_COLOR_ORANGE = 0x08;
+ public const byte HUB_LED_COLOR_RED = 0x09;
+ public const byte HUB_LED_COLOR_WHITE = 0xA;
+
+ // input command (single)
+ public const byte PORT_INPUT_COMMAND = 0x41;
+
+ public const byte PORT_VALUE_NOTIFICATION_DISABLED = 0x00;
+ public const byte PORT_VALUE_NOTIFICATION_ENABLED = 0x01;
+
+ public const byte FEEDBACK_ACTION_NO_ACTION = 0x00;
+ public const byte FEEDBACK_ACTION_ACTION_COMPLETION = 0x01;
+ public const byte FEEDBACK_ACTION_ACTION_START = 0x10;
+ public const byte FEEDBACK_ACTION_BOTH = 0x11;
+
+ // conversion methods
+ public static void ToBytes(int value, out byte b0, out byte b1, out byte b2, out byte b3)
+ {
+ b0 = (byte)(value & 0xff);
+ b1 = (byte)((value >> 8) & 0xff);
+ b2 = (byte)((value >> 16) & 0xff);
+ b3 = (byte)((value >> 24) & 0xff);
+ }
+
+ public static byte ToByte(int value) => (byte)(value & 0xFF);
+
+ public static short ToInt16(byte[] value, int startIndex) => ToInt16(value.AsSpan(startIndex));
+ public static int ToInt32(byte[] value, int startIndex) => ToInt32(value.AsSpan(startIndex));
+
+ public static short ToInt16(ReadOnlySpan value) => BinaryPrimitives.ReadInt16LittleEndian(value);
+ public static int ToInt32(ReadOnlySpan value) => BinaryPrimitives.ReadInt32LittleEndian(value);
+
+ // message builders
+ public static byte[] BuildPortInputFormatSetup(byte portId, byte portMode, int interval = 2, byte notification = PORT_VALUE_NOTIFICATION_ENABLED)
+ {
+ // Message Type - Port Input Format Setup (Single) [0x41]
+ ToBytes(interval, out var i0, out var i1, out var i2, out var i3);
+ return [0x0a, 0x00, PORT_INPUT_COMMAND, portId, portMode, i0, i1, i2, i3, notification];
+ }
+
+ public static byte[] BuildPortOutput_LedMask(byte portId, byte portMode, byte ledMask, byte value)
+ // Message Type - Port Output Command [0x81] | Write Direct
+ => [9, 0x00, PORT_OUTPUT_COMMAND, portId, FEEDBACK_ACTION_BOTH,
+ PORT_OUTPUT_SUBCOMMAND_WRITE_DIRECT, portMode, ledMask, value];
+
+ public static byte[] BuildPortOutput_HubLed(byte portId, byte mode, byte color)
+ // Message Type - Port Output Command [0x81] | Write Direct
+ => [8, 0x00, PORT_OUTPUT_COMMAND, portId, FEEDBACK_ACTION_BOTH,
+ PORT_OUTPUT_SUBCOMMAND_WRITE_DIRECT, mode, color];
+
+ public static byte[] BuildPortOutput_PlayVm(int speedValue = 0, int servoValue = 0, byte vmCmd = PLAYVM_LIGHTS_OFF_OFF)
+ {
+ var speedRaw = ToByte(speedValue);
+ var steeringRaw = ToByte(servoValue);
+ return [13, 0x00, PORT_OUTPUT_COMMAND, PORT_PLAYVM, FEEDBACK_ACTION_BOTH,
+ PORT_OUTPUT_SUBCOMMAND_WRITE_DIRECT, PORT_MODE_0, 0x03, 0x00, speedRaw, steeringRaw, vmCmd, 0x00];
+ }
+}
diff --git a/BrickController2/BrickController2/UI/Controls/DeviceChannelLabel.cs b/BrickController2/BrickController2/UI/Controls/DeviceChannelLabel.cs
index b64a0cda..bf10194e 100644
--- a/BrickController2/BrickController2/UI/Controls/DeviceChannelLabel.cs
+++ b/BrickController2/BrickController2/UI/Controls/DeviceChannelLabel.cs
@@ -8,6 +8,7 @@ namespace BrickController2.UI.Controls
public class DeviceChannelLabel : Label
{
private readonly static string[] _controlPlusChannelLetters = new[] { "A", "B", "C", "D" };
+ private readonly static string[] _technicMove = ["A", "B", "C", "1", "2", "3", "4", "5", "6"];
private readonly static string[] _circuitCubesChannelLetters = new[] { "A", "B", "C" };
private readonly static string[] _buwizz3ChannelLetters = new[] { "1", "2", "3", "4", "A", "B" };
@@ -51,15 +52,21 @@ private void SetChannelText()
case DeviceType.PoweredUp:
case DeviceType.TechnicHub:
case DeviceType.WeDo2:
- Text = _controlPlusChannelLetters[Math.Min(Math.Max(Channel, 0), 3)];
+ SetChannelText(_controlPlusChannelLetters);
+ break;
+ case DeviceType.TechnicMove:
+ if (Channel == TechnicMoveDevice.CHANNEL_VM)
+ Text = "AB";
+ else
+ SetChannelText(_technicMove);
break;
case DeviceType.CircuitCubes:
- Text = _circuitCubesChannelLetters[Math.Min(Math.Max(Channel, 0), 2)];
+ SetChannelText(_circuitCubesChannelLetters);
break;
case DeviceType.BuWizz3:
- Text = _buwizz3ChannelLetters[Math.Min(Math.Max(Channel, 0), 6)];
+ SetChannelText(_buwizz3ChannelLetters);
break;
case DeviceType.Infrared:
@@ -73,5 +80,8 @@ private void SetChannelText()
break;
}
}
+
+ private void SetChannelText(string[] labels)
+ => Text = labels[Math.Min(Math.Max(Channel, 0), labels.Length - 1)];
}
}
diff --git a/BrickController2/BrickController2/UI/Controls/DeviceChannelSelector.xaml b/BrickController2/BrickController2/UI/Controls/DeviceChannelSelector.xaml
index 53d95f53..8a22550f 100644
--- a/BrickController2/BrickController2/UI/Controls/DeviceChannelSelector.xaml
+++ b/BrickController2/BrickController2/UI/Controls/DeviceChannelSelector.xaml
@@ -214,6 +214,32 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/BrickController2/BrickController2/UI/Controls/DeviceChannelSelector.xaml.cs b/BrickController2/BrickController2/UI/Controls/DeviceChannelSelector.xaml.cs
index f829e1ed..106728d0 100644
--- a/BrickController2/BrickController2/UI/Controls/DeviceChannelSelector.xaml.cs
+++ b/BrickController2/BrickController2/UI/Controls/DeviceChannelSelector.xaml.cs
@@ -1,7 +1,8 @@
-using Microsoft.Maui.Controls;
-using Microsoft.Maui.Controls.Xaml;
-using BrickController2.DeviceManagement;
+using BrickController2.DeviceManagement;
using BrickController2.UI.Commands;
+using Microsoft.Maui.Controls;
+using Microsoft.Maui.Controls.Xaml;
+using Device = BrickController2.DeviceManagement.Device;
namespace BrickController2.UI.Controls
{
@@ -44,15 +45,25 @@ public DeviceChannelSelector()
CircuitCubesC.Command = new SafeCommand(() => SelectedChannel = 2);
WedoChannel0.Command = new SafeCommand(() => SelectedChannel = 0);
WedoChannel1.Command = new SafeCommand(() => SelectedChannel = 1);
+ TechnicMoveChannelA.Command = new SafeCommand(() => SelectedChannel = 0);
+ TechnicMoveChannelB.Command = new SafeCommand(() => SelectedChannel = 1);
+ TechnicMoveChannelAB.Command = new SafeCommand(() => SelectedChannel = TechnicMoveDevice.CHANNEL_VM);
+ TechnicMoveChannelC.Command = new SafeCommand(() => SelectedChannel = 2);
+ TechnicMoveChannel1.Command = new SafeCommand(() => SelectedChannel = 3);
+ TechnicMoveChannel2.Command = new SafeCommand(() => SelectedChannel = 4);
+ TechnicMoveChannel3.Command = new SafeCommand(() => SelectedChannel = 5);
+ TechnicMoveChannel4.Command = new SafeCommand(() => SelectedChannel = 6);
+ TechnicMoveChannel5.Command = new SafeCommand(() => SelectedChannel = 7);
+ TechnicMoveChannel6.Command = new SafeCommand(() => SelectedChannel = 8);
}
- public static BindableProperty DeviceTypeProperty = BindableProperty.Create(nameof(DeviceType), typeof(DeviceType), typeof(DeviceChannelSelector), default(DeviceType), BindingMode.OneWay, null, OnDeviceTypeChanged);
- public static BindableProperty SelectedChannelProperty = BindableProperty.Create(nameof(SelectedChannel), typeof(int), typeof(DeviceChannelSelector), 0, BindingMode.TwoWay, null, OnSelectedChannelChanged);
+ public static readonly BindableProperty DeviceProperty = BindableProperty.Create(nameof(Device), typeof(Device), typeof(DeviceChannelSelector), default(Device), BindingMode.OneWay, null, OnDeviceChanged);
+ public static readonly BindableProperty SelectedChannelProperty = BindableProperty.Create(nameof(SelectedChannel), typeof(int), typeof(DeviceChannelSelector), 0, BindingMode.TwoWay, null, OnSelectedChannelChanged);
- public DeviceType DeviceType
+ public Device Device
{
- get => (DeviceType)GetValue(DeviceTypeProperty);
- set => SetValue(DeviceTypeProperty, value);
+ get => (Device)GetValue(DeviceProperty);
+ set => SetValue(DeviceProperty, value);
}
public int SelectedChannel
@@ -61,11 +72,11 @@ public int SelectedChannel
set => SetValue(SelectedChannelProperty, value);
}
- private static void OnDeviceTypeChanged(BindableObject bindable, object oldValue, object newValue)
+ private static void OnDeviceChanged(BindableObject bindable, object oldValue, object newValue)
{
- if (bindable is DeviceChannelSelector dcs)
+ if (bindable is DeviceChannelSelector dcs && newValue is Device device)
{
- var deviceType = (DeviceType)newValue;
+ var deviceType = device.DeviceType;
dcs.SbrickSection.IsVisible = deviceType == DeviceType.SBrick;
dcs.BuWizzSection.IsVisible = deviceType == DeviceType.BuWizz || deviceType == DeviceType.BuWizz2;
dcs.BuWizz3Section.IsVisible = deviceType == DeviceType.BuWizz3;
@@ -76,6 +87,12 @@ private static void OnDeviceTypeChanged(BindableObject bindable, object oldValue
dcs.DuploTrainHubSection.IsVisible = deviceType == DeviceType.DuploTrainHub;
dcs.CircuitCubes.IsVisible = deviceType == DeviceType.CircuitCubes;
dcs.Wedo2Section.IsVisible = deviceType == DeviceType.WeDo2;
+ // Technic Move enablement
+ var isPlayVm = device is TechnicMoveDevice moveDevice && moveDevice.EnablePlayVmMode;
+ dcs.TechnicMoveSection.IsVisible = deviceType == DeviceType.TechnicMove;
+ dcs.TechnicMoveChannelA.IsVisible = !isPlayVm;
+ dcs.TechnicMoveChannelB.IsVisible = !isPlayVm;
+ dcs.TechnicMoveChannelAB.IsVisible = isPlayVm;
}
}
@@ -116,6 +133,16 @@ private static void OnSelectedChannelChanged(BindableObject bindable, object old
dcs.CircuitCubesC.SelectedChannel = selectedChannel;
dcs.WedoChannel0.SelectedChannel = selectedChannel;
dcs.WedoChannel1.SelectedChannel = selectedChannel;
+ dcs.TechnicMoveChannelA.SelectedChannel = selectedChannel;
+ dcs.TechnicMoveChannelB.SelectedChannel = selectedChannel;
+ dcs.TechnicMoveChannelAB.SelectedChannel = selectedChannel;
+ dcs.TechnicMoveChannelC.SelectedChannel = selectedChannel;
+ dcs.TechnicMoveChannel1.SelectedChannel = selectedChannel;
+ dcs.TechnicMoveChannel2.SelectedChannel = selectedChannel;
+ dcs.TechnicMoveChannel3.SelectedChannel = selectedChannel;
+ dcs.TechnicMoveChannel4.SelectedChannel = selectedChannel;
+ dcs.TechnicMoveChannel5.SelectedChannel = selectedChannel;
+ dcs.TechnicMoveChannel6.SelectedChannel = selectedChannel;
}
}
}
diff --git a/BrickController2/BrickController2/UI/Converters/DeviceTypeToImageConverter.cs b/BrickController2/BrickController2/UI/Converters/DeviceTypeToImageConverter.cs
index ba6938e5..c20f6efa 100644
--- a/BrickController2/BrickController2/UI/Converters/DeviceTypeToImageConverter.cs
+++ b/BrickController2/BrickController2/UI/Converters/DeviceTypeToImageConverter.cs
@@ -49,6 +49,8 @@ public class DeviceTypeToImageConverter : IValueConverter
case DeviceType.WeDo2:
return ResourceHelper.GetImageResource("wedo2hub_image.png");
+ case DeviceType.TechnicMove:
+ return ResourceHelper.GetImageResource("technic_move.png");
default:
return null;
}
diff --git a/BrickController2/BrickController2/UI/Converters/DeviceTypeToSmallImageConverter.cs b/BrickController2/BrickController2/UI/Converters/DeviceTypeToSmallImageConverter.cs
index bc6d7b5b..682b9a0d 100644
--- a/BrickController2/BrickController2/UI/Converters/DeviceTypeToSmallImageConverter.cs
+++ b/BrickController2/BrickController2/UI/Converters/DeviceTypeToSmallImageConverter.cs
@@ -44,6 +44,9 @@ public class DeviceTypeToSmallImageConverter : IValueConverter
case DeviceType.WeDo2:
return ResourceHelper.GetImageResource("wedo2hub_image_small.png");
+ case DeviceType.TechnicMove:
+ return ResourceHelper.GetImageResource("technic_move_small.png");
+
default:
return null;
}
diff --git a/BrickController2/BrickController2/UI/Images/technic_move.png b/BrickController2/BrickController2/UI/Images/technic_move.png
new file mode 100644
index 00000000..e273428c
Binary files /dev/null and b/BrickController2/BrickController2/UI/Images/technic_move.png differ
diff --git a/BrickController2/BrickController2/UI/Images/technic_move_small.png b/BrickController2/BrickController2/UI/Images/technic_move_small.png
new file mode 100644
index 00000000..e0f926c6
Binary files /dev/null and b/BrickController2/BrickController2/UI/Images/technic_move_small.png differ
diff --git a/BrickController2/BrickController2/UI/Pages/ControllerActionPage.xaml b/BrickController2/BrickController2/UI/Pages/ControllerActionPage.xaml
index c5fc3462..c8d44af3 100644
--- a/BrickController2/BrickController2/UI/Pages/ControllerActionPage.xaml
+++ b/BrickController2/BrickController2/UI/Pages/ControllerActionPage.xaml
@@ -52,7 +52,7 @@
-
+
diff --git a/BrickController2/BrickController2/UI/ViewModels/ControllerActionPageViewModel.cs b/BrickController2/BrickController2/UI/ViewModels/ControllerActionPageViewModel.cs
index 5adca421..7b1d192b 100644
--- a/BrickController2/BrickController2/UI/ViewModels/ControllerActionPageViewModel.cs
+++ b/BrickController2/BrickController2/UI/ViewModels/ControllerActionPageViewModel.cs
@@ -228,15 +228,20 @@ private async Task OpenDeviceDetailsAsync()
private async Task SelectChannelOutputTypeAsync()
{
+ // do simple filtering of Stepper for TechnicMove
+ var channelOutputTypes = SelectedDevice?.DeviceType != DeviceType.TechnicMove ?
+ Enum.GetNames() :
+ Enum.GetNames().Where(x => x != Enum.GetName(ChannelOutputType.StepperMotor));
+
var result = await _dialogService.ShowSelectionDialogAsync(
- Enum.GetNames(typeof(ChannelOutputType)),
+ channelOutputTypes,
Translate("ChannelType"),
Translate("Cancel"),
_disappearingTokenSource?.Token ?? default);
if (result.IsOk)
{
- Action.ChannelOutputType = (ChannelOutputType)Enum.Parse(typeof(ChannelOutputType), result.SelectedItem);
+ Action.ChannelOutputType = Enum.Parse(result.SelectedItem);
}
}
diff --git a/README.md b/README.md
index b4dd8e0c..714bc71c 100644
--- a/README.md
+++ b/README.md
@@ -18,6 +18,7 @@ Cross platform mobile application for controlling your creations using a bluetoo
- Lego Boost Hub
- Lego Technic Hub
- Lego WeDo 2.0 Smart Hub
+- Lego Technic Move Hub (PLAYVM mode)
- Circuit Cubes
## Project details