/// <summary> 求某向量乘上某一純量 </summary>
public static rtVector VectorMultiple(rtVector a_tSrc, double a_eMultiple)
{
rtVector tMultipleVector = new rtVector();
tMultipleVector.eX = a_tSrc.eX * a_eMultiple;
tMultipleVector.eY = a_tSrc.eY * a_eMultiple;
return(tMultipleVector);
}
/// <summary> 求兩向量的夾角 </summary>
public static double GetTheta(rtVector a_tV1, rtVector a_tV2)
{
double eTheta = 0;
eTheta = Dot(a_tV1, a_tV2);
eTheta /= GetLength(a_tV1) * GetLength(a_tV2);
eTheta = Math.Acos(eTheta) * 180.0 / Math.PI;
return(eTheta);
}
/// <summary> 求向量a_tIn 要順時針轉幾度才會到 向量a_tTarget </summary>
public static double GetAngleDiff(rtVector a_tTarget, rtVector a_tIn)
{
double eTheta = 0, eCross = 0;
eTheta = rtVectorOP_2D.GetTheta(a_tTarget, a_tIn);
eCross = rtVectorOP_2D.Cross(a_tIn, a_tTarget);
eTheta = (eCross < 0) ? -eTheta : eTheta;
return(eTheta);
}
public static double FindVectorMultipleOfMeetPoint(rtVector a_tSrc1, rtVector a_tV1, rtVector a_tSrc2, rtVector a_tV2)
{
double eT1 = 0;
// 求交點對應的向量係數 for Line 1
eT1 = a_tSrc2.eX * a_tV2.eY - a_tSrc2.eY * a_tV2.eX - a_tSrc1.eX * a_tV2.eY + a_tSrc1.eY * a_tV2.eX;
eT1 /= a_tV1.eX * a_tV2.eY - a_tV1.eY * a_tV2.eX;
return(eT1);
}
public static double GetDistance(rtVector a_tP1, rtVector a_tP2)
{
double eDistance = 0;
double eGapX = 0, eGapY = 0;
eGapX = a_tP2.eX - a_tP1.eX;
eGapY = a_tP2.eY - a_tP1.eY;
eDistance = Math.Sqrt(eGapX * eGapX + eGapY * eGapY);
return(eDistance);
}
/// <summary> 求角度a_tIn 要順時針轉幾度才會到 向量a_tTarget </summary>
public static double GetAngleDiff(rtVector a_tTarget, double a_eIn)
{
double eTheta = 0, eCross = 0;
rtVector tIn = new rtVector();
tIn = rtVectorOP_2D.Angle2Vector(a_eIn);
eTheta = rtVectorOP_2D.GetTheta(a_tTarget, tIn);
eCross = rtVectorOP_2D.Cross(tIn, a_tTarget);
eTheta = (eCross < 0) ? -eTheta : eTheta;
return(eTheta);
}
/// <summary> 求向量a_tIn 要順時針轉幾度才會到 角度a_tTarget </summary>
public static double GetAngleDiff(double a_eTarget, rtVector a_tIn)
{
double eTheta = 0, eCross = 0;
rtVector tTarget = new rtVector();
tTarget = rtVectorOP_2D.Angle2Vector(a_eTarget);
eTheta = rtVectorOP_2D.GetTheta(tTarget, a_tIn);
eCross = rtVectorOP_2D.Cross(a_tIn, tTarget);
eTheta = (eCross < 0) ? -eTheta : eTheta;
return(eTheta);
}
/// <summary> 求某一點對某向量延伸一定長度 </summary>
public static rtVector ExtendPointAlongVector(rtVector a_tPoint, rtVector a_tDirection, int a_lExtendSize)
{
double eT = 0, eSizeVetor = 0;
rtVector tExtendPoint = new rtVector();
eSizeVetor = GetLength(a_tDirection);
eT = a_lExtendSize / eSizeVetor;
tExtendPoint.eX = a_tPoint.eX + a_tDirection.eX * eT;
tExtendPoint.eY = a_tPoint.eY + a_tDirection.eY * eT;
return(tExtendPoint);
}
/// <summary> 求兩線段的交點 </summary>
public static rtVector FindMeetPoint(rtVector a_tSrc1, rtVector a_tV1, rtVector a_tSrc2, rtVector a_tV2)
{
rtVector tMeetPoint = new rtVector();
double eT1 = 0;
eT1 = FindVectorMultipleOfMeetPoint(a_tSrc1, a_tV1, a_tSrc2, a_tV2);
// 求交點(current point 沿著向量與路徑的交點)
tMeetPoint.eX = a_tSrc1.eX + a_tV1.eX * eT1;
tMeetPoint.eY = a_tSrc1.eY + a_tV1.eY * eT1;
return(tMeetPoint);
}
/// <summary> 求某向量對另一向量的投影 </summary>
public static rtVector VectorProject(rtVector a_tSrc, rtVector a_tBase)
{
rtVector tProjectedVector = new rtVector();
double eMultiple = 0;
double eDistanceBase = 0;
eDistanceBase = GetLength(a_tBase);
eMultiple = Dot(a_tSrc, a_tBase) / (eDistanceBase * eDistanceBase);
tProjectedVector = VectorMultiple(a_tBase, eMultiple);
return(tProjectedVector);
}
/// <summary> 求某一點對某中心做旋轉的結果 </summary>
public static rtVector Rotate(rtVector a_tPoint, rtVector a_tCenter, double a_eTheta)
{ // 角度單位是徑度 甭轉換
rtVector tResult = new rtVector();
rtVector tTmp, tTmp1;
tTmp.eX = a_tPoint.eX - a_tCenter.eX;
tTmp.eY = a_tPoint.eY - a_tCenter.eY;
tTmp1.eX = Math.Cos(a_eTheta) * tTmp.eX - Math.Sin(a_eTheta) * tTmp.eY;
tTmp1.eY = Math.Sin(a_eTheta) * tTmp.eX + Math.Cos(a_eTheta) * tTmp.eY;
tResult.eX = tTmp1.eX + a_tCenter.eX;
tResult.eY = tTmp1.eY + a_tCenter.eY;
return(tResult);
}
/// <summary> 求角度差距: a_tV_Src轉到a_tV_Target 是順時針角度為正 否則為負 </summary>
public static double GetTheta_Difference(rtVector a_tV_Src, rtVector a_tV_Target)
{
double eTheta = 0, eCross = 0;
rtVector tCenter = new rtVector();
tCenter.Init();
eTheta = GetTheta(a_tV_Src, a_tV_Target);
eCross = Cross(a_tV_Src, a_tV_Target);
if (eCross < 0)
{
return(-eTheta);
}
else
{
return(eTheta);
}
}
/// <summary> 給向量回傳對應角度 0~360度 </summary>
public static double Vector2Angle(rtVector a_tVector)
{
double eAngle = 0;
if (a_tVector.eX == 0)
{
if (a_tVector.eY > 0)
{
eAngle = 90;
}
else if (a_tVector.eY < 0)
{
eAngle = -90;
}
else
{
// show error msg
eAngle = 0;
}
}
else
{
eAngle = Math.Atan(a_tVector.eY / a_tVector.eX) * 180 / Math.PI;
if (eAngle > 0)
{
if (a_tVector.eX < 0)
{
eAngle += 180;
}
}
else
{
if (a_tVector.eX < 0)
{
eAngle += 180;
}
else
{
eAngle += 360;
}
}
}
return(eAngle);
}
/// <summary> 求向量a_tIn 要順時針轉幾度才會到 角度a_tTarget </summary>
public static double GetAngleDiff(double a_eTarget, rtVector a_tIn)
{
double eTheta = 0, eCross = 0;
rtVector tTarget = new rtVector();
tTarget = rtVectorOP_2D.Angle2Vector(a_eTarget);
eTheta = rtVectorOP_2D.GetTheta(tTarget, a_tIn);
eCross = rtVectorOP_2D.Cross(a_tIn, tTarget);
eTheta = (eCross < 0) ? -eTheta : eTheta;
return eTheta;
}
/// <summary> 求角度a_tIn 要順時針轉幾度才會到 向量a_tTarget </summary>
public static double GetAngleDiff(rtVector a_tTarget, double a_eIn)
{
double eTheta = 0, eCross = 0;
rtVector tIn = new rtVector();
tIn = rtVectorOP_2D.Angle2Vector(a_eIn);
eTheta = rtVectorOP_2D.GetTheta(a_tTarget, tIn);
eCross = rtVectorOP_2D.Cross(tIn, a_tTarget);
eTheta = (eCross < 0) ? -eTheta : eTheta;
return eTheta;
}
/// <summary> 求某一點對某向量延伸一定長度 </summary>
public static rtVector ExtendPointAlongVector(rtVector a_tPoint, rtVector a_tDirection, int a_lExtendSize)
{
double eT = 0, eSizeVetor = 0;
rtVector tExtendPoint = new rtVector();
eSizeVetor = GetLength(a_tDirection);
eT = a_lExtendSize / eSizeVetor;
tExtendPoint.eX = a_tPoint.eX + a_tDirection.eX * eT;
tExtendPoint.eY = a_tPoint.eY + a_tDirection.eY * eT;
return tExtendPoint;
}
/// <summary> 求某向量對另一向量的投影 </summary>
public static rtVector VectorProject(rtVector a_tSrc, rtVector a_tBase)
{
rtVector tProjectedVector = new rtVector();
double eMultiple = 0;
double eDistanceBase = 0;
eDistanceBase = GetLength(a_tBase);
eMultiple = Dot(a_tSrc, a_tBase) / (eDistanceBase * eDistanceBase);
tProjectedVector = VectorMultiple(a_tBase, eMultiple);
return tProjectedVector;
}
/// <summary> calculate error of turn </summary>
/// <param name="a_tPathInfo">[IN] path data for navigate </param>
/// <param name="a_tPosition">[IN] car position </param>
/// <param name="a_tTurnCenter">[IN] turn Center </param>
/// <param name="a_lTurnRadius">[IN] turn Radius </param>
/// <param name="a_lTurnDirection">[IN] turn Direction </param>
/// <returns> error of turn </returns>
public static double MotorAngle_TurnErrorCal(
rtPath_Info a_tPathInfo, rtVector a_tPosition,
rtVector a_tTurnCenter, int a_lTurnRadius, int a_lTurnDirection)
{
double eErrorCurrent = 0, eDistance = 0;
if(a_tPathInfo.ucTurnType == (byte)rtPath_Info.rtTurnType.SMOOTH)
{
eDistance = rtVectorOP_2D.GetDistance(a_tPosition, a_tTurnCenter);
eErrorCurrent = eDistance - a_lTurnRadius; // 可能會有錯誤 如果在內側非扇型區域 >> TBD
}
else
{
return 0;
}
switch (a_lTurnDirection)
{
case (int)rtTurnType_Simple.CAR_TURN_RIGHT:
// inverse
eErrorCurrent = -eErrorCurrent;
break;
default:
// Do nothing
break;
}
return eErrorCurrent;
}
/// <summary> 誤差為點到路徑的距離: 點可順時鐘轉至路徑為正 否則為負 </summary>
/// <param name="a_tPathInfo">[IN] path data for navigate </param>
/// <param name="a_tPosition">[IN] position of car </param>
/// <returns> distance error </returns>
public static double PathErrorCal_Straight(rtPath_Info a_tPathInfo, rtVector a_tPosition)
{
double eErrorCurrent = 0, eCross = 0;
rtVector tVS2D, tVlaw, tVproject, tVS2C;
tVS2C = rtVectorOP_2D.GetVector(a_tPathInfo.tSrc, a_tPosition);
tVS2D = rtVectorOP_2D.GetVector(a_tPathInfo.tSrc, a_tPathInfo.tDest);
// 取右側的法向量
tVlaw.eX = tVS2D.eY;
tVlaw.eY = -tVS2D.eX;
// 將向量投影到法向量上
tVproject = rtVectorOP_2D.VectorProject(tVS2C, tVlaw);
// 向量長度即為點到路徑的距離
eErrorCurrent = rtVectorOP_2D.GetLength(tVproject);
eCross = rtVectorOP_2D.Cross(tVS2C, tVS2D);
if (eCross < 0)
{ // 當下座標轉到路徑向量為逆時針 要取負值
eErrorCurrent = -eErrorCurrent;
}
return eErrorCurrent;
}
private void DiffTimeForAlignment(ref rtVector PredictPosition, ref double PredictAngle, rtMotorCtrl Data)
{
TimeSpan DisTime = DateTime.Now - GlobalVar.NavTimeStamp;
LocateData.tPosition.eX = GlobalVar.CurrentPosition.LocationX;
LocateData.tPosition.eY = GlobalVar.CurrentPosition.LocationY;
LocateData.eAngle = GlobalVar.CurrentPosition.Direction;
LocateData.eWheelAngle = GlobalVar.RealMotorAngle;
a_tAGV_Data.tCarInfo = LocateData;
rtMotorCtrl.Motion_Predict(a_tAGV_Data.tCarInfo, Data, DisTime.TotalMilliseconds / 1000, out PredictPosition, out PredictAngle);
}
public static void LoadAllPoints(string PointsFileName, ref rtVector[] PointsArray)
{
//讀取節點並寫入陣列
string[] lines = System.IO.File.ReadAllLines(PointsFileName);
int PointsCount = lines.Length;
PointsArray = new rtVector[PointsCount];
int Count = 0;
foreach (string line in lines)
{
string[] EachPoint = line.Split(',');
for (int i = 0; i < EachPoint.Length; i++)
{
if (EachPoint[i] == "") continue;
if (EachPoint.Length == 2)
{
PointsArray[Count].eX = Convert.ToInt32(EachPoint[0]);
PointsArray[Count].eY = Convert.ToInt32(EachPoint[1]);
}
}
Count++;
}
}
/// <summary> 求某向量外積 (a_tV1 * a_tV2) </summary>
public static double Cross(rtVector a_tV1, rtVector a_tV2)
{
double eOut = 0;
eOut = a_tV1.eX * a_tV2.eY - a_tV1.eY * a_tV2.eX;
return eOut;
}
/// <summary> 給角度回傳對應的單位向量 </summary>
public static rtVector Angle2Vector(double a_eAngle)
{
// 回傳單位向量
rtVector tVector = new rtVector();
tVector.eX = Math.Cos(a_eAngle * Math.PI / 180);
tVector.eY = Math.Sin(a_eAngle * Math.PI / 180);
return tVector;
}
public static double GetDistance(rtVector a_tP1, rtVector a_tP2)
{
double eDistance = 0;
double eGapX = 0, eGapY = 0;
eGapX = a_tP2.eX - a_tP1.eX;
eGapY = a_tP2.eY - a_tP1.eY;
eDistance = Math.Sqrt(eGapX * eGapX + eGapY * eGapY);
return eDistance;
}
public static double FindVectorMultipleOfMeetPoint(rtVector a_tSrc1, rtVector a_tV1, rtVector a_tSrc2, rtVector a_tV2)
{
double eT1 = 0;
// 求交點對應的向量係數 for Line 1
eT1 = a_tSrc2.eX * a_tV2.eY - a_tSrc2.eY * a_tV2.eX - a_tSrc1.eX * a_tV2.eY + a_tSrc1.eY * a_tV2.eX;
eT1 /= a_tV1.eX * a_tV2.eY - a_tV1.eY * a_tV2.eX;
return eT1;
}
/// <summary> 求兩線段的交點 </summary>
public static rtVector FindMeetPoint(rtVector a_tSrc1, rtVector a_tV1, rtVector a_tSrc2, rtVector a_tV2)
{
rtVector tMeetPoint = new rtVector();
double eT1 = 0;
eT1 = FindVectorMultipleOfMeetPoint(a_tSrc1, a_tV1, a_tSrc2, a_tV2);
// 求交點(current point 沿著向量與路徑的交點)
tMeetPoint.eX = a_tSrc1.eX + a_tV1.eX * eT1;
tMeetPoint.eY = a_tSrc1.eY + a_tV1.eY * eT1;
return tMeetPoint;
}
/// <summary> 求向量a_tIn 要順時針轉幾度才會到 向量a_tTarget </summary>
public static double GetAngleDiff(rtVector a_tTarget, rtVector a_tIn)
{
double eTheta = 0, eCross = 0;
eTheta = rtVectorOP_2D.GetTheta(a_tTarget, a_tIn);
eCross = rtVectorOP_2D.Cross(a_tIn, a_tTarget);
eTheta = (eCross < 0) ? -eTheta : eTheta;
return eTheta;
}
public void Log()
{
//Log資料新增
if (GlobalVar.isLog)
{
int TempPathIndex = 0;
byte TempPathucStatus = 0;
byte TempPathTurnType = 0;
rtVector TempPathSrc = new rtVector();
rtVector TempPathDst = new rtVector();
if (DeliverData.tAGV_Data.atPathInfo != null)
{
if (DeliverData.tAGV_Data.atPathInfo.Length > 0)
{
TempPathIndex = DeliverData.tAGV_Data.CMotor.tMotorData.lPathNodeIndex;
TempPathucStatus = DeliverData.tAGV_Data.atPathInfo[TempPathIndex].ucStatus;
TempPathSrc = DeliverData.tAGV_Data.atPathInfo[TempPathIndex].tSrc;
TempPathDst = DeliverData.tAGV_Data.atPathInfo[TempPathIndex].tDest;
TempPathTurnType = DeliverData.tAGV_Data.atPathInfo[TempPathIndex].ucTurnType;
}
}
LocateData = new rtCarData();
rtMotorCtrl PIDAns = new rtMotorCtrl();
LocateData = DeliverData.tAGV_Data.tCarInfo;
PIDAns = DeliverData.tAGV_Data.CMotor;
LogCarInfo.Add(LocateData.eAngle + "," + LocateData.eWheelAngle + "," + Math.Round(LocateData.tPosition.eX, 2) + "," + Math.Round(LocateData.tPosition.eY, 2) + "," + Math.Round(LocateData.tCarTirepositionR.eX, 2) + "," +
Math.Round(LocateData.tCarTirepositionR.eY, 2) + "," + Math.Round(LocateData.tCarTirepositionL.eX, 2) + "," + Math.Round(LocateData.tCarTirepositionL.eY, 2) + "," + Math.Round(LocateData.tMotorPosition.eX, 2) + "," +
Math.Round(LocateData.tMotorPosition.eY, 2) + "," + LocateData.eCarTireSpeedLeft + "," + LocateData.eCarTireSpeedRight);
LogMainData.Add(DeliverData.tAGV_Data.ucAGV_Status + "," + DeliverData.tAGV_Data.CFork.tForkData.ucStatus + "," + PIDAns.tMotorData.ePathError + "," + PIDAns.tMotorData.eDistance2Dest + "," + PIDAns.tMotorData.lMotorAngle + "," +
PIDAns.tMotorData.lMotorPower + "," + PIDAns.tMotorData.bFinishFlag + "," + TempPathIndex + "," + TempPathucStatus + "," + TempPathSrc.eX + "/" + TempPathSrc.eY + "," + TempPathDst.eX + "/" + TempPathDst.eY +
"," + "0" + "," + TempPathTurnType.ToString());
LogDetailData.Add(Math.Round(PIDAns.tMotorData.Debug_ePathThetaError, 2) + "," + Math.Round(PIDAns.tMotorData.Debug_TargetAngleOffset1, 2) + "," + Math.Round(PIDAns.tMotorData.Debug_TargetAngleOffset2, 2) +
"," + PIDAns.tMotorData.Debug_CenterSpeed + "," + Math.Round(PIDAns.tMotorData.Debug_eDeltaCarAngle, 2) + "," + PIDAns.tMotorData.bOverDest + "," + PIDAns.tMotorData.bBackWard + "," + PIDAns.tMotorData.lNavigateOffset);
}
}
public static void CreateWeightingTable(string PointsFileName, string OutTableFileName)
{
string[] lines = System.IO.File.ReadAllLines(PointsFileName);
int PointsCount = lines.Length;
int[] TableArray = new int[PointsCount * PointsCount];
rtVector[] AllPoint = new rtVector[PointsCount];
int Count = 0;
//從檔案讀取所有座標點資料
foreach (string line in lines)
{
string[] EachPoint = line.Split(',');
AllPoint[Count].eX = Convert.ToInt32(EachPoint[0]);
AllPoint[Count].eY = Convert.ToInt32(EachPoint[1]);
Count++;
}
//計算所有點之間的距離
for (int i = 0; i < PointsCount; i++)
{
for (int j = i; j < PointsCount; j++)
{
//自己
if (i == j) TableArray[i * PointsCount + j] = 0;
else
{
int Distance = (int)rtVectorOP_2D.GetDistance(AllPoint[i], AllPoint[j]);
TableArray[i * PointsCount + j] = Distance;
TableArray[j * PointsCount + i] = Distance;
}
}
}
// 文件寫入
FileStream fs = new FileStream(OutTableFileName, FileMode.Create);
StreamWriter sw = new StreamWriter(fs);
// 開始寫入
//
int lCnt;
for (lCnt = 0; lCnt < TableArray.Length; lCnt++)
{
if (lCnt % PointsCount == PointsCount - 1)
{
sw.Write(TableArray[lCnt] + "\n");
}
else
{
sw.Write(TableArray[lCnt] + "\t\t");
}
}
//清空暫存
sw.Flush();
//關閉檔案
sw.Close();
fs.Close();
////
}
/// <summary> control during car direction aligment </summary>
/// <param name="a_atPathInfo">[IN] path data for navigate </param>
/// <param name="a_tCarData">[IN] data of car </param>
public void Motor_CtrlNavigateAligment(rtPath_Info[] a_atPathInfo, rtCarData a_tCarData)
{
bool bAlignment = false;
double eErrorCurrent = 0, eTargetAngle = 0;
int lPathNodeIndex = 0;
rtVector tV_S2D_Next = new rtVector();
rtVector tV_Aligment = new rtVector();
rtPath_Info.rtTurnType tTurnTypeNext;
lPathNodeIndex = tMotorData.lPathNodeIndex;
tTurnTypeNext = (rtPath_Info.rtTurnType)a_atPathInfo[lPathNodeIndex + 1].ucTurnType;
tV_S2D_Next = rtVectorOP_2D.GetVector(a_atPathInfo[lPathNodeIndex+1].tSrc, a_atPathInfo[lPathNodeIndex+1].tDest);
tMotorData.bBackWard = BackWardVerify(a_atPathInfo[lPathNodeIndex+1], a_tCarData.eAngle);
if(tTurnTypeNext == rtPath_Info.rtTurnType.ARRIVE)
{ // 下段是要取放貨 >> 一定要正走
tV_Aligment = tV_S2D_Next;
}
else if(tTurnTypeNext == rtPath_Info.rtTurnType.PARK)
{ // 下段是要停車 >> 一定要反走走
tV_Aligment = rtVectorOP_2D.VectorMultiple(tV_S2D_Next, -1);
}
else
{ // 下段非停車或取放貨 >> 依照方便度自行決定正反走
tV_Aligment = (tMotorData.bBackWard) ? rtVectorOP_2D.VectorMultiple(tV_S2D_Next, -1) : tV_S2D_Next;
}
eTargetAngle = rtVectorOP_2D.Vector2Angle(tV_Aligment);
// 用車身方向與目標方向的夾角當誤差
eErrorCurrent = Math.Abs(rtAngleDiff.GetAngleDiff(tV_Aligment, a_tCarData.eAngle));
bAlignment = CarAngleAlignment(eTargetAngle, a_tCarData);
if (bAlignment)
{
a_atPathInfo[lPathNodeIndex].ucStatus = (byte)rtPath_Info.rtStatus.DONE;
tMotorData.lPathNodeIndex++;
}
tMotorData.ePathError = 0;
tMotorData.eDistance2Dest = eErrorCurrent;
}
/// <summary> calculate path error of turn </summary>
/// <param name="a_tPathInfo">[IN] path data for navigate </param>
/// <param name="a_tPosition">[IN] car position </param>
/// <param name="a_tTurnCenter">[IN] turn Center </param>
/// <param name="a_lTurnRadius">[IN] turn Radius </param>
/// <returns> path error of turn </returns>
public static double PathErrorCal_Turn(
rtPath_Info a_tPathInfo, rtVector a_tPosition,
rtVector a_tTurnCenter, int a_lTurnRadius)
{
double eErrorCurrent = 0, eDistance = 0, eCross = 0;
rtVector tVs2d = new rtVector();
rtVector tVs2center = new rtVector();
tVs2d = rtVectorOP_2D.GetVector(a_tPathInfo.tSrc, a_tPathInfo.tDest);
tVs2center = rtVectorOP_2D.GetVector(a_tPathInfo.tSrc, a_tTurnCenter);
eCross = rtVectorOP_2D.Cross(tVs2center, tVs2d);
eDistance = rtVectorOP_2D.GetDistance(a_tPosition, a_tTurnCenter);
if (a_tPathInfo.ucTurnType == (byte)rtPath_Info.rtTurnType.SMOOTH)
{
if(eCross < 0)
{ // 向左轉 >> 正的部分圓弧路徑外側
eErrorCurrent = eDistance - a_lTurnRadius;
}
else
{ // 向右轉 >> 正的部分圓弧路徑內側
eErrorCurrent = a_lTurnRadius - eDistance;
}
}
else
{
return 0;
}
return eErrorCurrent;
}
/// <summary> 給向量回傳對應角度 0~360度 </summary>
public static double Vector2Angle(rtVector a_tVector)
{
double eAngle = 0;
if (a_tVector.eX == 0)
{
if (a_tVector.eY > 0)
{
eAngle = 90;
}
else if (a_tVector.eY < 0)
{
eAngle = -90;
}
else
{
// show error msg
eAngle = 0;
}
}
else
{
eAngle = Math.Atan(a_tVector.eY / a_tVector.eX) * 180 / Math.PI;
if (eAngle > 0)
{
if (a_tVector.eX < 0)
{
eAngle += 180;
}
}
else
{
if (a_tVector.eX < 0)
{
eAngle += 180;
}
else
{
eAngle += 360;
}
}
}
return eAngle;
}
/// <summary> Check if car Finish Smooth Turn </summary>
/// <param name="a_lPathIndex">[IN] index of current path </param>
/// <param name="a_lRotationDistance">[IN] Rotation Distance </param>
/// <param name="a_atPathInfo">[IN] path data for navigate </param>
/// <param name="a_tCarPostition">[IN] car position </param>
/// <param name="a_tRotateCenter">[IN] Rotate Center </param>
public static bool FinishSmoothTurnCheck(
int a_lPathIndex, int a_lRotationDistance,
rtPath_Info[] a_atPathInfo, rtVector a_tCarPostition, rtVector a_tRotateCenter)
{
bool bResult = false;
double eThetaBoundaty = 0, eThetaCurrent = 0;
rtVector tTurnStart, tTurnEnd, tVd2sCurrent, tVs2dNext;
rtVector tCenter2SrcTurn, tCenter2DestTurn, tCenter2Current;
// set current vector
tVd2sCurrent = rtVectorOP_2D.GetVector(a_atPathInfo[a_lPathIndex].tDest, a_atPathInfo[a_lPathIndex].tSrc);
// 取轉彎起始點
tTurnStart = rtVectorOP_2D.ExtendPointAlongVector(a_atPathInfo[a_lPathIndex].tDest, tVd2sCurrent, a_lRotationDistance);
// set next vector
tVs2dNext = rtVectorOP_2D.GetVector(a_atPathInfo[a_lPathIndex + 1].tSrc, a_atPathInfo[a_lPathIndex + 1].tDest);
// 取轉彎結束點
tTurnEnd = rtVectorOP_2D.ExtendPointAlongVector(a_atPathInfo[a_lPathIndex + 1].tSrc, tVs2dNext, a_lRotationDistance);
// 以下計算是否超出扇形區域
tCenter2SrcTurn = rtVectorOP_2D.GetVector(a_tRotateCenter, tTurnStart);
tCenter2DestTurn = rtVectorOP_2D.GetVector(a_tRotateCenter, tTurnEnd);
tCenter2Current = rtVectorOP_2D.GetVector(a_tRotateCenter, a_tCarPostition);
eThetaBoundaty = rtVectorOP_2D.GetTheta(tCenter2DestTurn, tCenter2SrcTurn);
eThetaCurrent = rtVectorOP_2D.GetTheta(tCenter2Current, tCenter2SrcTurn);
bResult = (eThetaCurrent > eThetaBoundaty) ? true : false;
return bResult;
}
/// <summary> 求某向量乘上某一純量 </summary>
public static rtVector VectorMultiple(rtVector a_tSrc, double a_eMultiple)
{
rtVector tMultipleVector = new rtVector();
tMultipleVector.eX = a_tSrc.eX * a_eMultiple;
tMultipleVector.eY = a_tSrc.eY * a_eMultiple;
return tMultipleVector;
}
/// <summary> 求某向量長度 </summary>
public static double GetLength(rtVector a_tIn)
{
double eOut = 0;
eOut = Math.Sqrt(a_tIn.eX * a_tIn.eX + a_tIn.eY * a_tIn.eY);
return eOut;
}
/// <summary> 求某向量內積 </summary>
public static double Dot(rtVector a_tV1, rtVector a_tV2)
{
double eOut = 0;
eOut = a_tV1.eX * a_tV2.eX + a_tV1.eY * a_tV2.eY;
return eOut;
}
/// <summary> 求兩向量的夾角 </summary>
public static double GetTheta(rtVector a_tV1, rtVector a_tV2)
{
double eTheta = 0;
eTheta = Dot(a_tV1, a_tV2);
eTheta /= GetLength(a_tV1) * GetLength(a_tV2);
eTheta = Math.Acos(eTheta) * 180.0 / Math.PI;
return eTheta;
}
/// <summary> 求角度差距: a_tV_Src轉到a_tV_Target 是順時針角度為正 否則為負 </summary>
public static double GetTheta_Difference(rtVector a_tV_Src, rtVector a_tV_Target)
{
double eTheta = 0, eCross = 0;
rtVector tCenter = new rtVector();
tCenter.Init();
eTheta = GetTheta(a_tV_Src, a_tV_Target);
eCross = Cross(a_tV_Src, a_tV_Target);
if (eCross < 0)
{
return -eTheta;
}
else
{
return eTheta;
}
}
public static void CreateWeightingTable(string PointsFileName, string OutTableFileName)
{
string[] lines = System.IO.File.ReadAllLines(PointsFileName);
int PointsCount = lines.Length;
int[] TableArray = new int[PointsCount * PointsCount];
rtVector[] AllPoint = new rtVector[PointsCount];
int Count = 0;
//從檔案讀取所有座標點資料
foreach (string line in lines)
{
string[] EachPoint = line.Split(',');
AllPoint[Count].eX = Convert.ToInt32(EachPoint[0]);
AllPoint[Count].eY = Convert.ToInt32(EachPoint[1]);
Count++;
}
//計算所有點之間的距離
for (int i = 0; i < PointsCount; i++)
{
for (int j = i; j < PointsCount; j++)
{
//自己
if (i == j)
{
TableArray[i * PointsCount + j] = 0;
}
else
{
int Distance = (int)rtVectorOP_2D.GetDistance(AllPoint[i], AllPoint[j]);
TableArray[i * PointsCount + j] = Distance;
TableArray[j * PointsCount + i] = Distance;
}
}
}
// 文件寫入
FileStream fs = new FileStream(OutTableFileName, FileMode.Create);
StreamWriter sw = new StreamWriter(fs);
// 開始寫入
//
int lCnt;
for (lCnt = 0; lCnt < TableArray.Length; lCnt++)
{
if (lCnt % PointsCount == PointsCount - 1)
{
sw.Write(TableArray[lCnt] + "\n");
}
else
{
sw.Write(TableArray[lCnt] + "\t\t");
}
}
//清空暫存
sw.Flush();
//關閉檔案
sw.Close();
fs.Close();
////
}
/// <summary> 求兩點構成的向量 </summary>
public static rtVector GetVector(rtVector a_tP_Src, rtVector a_tP_Dest)
{
rtVector tVector = new rtVector();
tVector.eX = a_tP_Dest.eX - a_tP_Src.eX;
tVector.eY = a_tP_Dest.eY - a_tP_Src.eY;
return tVector;
}
private void Trasform_rtVector(rtVector Ori, rtVector src, ref rtVector dst, double degrees)
{
rtVector temp = new rtVector();
temp.eX = src.eX;
temp.eY = src.eY;
//座標旋轉
int degr = (int)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.eX = (int)((double)(temp.eX - Ori.eX) * cosAngle + (double)(temp.eY - Ori.eY) * sinAngle) + Ori.eX;
dst.eY = (int)((double)-(temp.eX - Ori.eX) * sinAngle + (double)(temp.eY - Ori.eY) * cosAngle) + Ori.eY;
}
/// <summary> 求某一點對某中心做旋轉的結果 </summary>
public static rtVector Rotate(rtVector a_tPoint, rtVector a_tCenter, double a_eTheta)
{
// 角度單位是徑度 甭轉換
rtVector tResult = new rtVector();
rtVector tTmp, tTmp1;
tTmp.eX = a_tPoint.eX - a_tCenter.eX;
tTmp.eY = a_tPoint.eY - a_tCenter.eY;
tTmp1.eX = Math.Cos(a_eTheta) * tTmp.eX - Math.Sin(a_eTheta) * tTmp.eY;
tTmp1.eY = Math.Sin(a_eTheta) * tTmp.eX + Math.Cos(a_eTheta) * tTmp.eY;
tResult.eX = tTmp1.eX + a_tCenter.eX;
tResult.eY = tTmp1.eY + a_tCenter.eY;
return tResult;
}
public void UpdateDeliverData()
{
rtVector PredictPosition = new rtVector();
rtAGV_Data src = new rtAGV_Data();
rtAGV_Data dst = new rtAGV_Data();
//讀取PLC的高度及速度資訊
if (DeliverData.tAGV_Data.ucAGV_Status != 0) GetFrontWheelSpeed();
//紀錄讀取完的時間
//GlobalVar.Watch_Read_PLC_Data.Start();
//GlobalVar.Time_Read_PLC_Data = DateTime.Now;
//計算預測座標
double PredictAngle = 0;
DiffTimeForAlignment(ref PredictPosition, ref PredictAngle, DeliverData.tAGV_Data.CMotor);
src.tCarInfo.tPosition.eX = PredictPosition.eX;
src.tCarInfo.tPosition.eY = PredictPosition.eY;
src.tCarInfo.eAngle = PredictAngle;
//定位座標轉換為車體座標資訊
CoordinateTransformProcess(src, ref dst);
//更新車體資訊
DeliverData.tAGV_Data.tCarInfo.tPosition.eX = PredictPosition.eX;
DeliverData.tAGV_Data.tCarInfo.tPosition.eY = PredictPosition.eY;
DeliverData.tAGV_Data.tCarInfo.eAngle = PredictAngle;
DeliverData.tAGV_Data.tCarInfo.eCarTireSpeedLeft = GlobalVar.CarTireSpeedLeft; ;
DeliverData.tAGV_Data.tCarInfo.eCarTireSpeedRight = GlobalVar.CarTireSpeedRight;
DeliverData.tAGV_Data.tCarInfo.tCarTirepositionR.eX = dst.tCarInfo.tCarTirepositionR.eX;
DeliverData.tAGV_Data.tCarInfo.tCarTirepositionR.eY = dst.tCarInfo.tCarTirepositionR.eY;
DeliverData.tAGV_Data.tCarInfo.tCarTirepositionL.eX = dst.tCarInfo.tCarTirepositionL.eX;
DeliverData.tAGV_Data.tCarInfo.tCarTirepositionL.eY = dst.tCarInfo.tCarTirepositionL.eY;
DeliverData.tAGV_Data.tCarInfo.tMotorPosition.eX = dst.tCarInfo.tMotorPosition.eX;
DeliverData.tAGV_Data.tCarInfo.tMotorPosition.eY = dst.tCarInfo.tMotorPosition.eY;
/*DeliverData.tAGV_Data.tCarInfo.tPosition.eX = 17050;
DeliverData.tAGV_Data.tCarInfo.tPosition.eY = -950;
DeliverData.tAGV_Data.tCarInfo.eAngle = 90;
/*DeliverData.tAGV_Data.tCarInfo.tMotorPosition.eX = 17223;
DeliverData.tAGV_Data.tCarInfo.tMotorPosition.eY = -3058;*/
//更新車Sensor資訊
DeliverData.tAGV_Data.tCarInfo.eWheelAngle = GlobalVar.RealMotorAngle;
DeliverData.tAGV_Data.tSensorData.tForkInputData.height = (int)GlobalVar.ForkCurrentHeight;
/* DeliverData.tAGV_Data.tSensorData.tForkInputData.height = (int)120;
DeliverData.tAGV_Data.ucAGV_Status = (byte)7;*/
}
/// <summary> Distance Modify For Path Offset </summary>
/// <param name="a_tPathInfo">[IN] path data for navigate </param>
/// <param name="a_tPosition">[IN] Position of car </param>
/// <param name="a_eDistanceOri">[IN] Original Distance </param>
/// <returns> modified diatance </returns>
public double DistanceModifyForPathOffset(rtPath_Info a_tPathInfo, rtVector a_tPosition, double a_eDistanceOri)
{
double eDistance = 0, eTheta = 0, eDistanceD2C = 0, eDistanceProject = 0;
rtVector tVectorD2S = new rtVector();
rtVector tVetorD2C = new rtVector();
rtVector tVetorProject = new rtVector();
tVetorD2C = rtVectorOP_2D.GetVector(a_tPathInfo.tDest, a_tPosition);
tVectorD2S = rtVectorOP_2D.GetVector(a_tPathInfo.tDest, a_tPathInfo.tSrc);
eTheta = rtVectorOP_2D.GetTheta(tVetorD2C, tVectorD2S);
if(Math.Abs(eTheta) < 0.001)
{ // 夾角過小代表在路徑上 >> 不處理
eDistance = a_eDistanceOri;
}
else
{ // 投影至路徑向量上看長度
tVetorProject = rtVectorOP_2D.VectorProject(tVetorD2C, tVectorD2S);
eDistanceProject = rtVectorOP_2D.GetLength(tVetorProject);
eDistanceD2C = rtVectorOP_2D.GetLength(tVetorD2C);
eDistance = a_eDistanceOri - eDistanceD2C + eDistanceProject;
}
return eDistance;
}
