Hi everyone,

Most solutions (official and inofficial) to Control PoweredUP/Control+ models are for Android/iOS only. For a quite a while I have been looking for a simple, portable solution to run on any Windows PC/tablet, using a gamepad or a touchscreen as input device. I haven't found a satisfying program, so I tried to create something by reusing existing apps/code snippets. As a basis I chose the "Lego Brick Automation Project" (BAP).

It handles all the Bluetooth communication and Motor control for me, but unfortunately the source code is not yet available. But it allows to implement custom C# code. However, the problem is that the custom code is placed within a class, so you can't easily include new assemblies. But it is possibly to load DLLs, so I could implement a library for reading inputs from an XBox controller (https://github.com/speps/XInputDotNet).

The global code in the BAP now looks as follows:

Spoiler

// XInputDotNetPure
// https://github.com/speps/XInputDotNet

class Imports
{
    internal const string DLLName = "XInputInterface";

    [System.Runtime.InteropServices.DllImport(DLLName)]
    public static extern uint XInputGamePadGetState(uint playerIndex, out GamePadState.RawState state);
    [System.Runtime.InteropServices.DllImport(DLLName)]
    public static extern void XInputGamePadSetState(uint playerIndex, float leftMotor, float rightMotor);
}

public enum ButtonState
{
    Pressed,
    Released
}

public struct GamePadButtons
{
    ButtonState start, back, leftStick, rightStick, leftShoulder, rightShoulder, guide, a, b, x, y;

    internal GamePadButtons(ButtonState start, ButtonState back, ButtonState leftStick, ButtonState rightStick,
                            ButtonState leftShoulder, ButtonState rightShoulder, ButtonState guide,
                            ButtonState a, ButtonState b, ButtonState x, ButtonState y)
    {
        this.start = start;
        this.back = back;
        this.leftStick = leftStick;
        this.rightStick = rightStick;
        this.leftShoulder = leftShoulder;
        this.rightShoulder = rightShoulder;
        this.guide = guide;
        this.a = a;
        this.b = b;
        this.x = x;
        this.y = y;
    }

    public ButtonState Start
    {
        get { return start; }
    }

    public ButtonState Back
    {
        get { return back; }
    }

    public ButtonState LeftStick
    {
        get { return leftStick; }
    }

    public ButtonState RightStick
    {
        get { return rightStick; }
    }

    public ButtonState LeftShoulder
    {
        get { return leftShoulder; }
    }

    public ButtonState RightShoulder
    {
        get { return rightShoulder; }
    }

    public ButtonState Guide
    {
        get { return guide; }
    }

    public ButtonState A
    {
        get { return a; }
    }

    public ButtonState B
    {
        get { return b; }
    }

    public ButtonState X
    {
        get { return x; }
    }

    public ButtonState Y
    {
        get { return y; }
    }
}

public struct GamePadDPad
{
    ButtonState up, down, left, right;

    internal GamePadDPad(ButtonState up, ButtonState down, ButtonState left, ButtonState right)
    {
        this.up = up;
        this.down = down;
        this.left = left;
        this.right = right;
    }

    public ButtonState Up
    {
        get { return up; }
    }

    public ButtonState Down
    {
        get { return down; }
    }

    public ButtonState Left
    {
        get { return left; }
    }

    public ButtonState Right
    {
        get { return right; }
    }
}

public struct GamePadThumbSticks
{
    public struct StickValue
    {
        float x, y;

        internal StickValue(float x, float y)
        {
            this.x = x;
            this.y = y;
        }

        public float X
        {
            get { return x; }
        }

        public float Y
        {
            get { return y; }
        }
    }

    StickValue left, right;

    internal GamePadThumbSticks(StickValue left, StickValue right)
    {
        this.left = left;
        this.right = right;
    }

    public StickValue Left
    {
        get { return left; }
    }

    public StickValue Right
    {
        get { return right; }
    }
}

public struct GamePadTriggers
{
    float left;
    float right;

    internal GamePadTriggers(float left, float right)
    {
        this.left = left;
        this.right = right;
    }

    public float Left
    {
        get { return left; }
    }

    public float Right
    {
        get { return right; }
    }
}

public struct GamePadState
{
    [System.Runtime.InteropServices.StructLayout(System.Runtime.InteropServices.LayoutKind.Sequential)]
    internal struct RawState
    {
        public uint dwPacketNumber;
        public GamePad Gamepad;

        [System.Runtime.InteropServices.StructLayout(System.Runtime.InteropServices.LayoutKind.Sequential)]
        public struct GamePad
        {
            public ushort wButtons;
            public byte bLeftTrigger;
            public byte bRightTrigger;
            public short sThumbLX;
            public short sThumbLY;
            public short sThumbRX;
            public short sThumbRY;
        }
    }

    bool isConnected;
    uint packetNumber;
    GamePadButtons buttons;
    GamePadDPad dPad;
    GamePadThumbSticks thumbSticks;
    GamePadTriggers triggers;

    enum ButtonsConstants
    {
        DPadUp = 0x00000001,
        DPadDown = 0x00000002,
        DPadLeft = 0x00000004,
        DPadRight = 0x00000008,
        Start = 0x00000010,
        Back = 0x00000020,
        LeftThumb = 0x00000040,
        RightThumb = 0x00000080,
        LeftShoulder = 0x0100,
        RightShoulder = 0x0200,
        Guide = 0x0400,
        A = 0x1000,
        B = 0x2000,
        X = 0x4000,
        Y = 0x8000
    }

    internal GamePadState(bool isConnected, RawState rawState, GamePadDeadZone deadZone)
    {
        this.isConnected = isConnected;

        if (!isConnected)
        {
            rawState.dwPacketNumber = 0;
            rawState.Gamepad.wButtons = 0;
            rawState.Gamepad.bLeftTrigger = 0;
            rawState.Gamepad.bRightTrigger = 0;
            rawState.Gamepad.sThumbLX = 0;
            rawState.Gamepad.sThumbLY = 0;
            rawState.Gamepad.sThumbRX = 0;
            rawState.Gamepad.sThumbRY = 0;
        }

        packetNumber = rawState.dwPacketNumber;
        buttons = new GamePadButtons(
            (rawState.Gamepad.wButtons & (uint)ButtonsConstants.Start) != 0 ? ButtonState.Pressed : ButtonState.Released,
            (rawState.Gamepad.wButtons & (uint)ButtonsConstants.Back) != 0 ? ButtonState.Pressed : ButtonState.Released,
            (rawState.Gamepad.wButtons & (uint)ButtonsConstants.LeftThumb) != 0 ? ButtonState.Pressed : ButtonState.Released,
            (rawState.Gamepad.wButtons & (uint)ButtonsConstants.RightThumb) != 0 ? ButtonState.Pressed : ButtonState.Released,
            (rawState.Gamepad.wButtons & (uint)ButtonsConstants.LeftShoulder) != 0 ? ButtonState.Pressed : ButtonState.Released,
            (rawState.Gamepad.wButtons & (uint)ButtonsConstants.RightShoulder) != 0 ? ButtonState.Pressed : ButtonState.Released,
            (rawState.Gamepad.wButtons & (uint)ButtonsConstants.Guide) != 0 ? ButtonState.Pressed : ButtonState.Released,
            (rawState.Gamepad.wButtons & (uint)ButtonsConstants.A) != 0 ? ButtonState.Pressed : ButtonState.Released,
            (rawState.Gamepad.wButtons & (uint)ButtonsConstants.B) != 0 ? ButtonState.Pressed : ButtonState.Released,
            (rawState.Gamepad.wButtons & (uint)ButtonsConstants.X) != 0 ? ButtonState.Pressed : ButtonState.Released,
            (rawState.Gamepad.wButtons & (uint)ButtonsConstants.Y) != 0 ? ButtonState.Pressed : ButtonState.Released
        );
        dPad = new GamePadDPad(
            (rawState.Gamepad.wButtons & (uint)ButtonsConstants.DPadUp) != 0 ? ButtonState.Pressed : ButtonState.Released,
            (rawState.Gamepad.wButtons & (uint)ButtonsConstants.DPadDown) != 0 ? ButtonState.Pressed : ButtonState.Released,
            (rawState.Gamepad.wButtons & (uint)ButtonsConstants.DPadLeft) != 0 ? ButtonState.Pressed : ButtonState.Released,
            (rawState.Gamepad.wButtons & (uint)ButtonsConstants.DPadRight) != 0 ? ButtonState.Pressed : ButtonState.Released
        );

        thumbSticks = new GamePadThumbSticks(
            Utils.ApplyLeftStickDeadZone(rawState.Gamepad.sThumbLX, rawState.Gamepad.sThumbLY, deadZone),
            Utils.ApplyRightStickDeadZone(rawState.Gamepad.sThumbRX, rawState.Gamepad.sThumbRY, deadZone)
        );
        triggers = new GamePadTriggers(
            Utils.ApplyTriggerDeadZone(rawState.Gamepad.bLeftTrigger, deadZone),
            Utils.ApplyTriggerDeadZone(rawState.Gamepad.bRightTrigger, deadZone)
        );
    }

    public uint PacketNumber
    {
        get { return packetNumber; }
    }

    public bool IsConnected
    {
        get { return isConnected; }
    }

    public GamePadButtons Buttons
    {
        get { return buttons; }
    }

    public GamePadDPad DPad
    {
        get { return dPad; }
    }

    public GamePadTriggers Triggers
    {
        get { return triggers; }
    }

    public GamePadThumbSticks ThumbSticks
    {
        get { return thumbSticks; }
    }
}

public enum PlayerIndex
{
    One = 0,
    Two,
    Three,
    Four
}

public enum GamePadDeadZone
{
    Circular,
    IndependentAxes,
    None
}

public class GamePad
{
    public static GamePadState GetState(PlayerIndex playerIndex)
    {
        return GetState(playerIndex, GamePadDeadZone.IndependentAxes);
    }

    public static GamePadState GetState(PlayerIndex playerIndex, GamePadDeadZone deadZone)
    {
        GamePadState.RawState state;
        uint result = Imports.XInputGamePadGetState((uint)playerIndex, out state);
        return new GamePadState(result == Utils.Success, state, deadZone);
    }

    public static void SetVibration(PlayerIndex playerIndex, float leftMotor, float rightMotor)
    {
        Imports.XInputGamePadSetState((uint)playerIndex, leftMotor, rightMotor);
    }
}

// Utils
static class Utils
{
    public const uint Success = 0x000;
    public const uint NotConnected = 0x000;

    private const int LeftStickDeadZone = 7849;
    private const int RightStickDeadZone = 8689;
    private const int TriggerDeadZone = 30;

    public static float ApplyTriggerDeadZone(byte value, GamePadDeadZone deadZoneMode)
    {
        if (deadZoneMode == GamePadDeadZone.None)
        {
            return ApplyDeadZone(value, byte.MaxValue, 0.0f);
        }
        else
        {
            return ApplyDeadZone(value, byte.MaxValue, TriggerDeadZone);
        }
    }

    public static GamePadThumbSticks.StickValue ApplyLeftStickDeadZone(short valueX, short valueY, GamePadDeadZone deadZoneMode)
    {
        return ApplyStickDeadZone(valueX, valueY, deadZoneMode, LeftStickDeadZone);
    }

    public static GamePadThumbSticks.StickValue ApplyRightStickDeadZone(short valueX, short valueY, GamePadDeadZone deadZoneMode)
    {
        return ApplyStickDeadZone(valueX, valueY, deadZoneMode, RightStickDeadZone);
    }

    private static GamePadThumbSticks.StickValue ApplyStickDeadZone(short valueX, short valueY, GamePadDeadZone deadZoneMode, int deadZoneSize)
    {
        if (deadZoneMode == GamePadDeadZone.Circular)
        {
            // Cast to long to avoid int overflow if valueX and valueY are both 32768, which would result in a negative number and Sqrt returns NaN
            float distanceFromCenter = (float)Math.Sqrt((long)valueX * (long)valueX + (long)valueY * (long)valueY);
            float coefficient = ApplyDeadZone(distanceFromCenter, short.MaxValue, deadZoneSize);
            coefficient = coefficient > 0.0f ? coefficient / distanceFromCenter : 0.0f;
            return new GamePadThumbSticks.StickValue(
                Clamp(valueX * coefficient),
                Clamp(valueY * coefficient)
            );
        }
        else if (deadZoneMode == GamePadDeadZone.IndependentAxes)
        {
            return new GamePadThumbSticks.StickValue(
                ApplyDeadZone(valueX, short.MaxValue, deadZoneSize),
                ApplyDeadZone(valueY, short.MaxValue, deadZoneSize)
            );
        }
        else
        {
            return new GamePadThumbSticks.StickValue(
                ApplyDeadZone(valueX, short.MaxValue, 0.0f),
                ApplyDeadZone(valueY, short.MaxValue, 0.0f)
            );
        }
    }

    private static float Clamp(float value)
    {
        return value < -1.0f ? -1.0f : (value > 1.0f ? 1.0f : value);
    }

    private static float ApplyDeadZone(float value, float maxValue, float deadZoneSize)
    {
        if (value < -deadZoneSize)
        {
            value += deadZoneSize;
        }
        else if (value > deadZoneSize)
        {
            value -= deadZoneSize;
        }
        else
        {
            return 0.0f;
        }

        value /= maxValue - deadZoneSize;

        return Clamp(value);
    }
}

It requires to place the "XInputInterface.dll" (https://github.com/speps/XInputDotNet/blob/master/XInputUnity4/XInputInterface.dll) next to the BAP executable.

Then this newly defined classes/functions can be used to control any PUP/C+ model using an XBox controller! I tried it with 42100, using the following custom program in BAP and a MS Surface tablet.

Spoiler

int trackLeft, trackRight, rotation, boom, arm, bucketTilt, bucketOpen;
int trackLeftOld = 0, trackRightOld = 0, rotationOld = 0, boomOld = 0, armOld = 0, bucketTiltOld = 0, bucketOpenOld = 0;
const int HUB_SUPERSTR = 0;
const int HUB_BASE = 1;

GamePadState state = GamePad.GetState(PlayerIndex.One);

MainBoard.WriteLine("-----------------");
if(state.IsConnected){
    MainBoard.WriteLine("Controller (player 1) connected!");
}else{
    MainBoard.WriteLine("No controller (player 1) connected! Will exit.");
    return;
}

do{
    state = GamePad.GetState(PlayerIndex.One, GamePadDeadZone.IndependentAxes);

    // ----
    trackLeft = (int) (-100*state.Triggers.Left);
    if(state.Buttons.LeftShoulder == ButtonState.Released){
        trackLeft = -trackLeft;
    }
    if(trackLeft != trackLeftOld){
        Hub[HUB_BASE].SetMotorSpeed("A", trackLeft);
        trackLeftOld = trackLeft;
    }
    // ----
    trackRight = (int) (100*state.Triggers.Right);
    if(state.Buttons.RightShoulder == ButtonState.Released){
        trackRight = -trackRight;
    }
    if(trackRight != trackRightOld){
        Hub[HUB_BASE].SetMotorSpeed("B", trackRight);
        trackRightOld = trackRight;
    }
    // ----  
    rotation = (int) (-100*state.ThumbSticks.Left.X);
    if(rotation != rotationOld){
        Hub[HUB_BASE].SetMotorSpeed("D", rotation);
        rotationOld = rotation;
    }
    // ----
    boom = (int) (100*state.ThumbSticks.Right.Y);
    if(boom != boomOld){
        Hub[HUB_SUPERSTR].SetMotorSpeed("A", boom);
        boomOld = boom;
    }
    // ----
    arm = (int) (-100*state.ThumbSticks.Left.Y);
    if(arm != armOld){
        Hub[HUB_SUPERSTR].SetMotorSpeed("B", arm);
        armOld = arm;
    }
    // ----
    bucketTilt = (int) (-100*state.ThumbSticks.Right.X);
    if(bucketTilt != bucketTiltOld){
        Hub[HUB_SUPERSTR].SetMotorSpeed("C", bucketTilt);
        bucketTiltOld = bucketTilt;
    }
    // ----
    if(state.Buttons.X == ButtonState.Pressed && state.Buttons.Y == ButtonState.Released){
        bucketOpen = 100;
    }else if(state.Buttons.Y == ButtonState.Pressed && state.Buttons.X == ButtonState.Released){
        bucketOpen = -100;
    }else{
        bucketOpen = 0;
    }
    if(bucketOpen != bucketOpenOld){
        Hub[HUB_SUPERSTR].SetMotorSpeed("D", bucketOpen);
        bucketOpenOld = bucketOpen;
    }

    Wait(20);
}while(state.IsConnected && state.Buttons.B == ButtonState.Released);

Hub[HUB_BASE].SetMotorSpeed("A", 0);
Hub[HUB_BASE].SetMotorSpeed("B", 0);
Hub[HUB_BASE].SetMotorSpeed("D", 0);
Hub[HUB_SUPERSTR].SetMotorSpeed("A", 0);
Hub[HUB_SUPERSTR].SetMotorSpeed("B", 0);
Hub[HUB_SUPERSTR].SetMotorSpeed("C", 0);
Hub[HUB_SUPERSTR].SetMotorSpeed("D", 0);

MainBoard.WriteLine("Stopped.");

It uses a button layout similar to the official app. Holding the shoulder buttons reverses the track direction, so I can use proportional control for forward and reverse. B stops all motors and the program. X and Y control the opening of the bucket. Commands are only sent, if the motor speed changes. It works like a charm, there is no noticable delay in the control.

A big thanks to the developers of the two frameworks/apps I used here!

Are you aware of any other solutions on PC, maybe capable of reading motor positions (for steering)?

Edited April 13, 20204 yr by technicfan20

Nice :)

Other libs/solutions:

Javscript: https://github.com/nathankellenicki/node-poweredup

There seems to be an extension for scratch (at least for wedo/trainhubs)

MicroPython: https://pybricks.com/

pybricks has a documention for technic motors with position reading https://docs.pybricks.com/en/latest/pupdevices.html

But I'm not sure if it's already implemented in the latest beta.

 

In my opinion, node-powereduo has quite a lot of dependencies (at least on windows), including the use of another bluetooth driver.

MicroPython looks nice. Unfortunately it seems that support for Control+ is planned, but not yet implemented. Would it require to use
an alternative firmware on the hubs, which could break the compatibility with the official app (at least until restoring the firmware)?

26 minutes ago, technicfan20 said:

MicroPython looks nice. Unfortunately it seems that support for Control+ is planned, but not yet implemented. Would it require to use
an alternative firmware on the hubs, which could break the compatibility with the official app (at least until restoring the firmware)?

Not even in the latest beta? Is there a release date?

All libs/programs should (I don't think anyone is going to write a new firmware) be based on the official Lego BT protocol https://lego.github.io/lego-ble-wireless-protocol-docs/

So you can basically write all needed commands by your self or check some other projects and copy/paste (brickcontroller2 maybe) :D

 

Edit:

https://vouzamo.wordpress.com/2020/04/06/c-sdk-for-lego-bluetooth-le-hubs/amp/

Edited April 13, 20204 yr by Gimmick