/// <summary> init path with length </summary>
/// <param name="a_lPathLength">[IN] Path's Length</param>
/// <returns> Path struct </returns>
public static rtPath_Info[] InitSet(int a_lPathLength)
{
int lCnt = 0;
rtPath_Info[] atPathInfo = new rtPath_Info[a_lPathLength];
for (lCnt = 0; lCnt < a_lPathLength - 1; lCnt++)
{
atPathInfo[lCnt].tSrc.Init();
atPathInfo[lCnt].tDest.Init();
atPathInfo[lCnt].ucStatus = (byte)rtStatus.STRAIGHT;
atPathInfo[lCnt].ucTurnType = (byte)rtTurnType.SIMPLE;
}
if (a_lPathLength != 0)
{
atPathInfo[lCnt].tSrc.Init();
atPathInfo[lCnt].tDest.Init();
atPathInfo[lCnt].ucStatus = (byte)rtStatus.STRAIGHT;
atPathInfo[lCnt].ucTurnType = (byte)rtTurnType.ARRIVE;
}
return(atPathInfo);
}
/// <summary> Path Planning for AGV navigation </summary>
/// <param name="a_tMap">[IN] map for navigation </param>
/// <param name="a_atWarehousingInfo">[IN] Warehouse Information </param>
/// <param name="a_atRegionCfg">[IN] region configure </param>
/// <param name="a_atPathInfo">[INOUT] Path data </param>
/// <param name="a_tCarInfo">[IN] car data </param>
/// <param name="a_tDestData">[IN] Destination data </param>
/// <param name="a_atObstacle">[IN] Obstacle data </param>
public static void rtAGV_PathPlanning(
rtAGV_MAP a_tMap, rtWarehousingInfo[][] a_atWarehousingInfo, ROI[] a_atRegionCfg,
ref rtPath_Info[] a_atPathInfo, ref rtCarData a_tCarInfo, rtWarehousingInfo a_tDestData, ROI[] a_atObstacle)
{
NodeId tNodeId;
int lNodeNum = 0, lPathLength = 0;
int lRegionIndex = 0;
int[] alPathofRegion = new int[0];
//搜尋最靠近車體位置的節點
tNodeId = rtAGV_FindCurrentNode(a_tMap.atNodeLocal[lRegionIndex], a_tCarInfo);
#if rtAGV_NAVIGATE_MULTI_REGION
if (a_tDestData.tNodeId.lRegion == tNodeId.lRegion)
{ // different region
if(alPathofRegion.Length == 0)
{
// show error
}
while (lRegionIndex < alPathofRegion.Length)
{
FindPathNode2NextRegion();
lRegionIndex++;
}
}
#endif
// already in same region
lRegionIndex = a_tDestData.tNodeId.lRegion;
lNodeNum = a_tMap.alNodeNumLocal[lRegionIndex];
rtAGV_FindPathNode2Dest(
tNodeId.lIndex, a_tDestData.tNodeId.lIndex,
a_tMap.atNodeLocal[lRegionIndex], a_tMap.alPathTableLocal[lRegionIndex], lNodeNum, ref lPathLength, ref a_atPathInfo);
}
/// <summary> check the direction car should turn </summary>
/// <param name="a_atPathInfo">[IN] path data for navigate </param>
/// <param name="a_lPathNodeIndex">[IN] index of current path </param>
/// <returns> CAR_TURN_RIGHT: need turn right CAR_TURN_LEFT: need turn left ERROR: error </returns>
public static int TurnDirectionCal(rtPath_Info[] a_atPathInfo, int a_lPathNodeIndex)
{
int lTurnDirection = 0;
double eAngle = 0, eCross = 0;
rtVector tVd2sCurrent;
rtVector tVs2dNext;
tVd2sCurrent = rtVectorOP_2D.GetVector(a_atPathInfo[a_lPathNodeIndex].tDest, a_atPathInfo[a_lPathNodeIndex].tSrc);
tVs2dNext = rtVectorOP_2D.GetVector(a_atPathInfo[a_lPathNodeIndex+1].tSrc, a_atPathInfo[a_lPathNodeIndex+1].tDest);
eAngle = rtVectorOP_2D.GetTheta(tVd2sCurrent, tVs2dNext);
eCross = rtVectorOP_2D.Cross(tVd2sCurrent, tVs2dNext);
if (eAngle > ANGLE_TH_PATH)
{
if (eCross > 0)
{ // 點在右邊 >> 車子需要向右轉(不管正走或反走)
lTurnDirection = (int)rtTurnType_Simple.CAR_TURN_RIGHT;
}
else
{ // 點在左邊 >> 車子需要向左轉(不管正走或反走)
lTurnDirection = (int)rtTurnType_Simple.CAR_TURN_LEFT;
}
}
else
{ // 不可能轉這麼大
lTurnDirection = (int)rtTurnType_Simple.ERROR;
}
return lTurnDirection;
}
/// <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;
}
/// <summary> power control during Straight mode </summary>
/// <param name="a_atPathInfo">[IN] path data for navigate </param>
/// <param name="a_tCarData">[IN] data of car </param>
public void MotorPower_CtrlNavigateStraight(rtPath_Info[] a_atPathInfo, rtCarData a_tCarData)
{ // 直走狀態
double eErrorCurrent = 0, eDistanceMotor2Dest = 0, eDistanceTurn = 0;
tMotorData.bFinishFlag = false;
tMotorData.bOverDest = false;
tMotorData.bBackWard = BackWardVerify(a_atPathInfo[tMotorData.lPathNodeIndex], a_tCarData.eAngle );
tMotorData.bOverDest = OverDestination(a_atPathInfo, a_tCarData.tPosition, tMotorData.lPathNodeIndex);
if (tMotorData.bOverDest == true)
{ // 判斷超過終點
if (a_atPathInfo[tMotorData.lPathNodeIndex].ucTurnType == (byte)rtPath_Info.rtTurnType.ARRIVE)
{ // 到達最終目的地 >> 停車
tMotorData.StopCar();
a_atPathInfo[tMotorData.lPathNodeIndex].ucStatus = (byte)rtPath_Info.rtStatus.DONE;
tMotorData.bFinishFlag = true;
Console.WriteLine("到達最終目的地 >> 停車");
}
else if (a_atPathInfo[tMotorData.lPathNodeIndex].ucTurnType == (byte)rtPath_Info.rtTurnType.PARK)
{ // 到達最終目的地 >> 停車
tMotorData.StopCar();
a_atPathInfo[tMotorData.lPathNodeIndex].ucStatus = (byte)rtPath_Info.rtStatus.DONE;
tMotorData.bFinishFlag = true;
Console.WriteLine("到達最終目的地 >> 停進車位");
}
else
{ // 趕快進入下一段 (要不要先轉正再說)
a_atPathInfo[tMotorData.lPathNodeIndex].ucStatus = (byte)rtPath_Info.rtStatus.DONE;
tMotorData.lPathNodeIndex++;
// 將旋轉半徑、中心等資料清空
tMotorData.ClearTurnData();
}
}
else
{ // 尚未超過終點
// 算出跟終點距離
eErrorCurrent = Distance2PathDestCal(a_atPathInfo, a_tCarData.tPosition, tMotorData.lPathNodeIndex);
eDistanceMotor2Dest = rtVectorOP_2D.GetDistance(a_atPathInfo[tMotorData.lPathNodeIndex].tDest, a_tCarData.tMotorPosition);
// Motor power = function(Error)
tMotorData.lMotorPower = (int)(tMotorCfg.tPID_PowerCoe.eKp * eErrorCurrent) + MIN_POWER;
if (tMotorData.bBackWard)
{ // 反向走
tMotorData.lMotorPower = -tMotorData.lMotorPower;
}
eDistanceTurn = (tMotorData.bBackWard) ? eDistanceMotor2Dest : eErrorCurrent;
switch (a_atPathInfo[tMotorData.lPathNodeIndex].ucTurnType)
{
case (byte)rtPath_Info.rtTurnType.SIMPLE:
if (eErrorCurrent < DISTANCE_ERROR_SIMPLE)
{
a_atPathInfo[tMotorData.lPathNodeIndex].ucStatus = (byte)rtPath_Info.rtStatus.TURN;
}
break;
case (byte)rtPath_Info.rtTurnType.SMOOTH:
if (eDistanceTurn < rtMotor_Cfg.RADIUS_SMOOTH)
{
// 算出旋轉半徑 & 中心座標
RotationCenterAndRadiusCal(a_atPathInfo, tMotorCfg.lRotationDistance, ref tMotorData);
a_atPathInfo[tMotorData.lPathNodeIndex].ucStatus = (byte)rtPath_Info.rtStatus.TURN;
}
break;
case (byte)rtPath_Info.rtTurnType.ARRIVE:
if (eErrorCurrent < DISTANCE_ERROR_SIMPLE)
{ // 到達最終目的地
tMotorData.StopCar();
a_atPathInfo[tMotorData.lPathNodeIndex].ucStatus = (byte)rtPath_Info.rtStatus.DONE;
tMotorData.bFinishFlag = true;
Console.WriteLine("ARRIVE");
}
break;
case (byte)rtPath_Info.rtTurnType.PARK:
if (eErrorCurrent < DISTANCE_ERROR_SIMPLE)
{ // 到達最終目的地
tMotorData.StopCar();
a_atPathInfo[tMotorData.lPathNodeIndex].ucStatus = (byte)rtPath_Info.rtStatus.DONE;
tMotorData.bFinishFlag = true;
Console.WriteLine("ARRIVE");
}
break;
default:
// show error msg
break;
}
}
tMotorData.eDistance2Dest = eErrorCurrent;
}
/// <summary> Determine Motor Angle during Smooth Turn </summary>
/// <param name="a_atPathInfo">[IN] information of path </param>
/// <param name="a_tCarData">[IN] car data </param>
public void MotorAngle_CtrlNavigateSmoothTurn(rtPath_Info[] a_atPathInfo, rtCarData a_tCarData)
{
double eDistance = 0, eThetaError = 0, eCarAngle = 0, eMotorAngleOffset = 0, eMototAngleTmp = 0;
int lPathIndex = 0;
rtVector tPathVector = new rtVector();
rtVector tPostion = new rtVector();
// 倒退走: 以驅動車輪為路徑偏差基準 往前走: 以兩導輪中心為路徑偏差基準
tPostion = (tMotorData.bBackWard) ? a_tCarData.tMotorPosition : a_tCarData.tPosition;
lPathIndex = tMotorData.lPathNodeIndex;
eCarAngle = a_tCarData.eAngle;
#if rtAGV_BACK_MODE
if (tMotorData.bBackWard)
{ // 倒退走 >> 在某些情況下 要用車尾方向來計算
eCarAngle = (eCarAngle > 180) ? eCarAngle - 180 : eCarAngle + 180;
}
#endif
tPathVector = rtVectorOP_2D.GetVector(a_atPathInfo[lPathIndex].tSrc, a_atPathInfo[lPathIndex].tDest);
// 算出車子要往左轉還是右轉
tMotorData.lTurnDirection = TurnDirectionCal(a_atPathInfo, tMotorData.lPathNodeIndex);
eDistance = PathErrorCal_Turn(a_atPathInfo[lPathIndex], tPostion, tMotorData.tTurnCenter, tMotorData.lTurnRadius);
tPathVector = PathVectorDetermine_TurnMode(tPostion, tMotorData.tTurnCenter, tMotorData.lTurnDirection);
// 用算出的旋轉半徑得知車輪至少要轉幾度 >> 目前僅在正走時才需要 Offset 所以不需考慮反走的種種情況
if (tMotorData.bBackWard == false)
{
eMotorAngleOffset = TargetAngle_Cal(a_tCarData, tMotorData.lTurnRadius);
eMotorAngleOffset = eMotorAngleOffset * tMotorData.lTurnDirection;
}
tMotorData.lTargetAngle = eMotorAngleOffset;
// 算出車身角度和路徑角度偏差多少 (這裡目前僅為了 LOG)
eThetaError = rtAngleDiff.GetAngleDiff(tPathVector, eCarAngle);
tMotorData.Debug_ePathThetaError = eThetaError;
// 考慮靠左 or 靠右的 offset
tMotorData.ePathError = eDistance + tMotorData.lNavigateOffset;
eMototAngleTmp = DedtermineMotorAngleByPathError(tPathVector, a_tCarData);
tMotorData.lMotorAngle = (int)(Math.Round(eMototAngleTmp));
// boundary
if (Math.Abs(tMotorData.lMotorAngle) > MAX_ANGLE_OFFSET_MOTOR)
{
tMotorData.lMotorAngle = (tMotorData.lMotorAngle < 0) ? -MAX_ANGLE_OFFSET_MOTOR : MAX_ANGLE_OFFSET_MOTOR;
}
}
/// <summary> init path with length </summary>
/// <param name="a_lPathLength">[IN] Path's Length</param>
/// <returns> Path struct </returns>
public static rtPath_Info[] InitSet(int a_lPathLength)
{
int lCnt = 0;
rtPath_Info[] atPathInfo = new rtPath_Info[a_lPathLength];
for(lCnt = 0; lCnt< a_lPathLength - 1; lCnt++)
{
atPathInfo[lCnt].tSrc.Init();
atPathInfo[lCnt].tDest.Init();
atPathInfo[lCnt].ucStatus = (byte)rtStatus.STRAIGHT;
atPathInfo[lCnt].ucTurnType = (byte)rtTurnType.SIMPLE;
}
if (a_lPathLength != 0)
{
atPathInfo[lCnt].tSrc.Init();
atPathInfo[lCnt].tDest.Init();
atPathInfo[lCnt].ucStatus = (byte)rtStatus.STRAIGHT;
atPathInfo[lCnt].ucTurnType = (byte)rtTurnType.ARRIVE;
}
return atPathInfo;
}
/// <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> check if car over destination </summary>
/// <param name="a_atPathInfo">[IN] entire path data for navigate </param>
/// <param name="a_tPosition">[IN] Position of car </param>
/// <param name="a_lPathNodeIndex">[IN] index of ccurrent path </param>
/// <returns> true: yes false: no </returns>
public static bool OverDestination(rtPath_Info[] a_atPathInfo, rtVector a_tPosition, int a_lPathNodeIndex)
{
double eTheta = 0;
rtVector tV_C2D; // current point to destination
rtVector tV_S2D; // source point to destination
tV_S2D.eX = a_atPathInfo[a_lPathNodeIndex].tDest.eX - a_atPathInfo[a_lPathNodeIndex].tSrc.eX;
tV_S2D.eY = a_atPathInfo[a_lPathNodeIndex].tDest.eY - a_atPathInfo[a_lPathNodeIndex].tSrc.eY;
tV_C2D.eX = a_atPathInfo[a_lPathNodeIndex].tDest.eX - a_tPosition.eX;
tV_C2D.eY = a_atPathInfo[a_lPathNodeIndex].tDest.eY - a_tPosition.eY;
eTheta = rtVectorOP_2D.GetTheta(tV_S2D, tV_C2D);
// 判斷是否已超終點
if (eTheta >= ANGLE_TH)
{ // 超過終點 >> 必須反向行走
return true;
}
return false;
}
/// <summary> check if car need to BackWard </summary>
/// <param name="a_atPathInfo">[IN] entire path data for navigate </param>
/// <param name="a_eCarAngle">[IN] direction of car </param>
/// <returns> true: need false: do not need </returns>
public static bool BackWardVerify(rtPath_Info a_tPathInfo, double a_eCarAngle)
{
double eDeltaAngle = 0;
rtVector tV_S2D; // source point to destination
// 算出當前路徑向量
tV_S2D = rtVectorOP_2D.GetVector(a_tPathInfo.tSrc, a_tPathInfo.tDest);
// 算出車身向量與路徑向量的夾角
eDeltaAngle = rtAngleDiff.GetAngleDiff(tV_S2D, a_eCarAngle);
// 判斷是否該倒退走比較合適
if (Math.Abs(eDeltaAngle) >= ANGLE_TH)
{ // 角度過大 適合倒退走
return true;
}
return false;
}
/// <summary> 判斷從線開始到終點是否屬於一直線 (中途是否有過大的轉彎) </summary>
/// <param name="a_atPathInfo">[IN] entire path data for navigate </param>
/// <param name="a_lPathNodeIndex">[IN] index of current path </param>
/// <returns> true: 沒過大的轉彎 false: 中間會有大轉彎 </returns>
public static bool Link2DestCheck(rtPath_Info[] a_atPathInfo, int a_lPathNodeIndex)
{
rtVector tV_C2D; // current point to destination
rtVector tV_C2D_Last; // current point to destination
int lPathLength = 0;
int lNodeIndex = 0;
double ePathTheta = 0;
lPathLength = a_atPathInfo.Length;
for (lNodeIndex = a_lPathNodeIndex + 1; lNodeIndex < lPathLength; lNodeIndex++)
{
tV_C2D.eX = a_atPathInfo[lNodeIndex].tDest.eX - a_atPathInfo[lNodeIndex].tSrc.eX;
tV_C2D.eY = a_atPathInfo[lNodeIndex].tDest.eY - a_atPathInfo[lNodeIndex].tSrc.eY;
tV_C2D_Last.eX = a_atPathInfo[lNodeIndex - 1].tDest.eX - a_atPathInfo[lNodeIndex - 1].tSrc.eX;
tV_C2D_Last.eY = a_atPathInfo[lNodeIndex - 1].tDest.eY - a_atPathInfo[lNodeIndex - 1].tSrc.eY;
ePathTheta = rtVectorOP_2D.GetTheta(tV_C2D_Last, tV_C2D);
if (ePathTheta >= THETA_ERROR_TURN)
{
return false;
}
}
return true;
}
/// <summary> calculate distance to the destination </summary>
/// <param name="a_atPathInfo">[IN] entire path data for navigate </param>
/// <param name="a_tPosition">[IN] current car position </param>
/// <param name="a_lPathNodeIndex">[IN] index of current path </param>
/// <returns> distance to the destination </returns>
public static double Distance2PathDestCal(rtPath_Info[] a_atPathInfo, rtVector a_tPosition, int a_lPathNodeIndex)
{
double eErrorCurrent = 0, eTheta = 0;
rtVector tV_C2D; // current point to destination
rtVector tVectorD2S = new rtVector();
rtVector tVetorProject = new rtVector();
#if rtAGV_POWER_SMOOTH
rtVector tV_C2D_Last; // current point to destination
int lPathLength = 0;
int lNodeIndex = 0;
double ePathTheta = 0;
#endif
tV_C2D = rtVectorOP_2D.GetVector(a_tPosition, a_atPathInfo[a_lPathNodeIndex].tDest);
tVectorD2S = rtVectorOP_2D.GetVector(a_atPathInfo[a_lPathNodeIndex].tDest, a_atPathInfo[a_lPathNodeIndex].tSrc);
eTheta = 180 - rtVectorOP_2D.GetTheta(tV_C2D, tVectorD2S);
eErrorCurrent = rtVectorOP_2D.GetLength(tV_C2D);
if (eTheta > 0.1)
{ // 角度夠大 >> 投影至路徑向量上看長度
tVetorProject = rtVectorOP_2D.VectorProject(tV_C2D, tVectorD2S);
eErrorCurrent = rtVectorOP_2D.GetLength(tVetorProject);
}
#if rtAGV_POWER_SMOOTH
lPathLength = a_atPathInfo.Length;
for (lNodeIndex = a_lPathNodeIndex + 1; lNodeIndex < lPathLength; lNodeIndex++)
{
tV_C2D = rtVectorOP_2D.GetVector(a_atPathInfo[lNodeIndex].tSrc, a_atPathInfo[lNodeIndex].tDest);
tV_C2D_Last = rtVectorOP_2D.GetVector(a_atPathInfo[lNodeIndex - 1].tSrc, a_atPathInfo[lNodeIndex - 1].tDest);
ePathTheta = rtVectorOP_2D.GetTheta(tV_C2D_Last, tV_C2D);
if (ePathTheta < THETA_ERROR_TURN)
{
eErrorCurrent += rtVectorOP_2D.GetLength(tV_C2D);
}
else
{
break;
}
}
#endif
return eErrorCurrent;
}
/// <summary> distance error calculate during Straight mode </summary>
/// <param name="a_atPathInfo">[IN] entire path data for navigate </param>
/// <param name="a_tPosition">[IN] current car position </param>
/// <param name="a_lPathNodeIndex">[IN] index of current path </param>
/// <returns> distance error </returns>
public static double MotorPower_StraightErrorCal(rtPath_Info[] a_atPathInfo, rtVector a_tPosition, int a_lPathNodeIndex)
{
double eErrorCurrent = 0;
rtVector tV_C2D; // current point to destination
#if rtAGV_POWER_SMOOTH
rtVector tV_C2D_Last; // current point to destination
int lPathLength = 0;
int lNodeIndex = 0;
double ePathTheta = 0;
#endif
tV_C2D.eX = a_atPathInfo[a_lPathNodeIndex].tDest.eX - a_tPosition.eX;
tV_C2D.eY = a_atPathInfo[a_lPathNodeIndex].tDest.eY - a_tPosition.eY;
eErrorCurrent = rtVectorOP_2D.GetLength(tV_C2D);
#if rtAGV_POWER_SMOOTH
lPathLength = a_atPathInfo.Length;
for (lNodeIndex = a_lPathNodeIndex+1; lNodeIndex < lPathLength; lNodeIndex++)
{
tV_C2D.eX = a_atPathInfo[lNodeIndex].tDest.eX - a_atPathInfo[lNodeIndex].tSrc.eX;
tV_C2D.eY = a_atPathInfo[lNodeIndex].tDest.eY - a_atPathInfo[lNodeIndex].tSrc.eY;
tV_C2D_Last.eX = a_atPathInfo[lNodeIndex - 1].tDest.eX - a_atPathInfo[lNodeIndex - 1].tSrc.eX;
tV_C2D_Last.eY = a_atPathInfo[lNodeIndex - 1].tDest.eY - a_atPathInfo[lNodeIndex - 1].tSrc.eY;
ePathTheta = rtVectorOP_2D.GetTheta(tV_C2D_Last, tV_C2D);
if (ePathTheta< THETA_ERROR_TURN)
{
eErrorCurrent += rtVectorOP_2D.GetLength(tV_C2D);
}
else
{
break;
}
}
#endif
return eErrorCurrent;
}
/// <summary> Determine Motor Angle Straight mode </summary>
/// <param name="a_atPathInfo">[IN] information of path </param>
/// <param name="a_tCarData">[IN] car data </param>
public void MotorAngle_CtrlNavigateStraight(rtPath_Info[] a_atPathInfo, rtCarData a_tCarData)
{
double eDistance = 0, eThetaError = 0, eCarAngle = 0, eMotorAngleOffset = 0, eMototAngleTmp = 0;
int lPathIndex = 0;
rtVector tPathVector = new rtVector();
rtVector tPostion = new rtVector();
// 倒退走: 以驅動車輪為路徑偏差基準 往前走: 以兩導輪中心為路徑偏差基準
tPostion = (tMotorData.bBackWard) ? a_tCarData.tMotorPosition : a_tCarData.tPosition;
lPathIndex = tMotorData.lPathNodeIndex;
eCarAngle = a_tCarData.eAngle;
#if rtAGV_BACK_MODE
if (tMotorData.bBackWard)
{ // 倒退走 >> 在某些情況下 要用車尾方向來計算
eCarAngle = (eCarAngle > 180) ? eCarAngle - 180 : eCarAngle + 180;
}
#endif
tPathVector = rtVectorOP_2D.GetVector(a_atPathInfo[lPathIndex].tSrc, a_atPathInfo[lPathIndex].tDest);
eDistance = PathErrorCal_Straight(a_atPathInfo[lPathIndex], tPostion);
tMotorData.lTargetAngle = eMotorAngleOffset;
// 算出車身角度和路徑角度偏差多少 (這裡目前僅為了 LOG)
eThetaError = rtAngleDiff.GetAngleDiff(tPathVector, eCarAngle);
tMotorData.Debug_ePathThetaError = eThetaError;
// 考慮靠左 or 靠右的 offset
tMotorData.ePathError = eDistance + tMotorData.lNavigateOffset;
eMototAngleTmp = DedtermineMotorAngleByPathError(tPathVector, a_tCarData);
tMotorData.lMotorAngle = (int)(Math.Round(eMototAngleTmp));
// boundary
if (Math.Abs(tMotorData.lMotorAngle) > MAX_ANGLE_OFFSET_MOTOR)
{
tMotorData.lMotorAngle = (tMotorData.lMotorAngle < 0) ? -MAX_ANGLE_OFFSET_MOTOR : MAX_ANGLE_OFFSET_MOTOR;
}
}
/// <summary> Calculate Rotation Center And Radius for turn </summary>
/// <param name="a_atPathInfo">[IN] path data for navigate </param>
/// <param name="a_lRotationDistance">[IN] Rotation Distance </param>
/// <param name="a_tMotorData">[INOUT] motor output data </param>
public static void RotationCenterAndRadiusCal(rtPath_Info[] a_atPathInfo, int a_lRotationDistance, ref rtMotor_Data a_tMotorData)
{
int lPathIndex = 0;
rtVector tVd2sCurrent, tVs2dNext, tVd2sCurrentLaw, tVs2dNextLaw, tTurnStart, tTurnEnd;
lPathIndex = a_tMotorData.lPathNodeIndex;
// set current vector
tVd2sCurrent = rtVectorOP_2D.GetVector(a_atPathInfo[lPathIndex].tDest, a_atPathInfo[lPathIndex].tSrc);
// 取右側法向量
tVd2sCurrentLaw.eX = tVd2sCurrent.eY;
tVd2sCurrentLaw.eY = -tVd2sCurrent.eX;
// 取轉彎起始點
tTurnStart = rtVectorOP_2D.ExtendPointAlongVector(a_atPathInfo[lPathIndex].tDest, tVd2sCurrent, a_lRotationDistance);
// set next vector
tVs2dNext = rtVectorOP_2D.GetVector(a_atPathInfo[lPathIndex + 1].tSrc, a_atPathInfo[lPathIndex + 1].tDest);
// 取右側法向量
tVs2dNextLaw.eX = tVs2dNext.eY;
tVs2dNextLaw.eY = -tVs2dNext.eX;
// 取轉彎結束點
tTurnEnd = rtVectorOP_2D.ExtendPointAlongVector(a_atPathInfo[lPathIndex + 1].tSrc, tVs2dNext, a_lRotationDistance);
// 取兩條線交點當旋轉中心座標
a_tMotorData.tTurnCenter = rtVectorOP_2D.FindMeetPoint(tTurnStart, tVd2sCurrentLaw, tTurnEnd, tVs2dNextLaw);
// 計算旋轉半徑 (轉彎路徑用)
a_tMotorData.lTurnRadius = (int)Math.Round(rtVectorOP_2D.GetDistance(tTurnStart, a_tMotorData.tTurnCenter));
}
/// <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> motor power calculate during Turn mode </summary>
/// <param name="a_tPathInfo">[IN] path data for navigate </param>
/// <param name="a_eCarAngle">[IN] direction of car </param>
/// <returns> power value </returns>
public static double MotorPower_TurnErrorCal(rtPath_Info a_tPathInfo, double a_eCarAngle)
{
double eTheta = 0;
rtVector tV_Car; // car current direction
rtVector tV_NextS2D; // next src point to destination
tV_Car.eX = Math.Cos(a_eCarAngle * Math.PI / 180);
tV_Car.eY = Math.Sin(a_eCarAngle * Math.PI / 180);
tV_NextS2D.eX = a_tPathInfo.tDest.eX - a_tPathInfo.tSrc.eX;
tV_NextS2D.eY = a_tPathInfo.tDest.eY - a_tPathInfo.tSrc.eY;
eTheta = rtVectorOP_2D.GetTheta(tV_Car, tV_NextS2D);
return eTheta;
}
/// <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> 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;
}
/// <summary> Find Node list of Path to Destination </summary>
/// <param name="a_lSrc">[IN] source node </param>
/// <param name="a_lDst">[IN] Destination node </param>
/// <param name="a_atNodeLocal">[IN] node information in current region </param>
/// <param name="a_alMapCurrent">[IN] look up table of node in current region </param>
/// <param name="a_lNodeNum">[IN] node number in current region </param>
/// <param name="a_lPathLength">[INOUT] wanted path size </param>
/// <param name="a_atPathInfo">[INOUT] wanted path </param>
public static void rtAGV_FindPathNode2Dest(int a_lSrc, int a_lDst, rtAGV_MAP_node[] a_atNodeLocal, int[] a_alMapCurrent, int a_lNodeNum, ref int a_lPathLength, ref rtPath_Info[] a_atPathInfo)
{
int lCnt = 0;
int[] alPathResult = new int[0];
alPathResult = FindPathofNode(a_lSrc, a_lDst, a_alMapCurrent, a_lNodeNum, ref a_lPathLength);
a_lPathLength--; // 路徑段數比節點數少1
a_atPathInfo = rtPath_Info.InitSet(a_lPathLength);
for (lCnt = 0; lCnt < a_lPathLength; lCnt++)
{
a_atPathInfo[lCnt].tSrc.eX = a_atNodeLocal[alPathResult[lCnt]].tCoordinate.eX;
a_atPathInfo[lCnt].tSrc.eY = a_atNodeLocal[alPathResult[lCnt]].tCoordinate.eY;
a_atPathInfo[lCnt].tDest.eX = a_atNodeLocal[alPathResult[lCnt+1]].tCoordinate.eX;
a_atPathInfo[lCnt].tDest.eY = a_atNodeLocal[alPathResult[lCnt+1]].tCoordinate.eY;
}
}
/// <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> power control during Smooth Turn mode </summary>
/// <param name="a_atPathInfo">[IN] path data for navigate </param>
/// <param name="a_tCarData">[IN] data of car </param>
public void MotorPower_CtrlNavigateSmoothTurn(rtPath_Info[] a_atPathInfo, rtCarData a_tCarData)
{ // 轉彎狀態
bool bOutFlag = false;
double eErrorCurrent = 0;
tMotorData.bBackWard = BackWardVerify(a_atPathInfo[tMotorData.lPathNodeIndex+1], a_tCarData.eAngle);
// 用車身與下一段路徑的夾角當power誤差
eErrorCurrent = MotorPower_TurnErrorCal(a_atPathInfo[tMotorData.lPathNodeIndex + 1], a_tCarData.eAngle);
tMotorData.lMotorPower = TURN_POWER;
if (tMotorData.bBackWard)
{ // 反向走 >> 所以車身向量會差180度 & power會取負號
eErrorCurrent = 180 - eErrorCurrent;
tMotorData.lMotorPower = -tMotorData.lMotorPower;
}
// 判斷是否已經走完轉彎的扇形區
if (tMotorData.bBackWard)
{
bOutFlag = FinishSmoothTurnCheck(
tMotorData.lPathNodeIndex, tMotorCfg.lRotationDistance,
a_atPathInfo, a_tCarData.tMotorPosition, tMotorData.tTurnCenter);
}
else
{
bOutFlag = FinishSmoothTurnCheck(
tMotorData.lPathNodeIndex, tMotorCfg.lRotationDistance,
a_atPathInfo, a_tCarData.tPosition, tMotorData.tTurnCenter);
}
if (eErrorCurrent < THETA_ERROR_TURN || bOutFlag == true)
{
a_atPathInfo[tMotorData.lPathNodeIndex].ucStatus = (byte)rtPath_Info.rtStatus.DONE;
tMotorData.lPathNodeIndex++;
}
}
/// <summary> Path Offset Calculate </summary>
/// <param name="a_atPathInfo">[IN] information of path </param>
/// <param name="a_eDestDirection">[IN] Destination Direction </param>
public void PathOffsetCal(rtPath_Info[] a_atPathInfo, double a_eDestDirection)
{
int lPathIndex = 0;
double eTmp = 0, eDeltaTmp = 0;
rtVector tDestVector = new rtVector();
rtVector tPathVector = new rtVector();
lPathIndex = tMotorData.lPathNodeIndex;
tPathVector.eX = a_atPathInfo[lPathIndex].tDest.eX - a_atPathInfo[lPathIndex].tSrc.eX;
tPathVector.eY = a_atPathInfo[lPathIndex].tDest.eY - a_atPathInfo[lPathIndex].tSrc.eY;
tDestVector = rtVectorOP_2D.Angle2Vector(a_eDestDirection);
eDeltaTmp = rtVectorOP_2D.GetTheta(tPathVector, tDestVector);
if (Link2DestCheck(a_atPathInfo, lPathIndex) && eDeltaTmp > DELTA_ANGLE_TH)
{ // need offset
eTmp = rtVectorOP_2D.Cross(tPathVector, tDestVector);
if (eTmp > 0)
{ // 車子要往左轉 (不管前進或到退走) >> offset 為負
tMotorData.lNavigateOffset = DEFAULT_PATH_OFFSET;
}
else
{ // 車子要往右轉 (不管前進或到退走) >> offset 為正
tMotorData.lNavigateOffset = -DEFAULT_PATH_OFFSET;
}
}
else
{ // do not need offset
tMotorData.lNavigateOffset = 0;
}
}
