/*private void TrasformCoordinate(NavigationInfo Ori, NavigationInfo src, NavigationInfo dst, int degrees)
{
NavigationInfo temp = new NavigationInfo();
temp.LocationX = src.LocationX;
temp.LocationY = src.LocationY;
//座標旋轉
int degr = degrees;
if (degr > 180) degr = 360 - degr;
else degr = -degr;
double angle = Math.PI * degr / 180.0;
double sinAngle = Math.Sin(angle);
double cosAngle = Math.Cos(angle);
dst.LocationX = (int)((double)(temp.LocationX - Ori.LocationX) * cosAngle + (double)(temp.LocationY - Ori.LocationY) * sinAngle) + Ori.LocationX;
dst.LocationY = (int)((double)-(temp.LocationX - Ori.LocationX) * sinAngle + (double)(temp.LocationY - Ori.LocationY) * cosAngle) + Ori.LocationY;
}*/
private void TrasformCoordinateForFork(NavigationInfo Ori, NavigationInfo src, NavigationInfo dst, int degrees)
{
//座標旋轉
int degr = degrees;
if (degr > 180) degr = 360 - degr;
else degr = -degr;
if (degr >= 88 && degr <= 90) degr = 90;//減少座標誤差
double angle = Math.PI * degr / 180.0;
double sinAngle = Math.Sin(angle);
double cosAngle = Math.Cos(angle);
dst.LocationX = (int)((double)(src.LocationX - Ori.LocationX) * cosAngle + (double)(src.LocationY - Ori.LocationY) * sinAngle) + Ori.LocationX;
dst.LocationY = (int)((double)-(src.LocationX - Ori.LocationX) * sinAngle + (double)(src.LocationY - Ori.LocationY) * cosAngle) + Ori.LocationY;
}
/// <summary>
/// 堆高機座標推算
/// </summary>
/// <param name="Ori">[IN] 輸入座標</param>
/// <param name="src">[IN] 輸入計算座標</param>
/// <param name="dst">[OUT] 回傳計算結果座標</param>
/// <param name="degrees">角度</param>
public void TrasformCoordinate(NavigationInfo Ori, NavigationInfo src, NavigationInfo dst, int degrees)
{
NavigationInfo temp = new NavigationInfo();
temp.LocationX = src.LocationX;
temp.LocationY = src.LocationY;
//座標旋轉
int degr = degrees;
if (degr > 180) degr = 360 - degr;
else degr = -degr;
double angle = Math.PI * degr / 180.0;
double sinAngle = Math.Sin(angle);
double cosAngle = Math.Cos(angle);
dst.LocationX = (int)((double)(temp.LocationX - Ori.LocationX) * cosAngle + (double)(temp.LocationY - Ori.LocationY) * sinAngle) + Ori.LocationX;
dst.LocationY = (int)((double)-(temp.LocationX - Ori.LocationX) * sinAngle + (double)(temp.LocationY - Ori.LocationY) * cosAngle) + Ori.LocationY;
}
/// <summary>
/// 解析NAV回傳資料
/// </summary>
/// <param name="Receive">[IN] 資料字串</param>
public void CheckReceiveData(string Receive)
{
//分析座標資訊
if (Receive.IndexOf("sAN mNPOSGetPose") != -1)
{
//字串用空格隔開
string[] EachData = Receive.Split(' ');
if (EachData.Length < 4) return;
//檢查錯誤碼
string ErrorCode = EachData[3];
if (ErrorCode != "0")
{
//有錯誤 解析錯誤碼
Console.WriteLine(ChecNavkErrorCode(ErrorCode));
Console.WriteLine(Receive);
Console.WriteLine("------------------------------------------");
return;
}
if (EachData.Length < 7) return;
//取得座標資訊
int Phi = Convert.ToInt32(EachData[8], 16); //解析角度
float Direction = (float)Phi / (float)1000; //角度單位換算
NavigationInfo ReceiveInfo = new NavigationInfo(); //New資訊空間
ReceiveInfo.LocationX = Convert.ToInt32(EachData[6], 16); //解析X座標
ReceiveInfo.LocationY = Convert.ToInt32(EachData[7], 16); //解析Y座標
ReceiveInfo.Direction = (int)Direction; //解析角度
CrrentLocation = ReceiveInfo; //記錄當前座標
}
if (Receive.IndexOf("sAN mNAVGetTimestamp") != -1)
{
//字串用空格隔開
string[] EachData = Receive.Split(' ');
if (EachData.Length < 3) return;
//檢查錯誤碼
string ErrorCode = EachData[2];
if (ErrorCode != "0")
{
//有錯誤 解析錯誤碼
Console.WriteLine(ChecNavkErrorCode(ErrorCode));
return;
}
//解析NAV回傳之TimeStamp
string StampData = EachData[3];
if (StampData != "") NavTimeStamp = StampData;
}
if (Receive.IndexOf("sAN mNPOSSetSpeed") != -1)
{
//Console.WriteLine(Receive.ToString());
}
}
/// <summary>
/// NAV連線
/// </summary>
/// <param name="strIP">[IN] 輸入IP</param>
/// <param name="strPort">[IN] 輸入Port</param>
/// <returns>連線成功回傳true，否則回傳false</returns>
public bool ConnectNAV(string strIP, string strPort)
{
try
{
if (strIP != null && strPort != null)
{
//New資訊空間
CrrentLocation = new NavigationInfo();
TFrontRightLocation = new NavigationInfo();
int port = Convert.ToInt16(strPort);
//開始連線
sender_TCP = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);
sender_TCP.Connect(strIP, port);
if (sender_TCP.Connected) return true;
else return false;
}
else return false;
}
catch (Exception ex)
{
MessageBox.Show(ex.ToString());
return false;
}
}
