// Man muss mittels NuGet das Paket System.IO.Ports einbinden. Es gibt auch ein Lego Mindstorms Paket, funktioniert nicht zuverlässig.
// Der ausgehende Port für das betroffene Brick ist COM9 (das muss angepasst werden).
// Zwei große Motoren sind an Ports A und D des Bricks angeschlossen.
using System.IO.Ports;
using System.Text;
const string PortName = "COM9";
Console.WriteLine("EV3C Hardware-Test");
Console.WriteLine("==================");
using var ev3 = new Ev3Brick(PortName);
try
{
ev3.Connect();
Console.WriteLine();
Console.WriteLine("SYSTEMINFO EV3C");
Console.WriteLine("----------------");
Ev3SystemInfo info = ev3.ReadSystemInfo();
Console.WriteLine($"Hardware-Version : {info.HardwareVersion}");
Console.WriteLine($"Firmware-Version : {info.FirmwareVersion}");
Console.WriteLine($"Firmware-Build : {info.FirmwareBuild}");
Console.WriteLine($"OS-Version : {info.OsVersion}");
Console.WriteLine($"OS-Build : {info.OsBuild}");
Console.WriteLine($"Batteriespannung : {info.BatteryVoltage:F2} V");
Console.WriteLine();
Console.WriteLine("Motoren bitte an A und D anschließen.");
Console.WriteLine("ENTER startet den Motortest.");
Console.ReadLine();
const int speed = 20;
const int durationMs = 800;
Console.WriteLine();
Console.WriteLine("Motor A vorwärts ...");
ev3.SetMotorSpeed(Ev3Output.A, speed);
Thread.Sleep(durationMs);
ev3.StopMotor(Ev3Output.A);
Thread.Sleep(500);
Console.WriteLine("Motor A rückwärts ...");
ev3.SetMotorSpeed(Ev3Output.A, -speed);
Thread.Sleep(durationMs);
ev3.StopMotor(Ev3Output.A);
Thread.Sleep(800);
Console.WriteLine();
Console.WriteLine("Motor D vorwärts ...");
ev3.SetMotorSpeed(Ev3Output.D, speed);
Thread.Sleep(durationMs);
ev3.StopMotor(Ev3Output.D);
Thread.Sleep(500);
Console.WriteLine("Motor D rückwärts ...");
ev3.SetMotorSpeed(Ev3Output.D, -speed);
Thread.Sleep(durationMs);
ev3.StopMotor(Ev3Output.D);
Console.WriteLine();
Console.WriteLine("Test beendet.");
}
catch (Exception ex)
{
Console.WriteLine();
Console.WriteLine("FEHLER:");
Console.WriteLine(ex);
}
finally
{
try
{
ev3.StopAllMotors();
}
catch
{
// Beim Beenden nichts mehr erzwingen.
}
}
// ================================================== ==============
// EV3
// ================================================== ==============
public enum Ev3Output : byte
{
A = 0x01,
B = 0x02,
C = 0x04,
D = 0x08
}
public sealed record Ev3SystemInfo(
string HardwareVersion,
string FirmwareVersion,
string FirmwareBuild,
string OsVersion,
string OsBuild,
double BatteryVoltage);
public sealed class Ev3Brick : IDisposable
{
private readonly SerialPort port;
private ushort messageCounter = 1;
public Ev3Brick(string portName)
{
port = new SerialPort(
portName,
115200,
Parity.None,
8,
StopBits.One);
port.Handshake = Handshake.None;
port.ReadTimeout = 15000;
port.WriteTimeout = 3000;
}
public void Connect()
{
if (port.IsOpen)
return;
Console.WriteLine($"Öffne {port.PortName} ...");
port.Open();
Console.WriteLine("Bluetooth-Verbindung geöffnet.");
}
// ================================================== ==========
// SYSTEMINFO
// ================================================== ==========
public Ev3SystemInfo ReadSystemInfo()
{
double batteryVoltage =
ReadUiFloat(0x01);
string osVersion =
ReadUiString(0x03, 31);
string hardwareVersion =
ReadUiString(0x09,
;
string firmwareVersion =
ReadUiString(0x0A,
;
string firmwareBuild =
ReadUiString(0x0B, 12);
string osBuild =
ReadUiString(0x0C, 12);
return new Ev3SystemInfo(
hardwareVersion,
firmwareVersion,
firmwareBuild,
osVersion,
osBuild,
batteryVoltage);
}
private double ReadUiFloat(byte subCode)
{
byte[] command =
{
0x81, // opUI_Read
subCode,
0x60 // GV0
};
byte[] data =
SendDirectCommand(
command,
globalMemoryBytes: 4);
return BitConverter.ToSingle(data, 0);
}
private string ReadUiString(
byte subCode,
int maxLength)
{
List<byte> command = new();
command.Add(0x81); // opUI_Read
command.Add(subCode);
command.AddRange(
LCX(maxLength));
command.Add(0x60); // GV0
byte[] data =
SendDirectCommand(
command.ToArray(),
globalMemoryBytes: maxLength);
int zero =
Array.IndexOf(data, (byte)0);
int length =
zero >= 0
? zero
: data.Length;
return Encoding.UTF8
.GetString(data, 0, length)
.Trim();
}
// ================================================== ==========
// MOTOREN
// ================================================== ==========
public void SetMotorSpeed(
Ev3Output output,
int speed)
{
speed =
Math.Clamp(speed, -100, 100);
if (speed == 0)
{
StopMotor(output);
return;
}
List<byte> command = new();
// opOutputSpeed
command.Add(0xA5);
command.AddRange(LCX(0)); // Layer
command.AddRange(LCX((int)output));
command.AddRange(LCX(speed));
// opOutputStart
command.Add(0xA6);
command.AddRange(LCX(0));
command.AddRange(LCX((int)output));
SendDirectCommand(
command.ToArray());
}
public void StopMotor(
Ev3Output output)
{
List<byte> command = new();
command.Add(0xA3); // opOutputStop
command.AddRange(LCX(0)); // Layer
command.AddRange(LCX((int)output));
command.AddRange(LCX(1)); // Brake = true
SendDirectCommand(
command.ToArray());
}
public void StopAllMotors()
{
if (!port.IsOpen)
return;
const int allOutputs = 0x0F;
List<byte> command = new();
command.Add(0xA3); // opOutputStop
command.AddRange(LCX(0));
command.AddRange(LCX(allOutputs));
command.AddRange(LCX(1));
SendDirectCommand(
command.ToArray());
}
// ================================================== ==========
// EV3 DIRECT COMMAND
// ================================================== ==========
private byte[] SendDirectCommand(
byte[] opcodes,
int globalMemoryBytes = 0)
{
if (!port.IsOpen)
throw new InvalidOperationException(
"EV3 ist nicht verbunden.");
ushort counter =
messageCounter++;
int bodyLength =
5 + opcodes.Length;
byte[] packet =
new byte[bodyLength + 2];
packet[0] =
(byte)(bodyLength & 0xFF);
packet[1] =
(byte)(bodyLength >>
;
packet[2] =
(byte)(counter & 0xFF);
packet[3] =
(byte)(counter >>
;
packet[4] = 0x00;
// Direct Command, Reply required
packet[5] =
(byte)(globalMemoryBytes & 0xFF);
packet[6] =
(byte)((globalMemoryBytes >>
& 0x03);
Array.Copy(
opcodes,
0,
packet,
7,
opcodes.Length);
port.Write(
packet,
0,
packet.Length);
byte[] lengthBytes =
ReadExactly(2);
int replyLength =
lengthBytes[0] |
(lengthBytes[1] <<
;
byte[] reply =
ReadExactly(replyLength);
if (reply.Length < 3)
throw new IOException(
"EV3-Antwort zu kurz.");
ushort replyCounter =
(ushort)(
reply[0] |
(reply[1] <<
);
if (replyCounter != counter)
{
throw new IOException(
$"Falscher Message Counter: " +
$"{replyCounter} statt {counter}");
}
if (reply[2] != 0x02)
{
throw new IOException(
$"EV3 Direct Command Fehler: " +
$"0x{reply[2]:X2}");
}
byte[] payload =
new byte[globalMemoryBytes];
if (globalMemoryBytes > 0)
{
Array.Copy(
reply,
3,
payload,
0,
globalMemoryBytes);
}
return payload;
}
private byte[] ReadExactly(int count)
{
byte[] buffer =
new byte[count];
int offset = 0;
while (offset < count)
{
int n =
port.Read(
buffer,
offset,
count - offset);
if (n <= 0)
{
throw new IOException(
"Bluetooth-Verbindung unterbrochen.");
}
offset += n;
}
return buffer;
}
// EV3 LCX Parameter-Encoding
private static byte[] LCX(int value)
{
if (value >= -32 &&
value <= 31)
{
return new[]
{
(byte)(value & 0x3F)
};
}
if (value >= -128 &&
value <= 127)
{
return new[]
{
(byte)0x81,
unchecked((byte)value)
};
}
if (value >= short.MinValue &&
value <= short.MaxValue)
{
short v =
(short)value;
return new[]
{
(byte)0x82,
(byte)(v & 0xFF),
(byte)((v >>
& 0xFF)
};
}
return new[]
{
(byte)0x83,
(byte)(value & 0xFF),
(byte)((value >>
& 0xFF),
(byte)((value >> 16) & 0xFF),
(byte)((value >> 24) & 0xFF)
};
}
public void Dispose()
{
if (port.IsOpen)
{
try
{
StopAllMotors();
}
catch
{
}
port.Close();
}
port.Dispose();
}
}
Lesezeichen