public void send <T>(GloObject <T> obj) where T : struct
{
if (!connectionOpen())
{
return;
}
byte[] dataBytes = GloLink.StructureToByteArray(ref obj.data);
transferBuffer[0] = Convert.ToByte(messageFrame[0]);
transferBuffer[1] = Convert.ToByte(messageFrame[1]);
transferBuffer[2] = Convert.ToByte(messageFrame[2]);
transferBuffer[3] = obj.ID;
transferBuffer[4] = Convert.ToByte((obj.instance << 8) >> 8);
transferBuffer[5] = Convert.ToByte(obj.instance >> 8);
transferBuffer[6] = Convert.ToByte(dataBytes.Length);
transferBuffer[dataBytes.Length + 7] = 0x0D;
transferBuffer[dataBytes.Length + 8] = 0x0A; //CR LF
dataBytes.CopyTo(transferBuffer, 7);
// how much space is actually used up for current packet
int packetLength = dataBytes.Length + 9;
connection.Write(transferBuffer, packetLength);
NumberPacketsSent++;
NumberBytesSent += packetLength;
}
public void sendMovementCommand(MovementCommand command)
{
// Create new command packet to send.
GloObject <GloDrivingCommand> gloCommand = new GloObject <GloDrivingCommand>(Glo.IDs.DrivingCommand);
gloCommand.data.movementType = (UInt32)command;
link.send(gloCommand);
}
private void sendGloObject <T>(ref T gloData, Glo.IDs ID) where T : struct
{
GloObject <T> gloObject = new GloObject <T>(ID)
{
data = gloData
};
try
{
link.send <T>(gloObject);
}
catch (TimeoutException)
{
logAndDisplayMessage("Port write timed out", MessageSource.UI);
}
}
void link_PacketReceived(object sender, PacketReceivedEventArgs e)
{
switch (e.ID)
{
case (byte)Glo.IDs.AssertMessage:
GloObject <GloAssertMessage> assertMessage = new GloObject <GloAssertMessage>(Glo.IDs.AssertMessage);
assertMessage.data = GloLink.ByteArrayToStructure <GloAssertMessage>(e.Data);
logAndDisplayMessage(assertMessage.data.text, MessageSource.RobotAssert);
break;
case (byte)Glo.IDs.DebugMessage:
GloObject <GloDebugMessage> debugMessage = new GloObject <GloDebugMessage>(Glo.IDs.DebugMessage);
debugMessage.data = GloLink.ByteArrayToStructure <GloDebugMessage>(e.Data);
logAndDisplayMessage(debugMessage.data.text, MessageSource.RobotDebug);
break;
case (byte)Glo.IDs.CaptureData:
if (capturingData)
{
GloObject <GloCaptureData> testData = new GloObject <GloCaptureData>(Glo.IDs.CaptureData);
testData.data = GloLink.ByteArrayToStructure <GloCaptureData>(e.Data);
captureData.Add(new float[] { (float)testData.data.time, (float)(testData.data.tiltAngle * 180.0 / Math.PI) });
}
break;
case (byte)Glo.IDs.StatusData:
GloObject <GloStatusData> statusData = new GloObject <GloStatusData>(Glo.IDs.StatusData);
statusData.data = GloLink.ByteArrayToStructure <GloStatusData>(e.Data);
view.TiltAngle = statusData.data.tiltAngle * 180.0 / Math.PI;
break;
default:
logAndDisplayMessage("Received unhandled packet with ID: " + e.ID, MessageSource.UI);
break;
}
}
