Unity之串列埠通訊（基於三姿態感測器）
阿新 • • 發佈：2019-01-29
/*******************************/
using UnityEngine;
using System.Collections;
//Other libraries
using System;
using System.Threading;
using System.Collections.Generic;
using System.ComponentModel;
//串列埠名稱空間
using System.IO.Ports;
using System.Text.RegularExpressions;
using System.Text;
public class ComTest : MonoBehaviour {
/******定義一個串列埠欄位*******/
private SerialPort sp;
/*******定義三個外部介面***************/
public static float FangXiangJiao;
public static float YangJiao;
public static float XuanZhuanJiao;
/*******定義三個角度儲存的臨時欄位***********/
public static float _FangXiangJiao;
public static float _YangJiao;
public static float _XuanZhuanJiao;
/***********三種角度的迴圈佇列****************/
private Queue<float> AVEFangXiangJiao = new Queue<float>();
private Queue<float> AVEYangJiao = new Queue<float>();
private Queue<float> AVEXuanZhuanJiao = new Queue<float>();
private byte[] DataBuf;
private bool flag = true;
private bool showMessage = false;//顯示訊息
private bool open = false;
private int NumberCount=0;
private Thread SerialPortThread;
private StringBuilder sbReadline;
private string strOutPool = string.Empty;
private Queue<string> queueDataPool;
private Thread tPort;
private Thread tPortDeal;
/***********Unity初始化方法*****************/
private void Start () {
sbReadline = new StringBuilder();
queueDataPool = new Queue<string>();
//DataBuf = new byte[10];
sp = new SerialPort("COM1", 9600, Parity.None, 8, StopBits.One);
if (!sp.IsOpen){
// 開啟串列埠
sp.Open();
open = true;
}
//開啟2個執行緒分別用於接收資料和處理資料
tPort = new Thread(DealData);
tPort.Start();
tPortDeal = new Thread(ReceiveData);
tPortDeal.Start();
//StartCoroutine(DealData());
}
/***************程式結束的時候，關閉串列埠**********************/
void OnApplicationQuit() {
sp.Close();
//SerialPortThread.Abort();//終止程序
}
/*************Update方法********************/
//一幀之呼叫多次
void FixedUpdate() {
//定期回收垃圾
if (Time.frameCount % 120 == 0) System.GC.Collect();
//處理資料的執行緒停止，那麼重啟該執行緒
if (!tPort.IsAlive) {
tPort = new Thread(DealData);
tPort.Start();
}
if (!tPortDeal.IsAlive) {
tPortDeal = new Thread(ReceiveData);
tPortDeal.Start();
}
// StartCoroutine(DealData());
//Debug.Log( SerialPortThread.ThreadState);
}
/// <summary>
/// 資料接收 程序, 使用佇列來儲存資料,避免陣列的上鎖和解鎖問題
/// 設計模式 多執行緒+資料池
/// </summary>
/// <returns>
/// The data.
/// </returns>
/*****************資料接收*************************/
private void ReceiveData () {
//讀取資料，同時存入資料池中
//newline結束標記,同時使用readIlne()方法
try {
//從輸入快取區中去讀取第一次出現"AA"時的內容
Byte[] buf=new Byte[120];
if (open) sp.Read(buf,0,120);
//如果沒有接到到資料,就返回
if (buf.Length ==0) {
return;
}
string sbReadline2str = string.Empty;
if (buf != null)
{
for (int i = 0; i < buf.Length; i++)
{
sbReadline2str += buf[i].ToString("X2");
}
}
//sbReadline2str = System.Text.Encoding.Default.GetString(buf, 0,buf.Length);
// Debug.LogError(sbReadline2str.ToString());
// sbReadline2str = buf.ToString();
// sbReadline2str = sbReadline.ToString();
//提取完整的一個數據包，壓入資料池中(佇列中)
if (sbReadline2str.StartsWith("DB90"))
{ //分組資料包
string[] _str = Regex.Split(sbReadline2str, "55AA", RegexOptions.IgnoreCase);
foreach (string s in _str)
{
if (s.Length == 16)
{
//資料進入佇列
queueDataPool.Enqueue(s + "55AA");
//Debug.LogError(s+"55AA");
}
}
}
else {
sbReadline2str.Remove(0,sbReadline2str.IndexOf("DB90")+1);
string[] _str = Regex.Split(sbReadline2str, "55AA", RegexOptions.IgnoreCase);
foreach (string s in _str)
{
if (s.Length == 16)
{
//資料進入佇列
queueDataPool.Enqueue(s + "55AA");
// Debug.LogError(s+"55AA");
}
// Convert.ToByte(s,16);
}
}
}catch (Exception ex) {
Debug.LogError(ex);
}
//yield return null;
}
/***************處理出隊的資料***********************/
private void DealData()
{
while (queueDataPool.Count != 0)
{//迴圈檢測佇列
//佇列中有資料
//處理出隊的資料
//迴圈佇列，首先載入第一包資料，只執行一次
if (flag)
{ //初始化第一組資料
for (int i = 0; i < 7; i++)
{ //出佇列，同時移除這個資料
strOutPool = queueDataPool.Dequeue();
float[] jiaodu = DealReceivedDatanow(strOutPool);
AVEFangXiangJiao.Enqueue(jiaodu[0]);
AVEYangJiao.Enqueue(jiaodu[1]);
AVEXuanZhuanJiao.Enqueue(jiaodu[2]);
}
FangXiangJiao = Caulation(AVEFangXiangJiao);
YangJiao = Caulation(AVEYangJiao);
XuanZhuanJiao = Caulation(AVEXuanZhuanJiao);
flag = false;
}
else
{/*採用7點平滑濾波，消除抖動現象.注意特殊點的處理360到0之間的資料*/
//沒有存在跳變的現象，那就直接儲存資料
for (int i = 0; i < 7; i++)
{
//出佇列，同時移除這個資料。
//載入7包資料，
strOutPool = queueDataPool.Dequeue();
//組合載入的資料，低位元組在前，高位元組在後，return 三個角度值
float[] jiaodu = DealReceivedDatanow(strOutPool);
//角度迴圈佇列更新最新進入的資料
AVEFangXiangJiao.Dequeue();
//角度迴圈佇列在隊尾插入新的資料
AVEFangXiangJiao.Enqueue(jiaodu[0]);
//計算更新後的迴圈佇列中的7個數據平均值
/*****************處理特殊資料************************/
int[] IntPanDuan = CheackDataTuBian(AVEFangXiangJiao);
//如果存在著跳變的現象
if (IntPanDuan[0] == 1)
{//保留佇列前面的資料，移除後面的資料
float[] FXj = AVEFangXiangJiao.ToArray();
float sum = 0f;
//獲取要移除資料的索引值
int indexOfRemovingData = IntPanDuan[2];
for (int j = 0; j < indexOfRemovingData; j++)
{
sum += FXj[j];
}
//計算平均值
FangXiangJiao = sum / indexOfRemovingData;
}
FangXiangJiao = Caulation(AVEFangXiangJiao);
/******************處理特殊資料結束*************************/
/*******************************************/
/*同上*/
AVEYangJiao.Dequeue();
AVEYangJiao.Enqueue(jiaodu[1]);
YangJiao = Caulation(AVEYangJiao);
/*同上*/
AVEXuanZhuanJiao.Dequeue();
AVEXuanZhuanJiao.Enqueue(jiaodu[2]);
XuanZhuanJiao = Caulation(AVEXuanZhuanJiao);
/******************************************/
}
}
}
}
/************************檢驗特殊值***************************/
private int[] CheackDataTuBian(Queue<float> cheackQueue)
{
float[] cheackDataArrary = cheackQueue.ToArray();
//flag =0表示false; flag=1表示true
int flag = 0;
int[] BoolAndIndexOfNext=new int[2];
for (int count = 0; count < 6; count++ )
{float Previous =cheackDataArrary[count];
float Next = cheackDataArrary[count+1];
if (Mathf.Abs(Previous - Next)>350.0F && Mathf.Abs(Previous -Next)<360.0f) {
flag = 1;
BoolAndIndexOfNext[0] = flag;
BoolAndIndexOfNext[1] = count + 1; break;
}
}
return BoolAndIndexOfNext;
}
/*****************計算平均值****************************/
private float Caulation(Queue<float> AVEf) {
float _f=0.0f;
foreach (float f in AVEf) {
_f+=f;
}
return _f/7;
}
/*****************處理來自資料池的資料**************************/
private float[] DealReceivedDatanow (string DataBuf) {
//this is a whole frame data package,then convert the
//First convert the Byte type into Char type
// Debug.Log("--starting");
//Debug.Log("databuf----"+DataBuf);
/*把16進位制字串轉換成位元組陣列*/
byte[] returnBytes = new byte[DataBuf.Length / 2];
for (int i = 0; i < returnBytes.Length; i++)
returnBytes[i] = Convert.ToByte(DataBuf.Substring(i * 2, 2), 16);
/*消除抖動，因為眼鏡感測器會不斷的傳送資料，即使是不動的狀態下，也會有這樣的現象*/
float[] jiaoduCollection =new float[3];
_FangXiangJiao = (float)((byte)returnBytes[2] + ((sbyte)returnBytes[3] << 8)) / 10;
jiaoduCollection[0] =_FangXiangJiao;
// Debug.Log("fx-->"+FangXiangJiao);
_YangJiao = (float)((byte)returnBytes[4] + ((sbyte)returnBytes[5] << 8)) / 10;
jiaoduCollection[1] =_YangJiao;
// Debug.Log("yj-->"+YangJiao);
_XuanZhuanJiao = -(float)((byte)returnBytes[6] + ((sbyte)returnBytes[7] << 8)) / 10;
jiaoduCollection[2]=_XuanZhuanJiao;
//Debug.Log("xz-->"+XuanZhuanJiao);
//Debug.Log("--ending");
return jiaoduCollection;
}
/********************處理來自資料池的資料**************************/
private void DealReceivedData (string dataOutPool)
{
//讀取串列埠的資料, 如果沒有反應就會出現timeout 異常
// 資料格式：DB 90 AF 0C A2 FF 2C 00 55 AA
//包頭是DB 90 包尾：55 AA
//一幀傳送一個數據包 總大小10個位元組，
//int DataNumber;
try {
//讀取資料
byte tempB;
tempB= (byte)sp.ReadByte();
//Time.time;
//Read the header first
if (tempB ==219)
{
//Debug.Log("i get the 0XDB"+(byte)0xDB);
DataBuf[0]=tempB;
tempB= (byte)sp.ReadByte();
if (tempB ==144)
DataBuf[1]=tempB;
int DataNumber=2;
while(DataNumber<10){
tempB= (byte) sp.ReadByte();
DataBuf[DataNumber]=tempB;
DataNumber++;
}
}
//read out the input data
//cheack the header and tail for a data package.
//this is a whole frame data package,then convert the
//First convert the Byte type into Char type
// Debug.Log("--starting");
FangXiangJiao=(float)(DataBuf[2]+((sbyte)DataBuf[3]<<8))/10;
/*while(NumberCount <7) {
//_FangXiangJiao =
}*/
// Debug.Log("fx-->"+FangXiangJiao);
YangJiao = (float)(DataBuf[4]+((sbyte)DataBuf[5]<<8))/10;
// Debug.Log("yj-->"+YangJiao);
XuanZhuanJiao = (float)(DataBuf[6]+((sbyte)DataBuf[7]<<8))/10;
// Debug.Log("xz-->"+XuanZhuanJiao);
//Debug.Log("--ending");
}
catch (Exception e) {
Debug.LogError(e);
}
//Debug.Log("starting");
foreach(byte b in DataBuf) {
//Debug.LogError(Convert.ToString(b,16));
}
//Debug.Log("ending");
//yield return null;
}
/*********************************************/
//在沒有開啟串列埠裝置電源的時候，顯示訊息視窗，開啟電源之後，點選確定按鈕
private void OnGUI() {
if (showMessage)
{
GUI.BeginGroup(new Rect(Screen.width/2-100,Screen.height-60,400,400));
GUI.Label(new Rect(Screen.width / 2 - 80, Screen.height - 60,200,150), "請開啟串列埠裝置電源！然後點選確定");
if (GUI.Button(new Rect(Screen.width / 2 -80, Screen.height +95 , 100, 100), "確定"))
{
open = true;
}
GUI.EndGroup();
}
}
using UnityEngine;
using System.Collections;
//Other libraries
using System;
using System.Threading;
using System.Collections.Generic;
using System.ComponentModel;
//串列埠名稱空間
using System.IO.Ports;
using System.Text.RegularExpressions;
using System.Text;
public class ComTest : MonoBehaviour {
/******定義一個串列埠欄位*******/
private SerialPort sp;
/*******定義三個外部介面***************/
public static float FangXiangJiao;
public static float YangJiao;
public static float XuanZhuanJiao;
/*******定義三個角度儲存的臨時欄位***********/
public static float _FangXiangJiao;
public static float _YangJiao;
public static float _XuanZhuanJiao;
/***********三種角度的迴圈佇列****************/
private Queue<float> AVEFangXiangJiao = new Queue<float>();
private Queue<float> AVEYangJiao = new Queue<float>();
private Queue<float> AVEXuanZhuanJiao = new Queue<float>();
private byte[] DataBuf;
private bool flag = true;
private bool showMessage = false;//顯示訊息
private bool open = false;
private int NumberCount=0;
private Thread SerialPortThread;
private StringBuilder sbReadline;
private string strOutPool = string.Empty;
private Queue<string> queueDataPool;
private Thread tPort;
private Thread tPortDeal;
/***********Unity初始化方法*****************/
private void Start () {
sbReadline = new StringBuilder();
queueDataPool = new Queue<string>();
//DataBuf = new byte[10];
sp = new SerialPort("COM1", 9600, Parity.None, 8, StopBits.One);
if (!sp.IsOpen){
// 開啟串列埠
sp.Open();
open = true;
}
//開啟2個執行緒分別用於接收資料和處理資料
tPort = new Thread(DealData);
tPort.Start();
tPortDeal = new Thread(ReceiveData);
tPortDeal.Start();
//StartCoroutine(DealData());
}
/***************程式結束的時候，關閉串列埠**********************/
void OnApplicationQuit() {
sp.Close();
//SerialPortThread.Abort();//終止程序
}
/*************Update方法********************/
//一幀之呼叫多次
void FixedUpdate() {
//定期回收垃圾
if (Time.frameCount % 120 == 0) System.GC.Collect();
//處理資料的執行緒停止，那麼重啟該執行緒
if (!tPort.IsAlive) {
tPort = new Thread(DealData);
tPort.Start();
}
if (!tPortDeal.IsAlive) {
tPortDeal = new Thread(ReceiveData);
tPortDeal.Start();
}
// StartCoroutine(DealData());
//Debug.Log( SerialPortThread.ThreadState);
}
/// <summary>
/// 資料接收 程序, 使用佇列來儲存資料,避免陣列的上鎖和解鎖問題
/// 設計模式 多執行緒+資料池
/// </summary>
/// <returns>
/// The data.
/// </returns>
/*****************資料接收*************************/
private void ReceiveData () {
//讀取資料，同時存入資料池中
//newline結束標記,同時使用readIlne()方法
try {
//從輸入快取區中去讀取第一次出現"AA"時的內容
Byte[] buf=new Byte[120];
if (open) sp.Read(buf,0,120);
//如果沒有接到到資料,就返回
if (buf.Length ==0) {
return;
}
string sbReadline2str = string.Empty;
if (buf != null)
{
for (int i = 0; i < buf.Length; i++)
{
sbReadline2str += buf[i].ToString("X2");
}
}
//sbReadline2str = System.Text.Encoding.Default.GetString(buf, 0,buf.Length);
// Debug.LogError(sbReadline2str.ToString());
// sbReadline2str = buf.ToString();
// sbReadline2str = sbReadline.ToString();
//提取完整的一個數據包，壓入資料池中(佇列中)
if (sbReadline2str.StartsWith("DB90"))
{ //分組資料包
string[] _str = Regex.Split(sbReadline2str, "55AA", RegexOptions.IgnoreCase);
foreach (string s in _str)
{
if (s.Length == 16)
{
//資料進入佇列
queueDataPool.Enqueue(s + "55AA");
//Debug.LogError(s+"55AA");
}
}
}
else {
sbReadline2str.Remove(0,sbReadline2str.IndexOf("DB90")+1);
string[] _str = Regex.Split(sbReadline2str, "55AA", RegexOptions.IgnoreCase);
foreach (string s in _str)
{
if (s.Length == 16)
{
//資料進入佇列
queueDataPool.Enqueue(s + "55AA");
// Debug.LogError(s+"55AA");
}
// Convert.ToByte(s,16);
}
}
}catch (Exception ex) {
Debug.LogError(ex);
}
//yield return null;
}
/***************處理出隊的資料***********************/
private void DealData()
{
while (queueDataPool.Count != 0)
{//迴圈檢測佇列
//佇列中有資料
//處理出隊的資料
//迴圈佇列，首先載入第一包資料，只執行一次
if (flag)
{ //初始化第一組資料
for (int i = 0; i < 7; i++)
{ //出佇列，同時移除這個資料
strOutPool = queueDataPool.Dequeue();
float[] jiaodu = DealReceivedDatanow(strOutPool);
AVEFangXiangJiao.Enqueue(jiaodu[0]);
AVEYangJiao.Enqueue(jiaodu[1]);
AVEXuanZhuanJiao.Enqueue(jiaodu[2]);
}
FangXiangJiao = Caulation(AVEFangXiangJiao);
YangJiao = Caulation(AVEYangJiao);
XuanZhuanJiao = Caulation(AVEXuanZhuanJiao);
flag = false;
}
else
{/*採用7點平滑濾波，消除抖動現象.注意特殊點的處理360到0之間的資料*/
//沒有存在跳變的現象，那就直接儲存資料
for (int i = 0; i < 7; i++)
{
//出佇列，同時移除這個資料。
//載入7包資料，
strOutPool = queueDataPool.Dequeue();
//組合載入的資料，低位元組在前，高位元組在後，return 三個角度值
float[] jiaodu = DealReceivedDatanow(strOutPool);
//角度迴圈佇列更新最新進入的資料
AVEFangXiangJiao.Dequeue();
//角度迴圈佇列在隊尾插入新的資料
AVEFangXiangJiao.Enqueue(jiaodu[0]);
//計算更新後的迴圈佇列中的7個數據平均值
/*****************處理特殊資料************************/
int[] IntPanDuan = CheackDataTuBian(AVEFangXiangJiao);
//如果存在著跳變的現象
if (IntPanDuan[0] == 1)
{//保留佇列前面的資料，移除後面的資料
float[] FXj = AVEFangXiangJiao.ToArray();
float sum = 0f;
//獲取要移除資料的索引值
int indexOfRemovingData = IntPanDuan[2];
for (int j = 0; j < indexOfRemovingData; j++)
{
sum += FXj[j];
}
//計算平均值
FangXiangJiao = sum / indexOfRemovingData;
}
FangXiangJiao = Caulation(AVEFangXiangJiao);
/******************處理特殊資料結束*************************/
/*******************************************/
/*同上*/
AVEYangJiao.Dequeue();
AVEYangJiao.Enqueue(jiaodu[1]);
YangJiao = Caulation(AVEYangJiao);
/*同上*/
AVEXuanZhuanJiao.Dequeue();
AVEXuanZhuanJiao.Enqueue(jiaodu[2]);
XuanZhuanJiao = Caulation(AVEXuanZhuanJiao);
/******************************************/
}
}
}
}
/************************檢驗特殊值***************************/
private int[] CheackDataTuBian(Queue<float> cheackQueue)
{
float[] cheackDataArrary = cheackQueue.ToArray();
//flag =0表示false; flag=1表示true
int flag = 0;
int[] BoolAndIndexOfNext=new int[2];
for (int count = 0; count < 6; count++ )
{float Previous =cheackDataArrary[count];
float Next = cheackDataArrary[count+1];
if (Mathf.Abs(Previous - Next)>350.0F && Mathf.Abs(Previous -Next)<360.0f) {
flag = 1;
BoolAndIndexOfNext[0] = flag;
BoolAndIndexOfNext[1] = count + 1; break;
}
}
return BoolAndIndexOfNext;
}
/*****************計算平均值****************************/
private float Caulation(Queue<float> AVEf) {
float _f=0.0f;
foreach (float f in AVEf) {
_f+=f;
}
return _f/7;
}
/*****************處理來自資料池的資料**************************/
private float[] DealReceivedDatanow (string DataBuf) {
//this is a whole frame data package,then convert the
//First convert the Byte type into Char type
// Debug.Log("--starting");
//Debug.Log("databuf----"+DataBuf);
/*把16進位制字串轉換成位元組陣列*/
byte[] returnBytes = new byte[DataBuf.Length / 2];
for (int i = 0; i < returnBytes.Length; i++)
returnBytes[i] = Convert.ToByte(DataBuf.Substring(i * 2, 2), 16);
/*消除抖動，因為眼鏡感測器會不斷的傳送資料，即使是不動的狀態下，也會有這樣的現象*/
float[] jiaoduCollection =new float[3];
_FangXiangJiao = (float)((byte)returnBytes[2] + ((sbyte)returnBytes[3] << 8)) / 10;
jiaoduCollection[0] =_FangXiangJiao;
// Debug.Log("fx-->"+FangXiangJiao);
_YangJiao = (float)((byte)returnBytes[4] + ((sbyte)returnBytes[5] << 8)) / 10;
jiaoduCollection[1] =_YangJiao;
// Debug.Log("yj-->"+YangJiao);
_XuanZhuanJiao = -(float)((byte)returnBytes[6] + ((sbyte)returnBytes[7] << 8)) / 10;
jiaoduCollection[2]=_XuanZhuanJiao;
//Debug.Log("xz-->"+XuanZhuanJiao);
//Debug.Log("--ending");
return jiaoduCollection;
}
/********************處理來自資料池的資料**************************/
private void DealReceivedData (string dataOutPool)
{
//讀取串列埠的資料, 如果沒有反應就會出現timeout 異常
// 資料格式：DB 90 AF 0C A2 FF 2C 00 55 AA
//包頭是DB 90 包尾：55 AA
//一幀傳送一個數據包 總大小10個位元組，
//int DataNumber;
try {
//讀取資料
byte tempB;
tempB= (byte)sp.ReadByte();
//Time.time;
//Read the header first
if (tempB ==219)
{
//Debug.Log("i get the 0XDB"+(byte)0xDB);
DataBuf[0]=tempB;
tempB= (byte)sp.ReadByte();
if (tempB ==144)
DataBuf[1]=tempB;
int DataNumber=2;
while(DataNumber<10){
tempB= (byte) sp.ReadByte();
DataBuf[DataNumber]=tempB;
DataNumber++;
}
}
//read out the input data
//cheack the header and tail for a data package.
//this is a whole frame data package,then convert the
//First convert the Byte type into Char type
// Debug.Log("--starting");
FangXiangJiao=(float)(DataBuf[2]+((sbyte)DataBuf[3]<<8))/10;
/*while(NumberCount <7) {
//_FangXiangJiao =
}*/
// Debug.Log("fx-->"+FangXiangJiao);
YangJiao = (float)(DataBuf[4]+((sbyte)DataBuf[5]<<8))/10;
// Debug.Log("yj-->"+YangJiao);
XuanZhuanJiao = (float)(DataBuf[6]+((sbyte)DataBuf[7]<<8))/10;
// Debug.Log("xz-->"+XuanZhuanJiao);
//Debug.Log("--ending");
}
catch (Exception e) {
Debug.LogError(e);
}
//Debug.Log("starting");
foreach(byte b in DataBuf) {
//Debug.LogError(Convert.ToString(b,16));
}
//Debug.Log("ending");
//yield return null;
}
/*********************************************/
//在沒有開啟串列埠裝置電源的時候，顯示訊息視窗，開啟電源之後，點選確定按鈕
private void OnGUI() {
if (showMessage)
{
GUI.BeginGroup(new Rect(Screen.width/2-100,Screen.height-60,400,400));
GUI.Label(new Rect(Screen.width / 2 - 80, Screen.height - 60,200,150), "請開啟串列埠裝置電源！然後點選確定");
if (GUI.Button(new Rect(Screen.width / 2 -80, Screen.height +95 , 100, 100), "確定"))
{
open = true;
}
GUI.EndGroup();
}
}