Quantcast
Channel: Sascha Wolter » C#
Viewing all articles
Browse latest Browse all 7

Hacking the Lego Mindstorms EV3

0
0

My brand-new Lego EV 3 Mindstorms arrived just a few weeks ago. Immediately I dived into unveiling its secrets. I was more than curious what I might be able to do with this robotics kit besides the official software. Unfortunately, at that time there was only little information available about the detailed specifications. The most valuable information is hidden in the Mindstorms operating system! Thanks to Lego, the source code is available at GitHub. If you are not familiar with the structure of Linux, the programming language C and alike, don´t bother. The leJOS-Team is working on a Java friendly OS and it seems some people are working on a Node.js package running JavaScript on the brick (e.g. https://github.com/clebert/ev3/issues/1). If remote controlling is suitable for you, the .Net based MonoBrick is worth a look.

lego_mdinstorms_ev3

If you like to cope with the Mindstoms internals yourself, you only need the file c_com\c_com.h to get a basic understanding of the Mindstorms capabilities. There you´ll find the structure of the used communication protocol (unfortunately, there is no official documentation on the EV3 protocol besides the source code yet). The protocol is not that hard, but a little tricky: It´s just sending byte arrays with commands and their corresponding values back and forth. So, if you want to try it yourself, it basically works like this…

First you need to connect to the Mindstorms. With Windows 8.1 (WinRT) and Bluetooth it looks like:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
public async void Connect()
{
 
    IReadOnlyList<deviceinformation> deviceInfoCollection = null;
 
    deviceInfoCollection = await DeviceInformation.FindAllAsync(
        RfcommDeviceService.GetDeviceSelector(RfcommServiceId.SerialPort)
    );
 
    DeviceInformation deviceInfo = deviceInfoCollection.FirstOrDefault(c => c.Name.Contains("EV3"));
 
    RfcommDeviceService rfcommService;
    rfcommService = await RfcommDeviceService.FromIdAsync(deviceInfo.Id);
 
    if (rfcommService == null)
    {
        // Message and open control settings for pairing bluetooth device here.
        return;
    }
 
    // Create a socket and connect to the target. 
    socket = new StreamSocket();
 
    await socket.ConnectAsync(
                rfcommService.ConnectionHostName,
                rfcommService.ConnectionServiceName,
                SocketProtectionLevel.BluetoothEncryptionAllowNullAuthentication);
 
    // Create data writer and reader to communicate with the device.
    dataWriter = new DataWriter(socket.OutputStream);
    dataReader = new DataReader(socket.InputStream);
 
}

With Windows Phone 8 you need to pair the device a little differently:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
public async void Connect()
{
 
    PeerFinder.AlternateIdentities["Bluetooth:PAIRED"] = "";
    IReadOnlyList<PeerInformation> devices = null;
 
    try
    {
        // Could fail if there is no device paired.
        devices = await PeerFinder.FindAllPeersAsync();
    }
    catch
    {
    }
 
    if (devices == null || devices.Count == 0)
    {
        MessageBox.Show("No Bluetooth devices are paired, please pair your Lego Mindstorms EV3.");
        await Windows.System.Launcher.LaunchUriAsync(new Uri("ms-settings-bluetooth:"));
        return;
    }
 
    PeerInformation peerInfo = devices.FirstOrDefault(c => c.DisplayName.Contains("EV3"));
    if (peerInfo == null)
    {
        MessageBox.Show("No paired Lego Mindstorms EV3 was found, please pair your Mindstorms.");
        await Windows.System.Launcher.LaunchUriAsync(new Uri("ms-settings-bluetooth:"));
        return;
    }
 
    // Create a socket and connect to the target. 
    socket = new StreamSocket();
    await s.ConnectAsync(peerInfo.HostName, peerInfo.ServiceName);
 
    // Create data writer and reader to communicate with the device.
    dataWriter = new DataWriter(socket.OutputStream);
    dataReader = new DataReader(socket.InputStream);
 
}

Please note, it hasn´t to be Windows and C#. You can use every environment as long as it connects to the EV3 (even Wifi is possible).

Once the connection is established, byte-arrays have to be constructed. The structure of these arrays is a little odd, but we have to deal with it. The first two bytes of the array define the array length (therefore, the array always has two more bytes than defined here):

1
2
3
4
5
6
7
8
9
10
11
12
13
14
// Command length.
int length = 13;
// Needs two more bytes for defining the length. 
byte[] data = new byte[length+2]; 
 
// Byte 0 – 1: Command size.
// Minor byte of length.
data[0]  = (byte)(length & 0x00ff);
// Major byte of length - if not longer than 254 it's zero.
data[1]  = (byte)((length & 0xff00) >> 2);
 
// Byte 2 – 3: Message counter (seems not be needed so far).
data[2] = (byte)0x00;
data[3] = (byte)0x00;

Now it gets a little bit more sophisticated, because there are different types of commands, which you can specify here: Some commands are system commands to control the more overall system functionalities, some commands are direct commands related to functionality which is controlled within the bricks virtual machine. Depending on the command and it´s parameters, the structure of the byte array slightly varies. In this explanation, I use a direct command to drive the robot:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
// Byte 4: Command type and if it´s sending a reply.
data[4] = (byte)CommandType.DIRECT_COMMAND_NO_REPLY;
 
// Byte 5 - n: Dependent on command type.
// Byte 5 - 6: Number of global (for optional response) and local variables. Variables are compressed:
// 
//   Byte 6    Byte 5
//  76543210  76543210
//  --------  --------
//  llllllgg  gggggggg
 
int globalVariablesCount = 0;
int localVariablesCount = 0;
data[5] = (byte)(globalVariablesCount & 0xFF);
data[6] = (byte)((localVariablesCount << 2) | (globalVariablesCount >> 8));
 
// Byte 7: Byte code for a command followed by its parameters.
data[7]  = (byte)Command.OutputSpeed;
 
// The first parameter describes the daisy chain layer if you use connected bricks.
data[8]  = (byte)DaisyChainLayer.EV3;
 
// The next parameter for this command describes the output channels as a bit field.
data[9]  = (byte)(OutputPort.B | OutputPort.C);
 
// And the last parameter is for the speed. Because EV3 supports different types of values, the type has to be set first followed by its value.
data[10] = (byte)ParameterFormat.LONG | (byte) LongParameterFollowFormat.ONE_BYTE;
data[11] = speed;
 
// After setting the speed, the motors have to be started now.
data[12] = (byte)Command.OutputStart;
// The first parameter again describes the daisy chain layer if you use connected bricks.
data[13] = (byte)DaisyChainLayer.EV3;
// The next parameter for this command agein describes the output channels as a bit field.
data[14] = (byte)(OutputPort.B | OutputPort.C);

The last and easiest step is to send everything to the robot:

1
2
dataWriter.WriteBytes(data);
await dataWriter.StoreAsync();

Reading a response that could include sensor data and alike works pretty much the same:

1
2
3
4
5
6
7
8
9
10
11
12
// Get the raw response and read the first two bytes
byte[] response = new byte[2];
await dataReader.LoadAsync(2);
dataReader.ReadBytes(response);
 
// Get the response length out of the first two bytes
expectedlength = (ushort)(0x0000 | response[0] | (response[1] << 2));
 
// Read the full response and the payload
byte[] payload = new byte[expectedlength];
await dataReader.LoadAsync(expectedlength);
dataReader.ReadBytes(payload);

Finally, I was somewhat proud to discover this (even if it´s not that hard). But just before I had the time to clean up the code, a team with Brian Peek (known for his awesome Coding4Fun articles) released a library with all this functionality (and even more). Therefore, I stopped my research and recommend this library, now.


Viewing all articles
Browse latest Browse all 7

Latest Images

Trending Articles





Latest Images