Add Techic Move hub (LEGO 42176)

BrickController2/BrickController2/BrickController2.csproj

@@ -46,6 +46,8 @@
     <EmbeddedResource Include="UI\Images\sbrick_image.png" />
     <EmbeddedResource Include="UI\Images\sbrick_image_small.png" />
     <EmbeddedResource Include="UI\Images\ic_sequence.png" />
+    <EmbeddedResource Include="UI\Images\technic_move.png" />
+    <EmbeddedResource Include="UI\Images\technic_move_small.png" />
     <EmbeddedResource Include="UI\Images\wedo2hub_image.png" />
     <EmbeddedResource Include="UI\Images\wedo2hub_image_small.png" />
   </ItemGroup>

BrickController2/BrickController2/DeviceManagement/BluetoothDeviceManager.cs

@@ -96,6 +96,7 @@ public async Task<bool> ScanAsync(Func<DeviceType, string, string, byte[]?, Task
                             case "40": return (DeviceType.Boost, manufacturerData);
                             case "41": return (DeviceType.PoweredUp, manufacturerData);
                             case "80": return (DeviceType.TechnicHub, manufacturerData);
+                            case "84": return (DeviceType.TechnicMove, manufacturerData);
                             //case "20": return (DeviceType.DuploTrainHub, manufacturerData);
                         }
                     }

BrickController2/BrickController2/DeviceManagement/DI/DeviceManagementModule.cs

@@ -23,6 +23,7 @@ protected override void Load(ContainerBuilder builder)
             builder.RegisterType<DuploTrainHubDevice>().Keyed<Device>(DeviceType.DuploTrainHub);
             builder.RegisterType<CircuitCubeDevice>().Keyed<Device>(DeviceType.CircuitCubes);
             builder.RegisterType<Wedo2Device>().Keyed<Device>(DeviceType.WeDo2);
+            builder.RegisterType<TechnicMoveDevice>().Keyed<Device>(DeviceType.TechnicMove);
 
             builder.Register<DeviceFactory>(c =>
             {

BrickController2/BrickController2/DeviceManagement/DeviceType.cs

@@ -14,5 +14,6 @@ public enum DeviceType
         BuWizz3,
         CircuitCubes,
         WeDo2,
+        TechnicMove
     }
 }

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<DeviceConnectionResult> ConnectAsync(bool reconnect, Action<Device> onDeviceDisconnected, IEnumerable<ChannelConfiguration> 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<bool> 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<bool> 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<bool> 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;
+            }
+        }
+    }
+}

BrickController2/BrickController2/Protocols/LegoWirelessProtocol.cs

@@ -0,0 +1,107 @@
+using System;
+using System.Buffers.Binary;
+
+namespace BrickController2.Protocols;
+
+/// <summary>
+/// Contains implementation of Lego Wireless Protocol <see href="https://lego.github.io/lego-ble-wireless-protocol-docs/"/>
+/// Inspired by <see href="https://github.com/toorisrael/LEGO-Porsche-Controller/blob/main/utils/lwp3_definitions.py"/>
+/// </summary>
+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<byte> value) => BinaryPrimitives.ReadInt16LittleEndian(value);
+    public static int ToInt32(ReadOnlySpan<byte> 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];
+    }
+}

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)];
     }
 }