Subversion Repositories Projects

Compare Revisions

Regard whitespace Rev 89 → Rev 90

/Microsoft Robotics Studio/MSRS Service/RoboBoardService/SerialCom.cs
0,0 → 1,403
using System;
using System.Collections.Generic;
using System.Text;
using System.IO.Ports;
using System.Threading;
 
namespace Robotics.Serial
{
class SerialCom
{
public delegate void BotEventHandler(object sender, EventArgs ev);
public static event BotEventHandler BotEvent;
public string ComPort = "COM1";
public static SerialPort serialPort1;// = new SerialPort();
public bool isOpen = false;
 
// ---------------------------------------------------------------------------
/// <summary>
/// Decode Message from the Roboboard
/// </summary>
/// <param name="message"></param>
/// <returns></returns>
// ---------------------------------------------------------------------------
public static string DecodeMessage(string command)
{
string message = command.Substring(0, command.Length - 2);
string target = "";
target += (char)command[command.Length - 2];
target += (char)command[command.Length - 1];
//-----------------------------------------------------------
// check CRC
Int16 tmpcrc = 0;
for (int i = 0; i < message.Length; i++)
{
tmpcrc += (Int16)message[i];
}
tmpcrc %= 4096;
 
string crc = "";
crc += (char)((byte)'=' + (tmpcrc / 64));
crc += (char)((byte)'=' + (tmpcrc % 64));
 
if (crc != target) return "";
//-----------------------------------------------------------
// decode message
target = "";
message = command.Substring(3, command.Length - 5);
 
int index = 0;
uint a, b, c, d;
uint x, y, z;
int len = message.Length;
message = message + "=====";
 
while (index <= len)
{
a = (uint)message[index++] - '=';
b = (uint)message[index++] - '=';
c = (uint)message[index++] - '=';
d = (uint)message[index++] - '=';
 
x = (a << 2) | (b >> 4);
y = ((b & 0x0f) << 4) | (c >> 2);
z = ((c & 0x03) << 6) | d;
 
target += (char)x;
target += (char)y;
target += (char)z;
}
return target;
}
 
// ---------------------------------------------------------------------------
/// <summary>
/// Encode Message to send to the Board
/// </summary>
/// <param name="message"></param>
/// <returns></returns>
// ---------------------------------------------------------------------------
public static string EncodeMessage(string message)
{
string target = "";
byte a, b, c;
int len = message.Length;
int ptr = 0;
 
while (len > 0)
{
if (len > 0) { a = (byte)message[ptr++]; len--; } else a = 0;
if (len > 0) { b = (byte)message[ptr++]; len--; } else b = 0;
if (len > 0) { c = (byte)message[ptr++]; len--; } else c = 0;
target += (char)((a >> 2) + (byte)'=');
target += (char)((((a & 0x03) << 4) | ((b & 0xf0) >> 4)) + (byte)'=');
target += (char)((((b & 0x0f) << 2) | ((c & 0xc0) >> 6)) + (byte)'=');
target += (char)((c & 0x3f) + (byte)'=');
}
return target;
}
 
// ---------------------------------------------------------------------------
/// <summary>
/// Creates a complete message including the command and the address byte
/// header. It calls thr EncodeMessage function to encode the message body.
/// command and adress byte will not be encoded. An additional 2 Byte CRC will
/// be added at the end of the message
/// </summary>
/// <param name="cmd"></param>
/// <param name="adr"></param>
/// <param name="message"></param>
/// <returns></returns>
// ---------------------------------------------------------------------------
public string CreateMessage(char cmd, char adr, string message)
{
string target = "";
target += '#';
target += adr;
target += cmd;
target += EncodeMessage(message);
 
//-----------------------------------------------------------
//add CRC
Int16 tmpcrc = 0;
for (int i = 0; i < target.Length; i++)
{
tmpcrc += (Int16)target[i];
}
tmpcrc %= 4096;
target += (char)((byte)'=' + (tmpcrc / 64));
target += (char)((byte)'=' + (tmpcrc % 64));
//-----------------------------------------------------------
target += (char)'\r';
return target;
}
 
// ---------------------------------------------------------------------------
/// <summary>
/// Initialize the COM Port. ComPort must be set before calling this function
/// </summary>
/// <returns></returns>
// ---------------------------------------------------------------------------
public int InitPort()
{
serialPort1 = new SerialPort();
serialPort1.DataReceived += new System.IO.Ports.SerialDataReceivedEventHandler(serialPort1_DataReceived);
try
{
serialPort1.PortName = ComPort;
serialPort1.BaudRate = 57600;
serialPort1.DataBits = 8;
serialPort1.Open();
serialPort1.DiscardInBuffer();
serialPort1.DiscardOutBuffer();
isOpen = true;
}
catch
{
isOpen = false;
return 1;
}
return 0;
}
 
// ---------------------------------------------------------------------------
/// <summary>
/// Writes a string to the Com Port
/// </summary>
/// <param name="data"></param>
// ---------------------------------------------------------------------------
public void SerialWrite(string data)
{
serialPort1.Write(data);
}
 
// ---------------------------------------------------------------------------
/// <summary>
/// Writes a string to the comport and adds a CR/LF at the end
/// </summary>
/// <param name="data"></param>
// ---------------------------------------------------------------------------
public void SerialWriteLine(string data)
{
serialPort1.WriteLine(data);
}
 
// ---------------------------------------------------------------------------
/// <summary>
/// colse the COm port if open
/// </summary>
// ---------------------------------------------------------------------------
public void ClosePort()
{
if (isOpen)
{
serialPort1.Close();
}
}
 
// ---------------------------------------------------------------------------
/// <summary>
/// Message functions
/// </summary>
// ---------------------------------------------------------------------------
public string GetLastMessage()
{
if (messagevalid == true)
{
return botmessage;
}
else return "";
}
 
public bool IsMessageValid()
{
return messagevalid;
}
 
public bool IsMessageError()
{
return messageerror;
}
 
public bool IsMessageOverrun()
{
return messageoverrun;
}
 
public void InvalidateMessage()
{
messagevalid = false;
messageerror = false;
messageoverrun = false;
}
 
public char GetMessageCmd()
{
return message_cmd;
}
 
public char GetMessageAdr()
{
return message_adr;
}
public string MessageAddByte(byte value, string command)
{
command = command + (char)value;
return command;
}
 
public string MessageAddInt16(Int16 value, string command)
{
byte s = 0;
s = (byte)value;
command = command + (char)s;
Int16 value1 = (Int16)(value >> 8);
s = (byte)value1;
command = command + (char)s;
return command;
}
 
public string MessageAddInt32(Int32 value, string command)
{
byte s = 0;
s = (byte)value;
command = command + (char)s;
Int32 value1 = (Int32)(value >> 8);
s = (byte)value1;
command = command + (char)s;
value1 = (Int32)(value >> 8);
s = (byte)value1;
command = command + (char)s;
value1 = (Int32)(value >> 8);
s = (byte)value1;
command = command + (char)s;
return command;
}
 
public byte MessageReadByte(string command)
{
byte bvalue = (byte)command[0];
return bvalue;
}
 
public Int16 MessageReadInt16(string command)
{
int index = 0;
Int16 tmp2;
Int16 result;
 
Int16 tmp;
tmp2 = (Int16)command[index++];
result = (Int16)command[index++];
result = (Int16)(result << 8);
result += (Int16)tmp2;
return result;
}
 
public Int32 MessageReadInt32(string command)
{
int index = 0;
Int32 tmp0;
Int32 tmp1;
Int32 tmp2;
Int32 result;
 
Int16 tmp;
tmp0 = (Int32)command[index++];
tmp1 = (Int32)command[index++];
tmp2 = (Int32)command[index++];
result = (Int32)command[index++];
result = (Int32)(result << 8);
result += (Int32)tmp2;
result = (Int32)(result << 8);
result += (Int32)tmp1;
result = (Int32)(result << 8);
result += (Int32)tmp0;
return result;
}
 
 
// ---------------------------------------------------------------------------
/// <summary>
/// serial data receive message handler.
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
// ---------------------------------------------------------------------------
private static string command;
private static bool messagevalid = false;
private static bool messageerror = false;
private static bool messageoverrun = false;
private static char message_cmd = (char)0;
private static char message_adr = (char)0;
private static int receiver_status = 0;
private static string botmessage;
 
private static void serialPort1_DataReceived(object sender, System.IO.Ports.SerialDataReceivedEventArgs e)
{
int data;
EventArgs ev = new EventArgs();
while (serialPort1.BytesToRead > 0)
{
data = serialPort1.ReadChar();
//Console.Write((char)data);
switch (receiver_status)
{
case 0:
if (data.CompareTo('#') == 0)
{
command = "#";
receiver_status++;
}
break;
case 1:
message_adr = (char)data;
command = command + (char)data;
receiver_status++;
break;
case 2:
message_cmd = (char)data;
command = command + (char)data;
receiver_status++;
break;
case 3:
if ((data.CompareTo(13) != 0) && (data.CompareTo(10) != 0))
{
command = command + (char)data;
if (command.Length > 350)
{
command = "";
serialPort1.DiscardInBuffer();
}
}
else
{
if (messagevalid == true)
{
messageoverrun = true;
}
else
{
botmessage = DecodeMessage(command);
if (botmessage == "")
{
messageerror = true;
}
else
{
messagevalid = true;
}
}
BotEvent(sender, ev);
receiver_status = 0;
command = "";
}
break;
}
}
}
}
}