ENG - Prototype end-to-end robot messages over TCP.

Assets/SerialComm/Scripts/PackageAssembler.cs

@@ -0,0 +1,68 @@
+using System.IO;
+
+// The package format used in this class is described in UnityThreads.cpp.
+public class PackageAssembler
+{
+	public const int packageSize = 64;
+
+	public void AddPackage(byte[] package)
+	{
+		if(package.Length != packageSize)
+			return;
+
+		var input = new BinaryReader(new MemoryStream(package));
+
+		var magic1        = input.ReadUInt32();
+		var messageId     = input.ReadUInt32();
+		var packageNumber = input.ReadUInt32();
+		var nBytesOfData  = input.ReadByte();
+		var data          = input.ReadBytes(47);
+		var magic2        = input.ReadUInt32();
+
+		if(magic1 != 0xaaaa5555 || magic2 != 0xaa55aa55 ||
+           nBytesOfData > 47 ||
+		   (messageId == lastMessageId && lastMessageWasBad) ||
+		   (messageId == lastMessageId && packageNumber != lastPackageNumber - 1))
+		{
+			lastMessageWasBad = true;
+			return;
+		}
+
+		if(messageId != lastMessageId)
+		{
+			fullMessageReceived = false;
+			message = "";
+		}
+
+		for(int i = 0; i < nBytesOfData; i++)
+			message += (char)data[i];
+
+		if(packageNumber == 0)
+			fullMessageReceived = true;
+
+		lastMessageId     = messageId;
+		lastPackageNumber = packageNumber;
+		lastMessageWasBad = false;
+	}
+
+	public bool FullMessageWasReceived()
+	{
+		return fullMessageReceived;
+	}
+
+	public string GetMessage()
+	{
+		return message;
+	}
+
+	public bool LastMessageWasBad()
+	{
+		return lastMessageWasBad;
+	}
+
+	uint lastMessageId = uint.MaxValue;
+	bool lastMessageWasBad = true;
+	uint lastPackageNumber = 0;
+	string message;
+	bool fullMessageReceived = false;
+}

Assets/SerialComm/Scripts/RobotCommand.cs

@@ -0,0 +1,58 @@
+using System;
+using System.Collections.Generic;
+
+public class RobotCommand
+{
+	public RobotCommand(string commandDescription, string actualCommand)
+	{
+		this.actualCommand = actualCommand;
+		this.commandDescription = commandDescription;
+
+		BuildActualParameterList();
+	}
+
+	public bool Is(string commandDescription)
+	{
+		return this.commandDescription == commandDescription;
+	}
+
+	public int GetInteger(int i)
+	{
+		return Convert.ToInt32(actualParameters[i]);
+   }
+
+	public string GetWord(int i)
+	{
+		return actualParameters[i];
+	}
+
+	public string GetString(int i)
+	{
+		return actualParameters[i];
+   }
+
+	void BuildActualParameterList()
+	{
+		Tokenizer commandDescriptionTokenizer = new Tokenizer(commandDescription, ' ');
+		Tokenizer actualCommandTokenizer = new Tokenizer(actualCommand, ' ');
+
+		while(actualCommandTokenizer.HasMore())
+		{
+			var formalParameter = commandDescriptionTokenizer.GetToken();
+
+			if(formalParameter == "#i" || formalParameter == "#w")
+				actualParameters.Add(actualCommandTokenizer.GetToken());
+
+			else if(formalParameter == "#s")
+				actualParameters.Add(actualCommandTokenizer.GetString());
+
+			else
+				actualCommandTokenizer.GetToken(); // The token is a keyword and we ignore it.
+		}
+
+	}
+
+	string actualCommand;
+	string commandDescription;
+	IList<string> actualParameters = new List<string>();
+}