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