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; |
} |
|
} |
} |
} |
} |