/// <summary> Path Planning for AGV navigation </summary> /// <param name="a_tRegionData">[IN] current region data 當前區域數據 </param> /// <param name="a_lWarehousIndex">[IN] current Warehouse index 當前櫃位數據 </param> /// <param name="a_atPathInfo">[INOUT] Path data 路徑數據 </param> /// <param name="a_tCarInfo">[IN] car data 車子數據 </param> /// <param name="a_atObstacle">[IN] Obstacle data 障礙數據 </param> public static void rtAGV_PathPlanning( RegionOfMap a_tRegionData, int a_lWarehousIndex, ref rtPath_Info[] a_atPathInfo, ref rtCarData a_tCarInfo, ROI[] a_atObstacle) { NodeId tNodeId; int lNodeNum = 0, lPathLength = 0; int[] alPathofRegion = new int[0]; //搜尋最靠近車體位置的節點 tNodeId = rtAGV_FindCurrentNode(a_tRegionData.atNode, 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(確保在同一個區域) lNodeNum = a_tRegionData.lNodeNum; rtAGV_FindPathNode2Dest( tNodeId.lIndex, a_lWarehousIndex, a_tRegionData.atNode, a_tRegionData.alPathTableNode, lNodeNum, ref lPathLength, ref a_atPathInfo); }
/// <summary> 搜尋最靠近車體位置的節點 </summary> /// <param name="a_atNodeLocal">[IN] node list </param> /// <param name="a_tCarInfo">[IN] car information </param> /// <returns> The closest node </returns> public static NodeId rtAGV_FindCurrentNode(rtAGV_MAP_node[] a_atNodeLocal, rtCarData a_tCarInfo) { NodeId tNodeId = new NodeId(); double MinDistance = EMPTY_DATA; for (int i = 0; i < a_atNodeLocal.Length; i++) { double EachDistance = rtVectorOP_2D.GetDistance(a_atNodeLocal[i].tCoordinate, a_tCarInfo.tPosition); if (EachDistance <= MinDistance) { MinDistance = EachDistance; tNodeId = a_atNodeLocal[i].tNodeId; } } return tNodeId; }
/// <summary> 搜尋最靠近車體位置的節點 </summary> /// <param name="a_atNode">[IN] node list 節點列表 </param> /// <param name="a_tCarInfo">[IN] car information 車子數據 </param> /// <returns> The closest node </returns> public static NodeId rtAGV_FindCurrentNode(rtNode[] a_atNode, rtCarData a_tCarInfo) { NodeId tNodeId = new NodeId(); double MinDistance = EMPTY_DATA; for (int i = 0; i < a_atNode.Length; i++) { double EachDistance = rtVectorOP_2D.GetDistance(a_atNode[i].tCoordinate, a_tCarInfo.tPosition); if (EachDistance <= MinDistance) { MinDistance = EachDistance; tNodeId.lIndex = i; } } return(tNodeId); }
/// <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> calculate Car Center Speed </summary> /// <param name="a_tCarData">[IN] data for car </param> /// <returns> Car Center Speed </returns> public static double CarCenterSpeedCal(rtCarData a_tCarData) { double eSpeed = 0, eTheta = 0, eT = 0; double eLength_C2M = 0; // 兩輪中心到後馬達的距離 double eLength_C2O = 0; // 兩輪中心到旋轉中心的距離 = 旋轉半徑 double eLength_R2O = 0; // 右輪中心到旋轉中心的距離 double eLength_L2O = 0; // 右輪中心到旋轉中心的距離 rtVector tV_Car, tVlaw, tRotateCenter; eTheta = Math.Abs(a_tCarData.eWheelAngle); eLength_C2M = rtVectorOP_2D.GetDistance(a_tCarData.tPosition, a_tCarData.tMotorPosition); eLength_C2O = Math.Tan((90 - eTheta) * Math.PI / 180) * eLength_C2M; tV_Car.eX = Math.Cos(a_tCarData.eAngle * Math.PI / 180); tV_Car.eY = Math.Sin(a_tCarData.eAngle * Math.PI / 180); // 取右側的法向量 tVlaw.eX = tV_Car.eY; tVlaw.eY = -tV_Car.eX; eT = Math.Sqrt(eLength_C2O * eLength_C2O / (tVlaw.eX * tVlaw.eX + tVlaw.eY * tVlaw.eY)); if (a_tCarData.eWheelAngle >= 0) { eT = -eT; } tRotateCenter.eX = a_tCarData.tPosition.eX + tVlaw.eX * eT; tRotateCenter.eY = a_tCarData.tPosition.eY + tVlaw.eY * eT; eLength_R2O = rtVectorOP_2D.GetDistance(a_tCarData.tCarTirepositionR, tRotateCenter); eLength_L2O = rtVectorOP_2D.GetDistance(a_tCarData.tCarTirepositionL, tRotateCenter); if (eTheta > ANGLE_TH_MOTION_PREDICT) { if (a_tCarData.eWheelAngle < 0) { // 車子往右轉 輪子角度為負 eSpeed = Math.Abs(a_tCarData.eCarTireSpeedLeft) * eLength_C2O / eLength_L2O; } else { // 車子往左轉 輪子角度為正 eSpeed = Math.Abs(a_tCarData.eCarTireSpeedRight) * eLength_C2O / eLength_R2O; } } else { // 當作直行 eSpeed = (a_tCarData.eCarTireSpeedLeft + a_tCarData.eCarTireSpeedRight) / 2; } return eSpeed; }
/// <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> 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> Motion Predict by coordinate rotation on 2D plan </summary> /// <param name="a_CurrentPos">[IN] data for car </param> /// <param name="a_tCarData">[IN] car data </param> /// <param name="a_CMotorInfo">[IN] motor control class </param> /// <param name="a_eDeltaTime">[IN] the time interval </param> /// <returns> predicted coordinate </returns> public static rtVector Coordinate_Predict(rtVector a_CurrentPos, rtCarData a_tCarData, rtMotorCtrl a_CMotorInfo, double a_eDeltaTime) { double eDistance = 0, eAngle = 0, eTheta = 0, eSpeed = 0, eT = 0, ePhi = 0, ePhiRad = 0; double eLength_C2M = 0; // 兩輪中心到後馬達的距離 double eLength_C2O = 0; // 兩輪中心到旋轉中心的距離 = 旋轉半徑 double eLength_R2O = 0; // 右輪中心到旋轉中心的距離 double eLength_L2O = 0; // 右輪中心到旋轉中心的距離 rtVector tNextPosition = new rtVector(); rtVector tV_Car, tVlaw, tRotateCenter; eAngle = a_tCarData.eAngle; tV_Car.eX = Math.Cos(eAngle * Math.PI / 180); tV_Car.eY = Math.Sin(eAngle * Math.PI / 180); // 取右側的法向量 tVlaw.eX = tV_Car.eY; tVlaw.eY = -tV_Car.eX; eTheta = Math.Abs(a_tCarData.eWheelAngle); eLength_C2M = rtVectorOP_2D.GetDistance(a_tCarData.tPosition, a_tCarData.tMotorPosition); eLength_C2O = Math.Tan((90 - eTheta) * Math.PI / 180) * eLength_C2M; if (eTheta > ANGLE_TH_MOTION_PREDICT) { // 用車模型預測 (對圓心旋轉) eT = Math.Sqrt(eLength_C2O * eLength_C2O / (tVlaw.eX * tVlaw.eX + tVlaw.eY * tVlaw.eY)); if (a_tCarData.eWheelAngle >= 0) { eT = -eT; } tRotateCenter.eX = a_tCarData.tPosition.eX + tVlaw.eX * eT; tRotateCenter.eY = a_tCarData.tPosition.eY + tVlaw.eY * eT; a_CMotorInfo.tMotorData.tPredictRotationCenter.eX = tRotateCenter.eX; a_CMotorInfo.tMotorData.tPredictRotationCenter.eY = tRotateCenter.eY; eLength_R2O = rtVectorOP_2D.GetDistance(a_tCarData.tCarTirepositionR, tRotateCenter); eLength_L2O = rtVectorOP_2D.GetDistance(a_tCarData.tCarTirepositionL, tRotateCenter); if (a_tCarData.eCarTireSpeedLeft > a_tCarData.eCarTireSpeedRight || a_tCarData.eWheelAngle < 0) { // 往右轉 eSpeed = Math.Abs(a_tCarData.eCarTireSpeedLeft) * eLength_C2O / eLength_L2O; } else { // 往左轉 eSpeed = Math.Abs(a_tCarData.eCarTireSpeedRight) * eLength_C2O / eLength_R2O; } eDistance = eSpeed * a_eDeltaTime; // distance = V x T = 所旋轉的弧長 ePhi = eDistance / eLength_C2O; // 這裡單位是徑度 >> 旋轉角度 = 弧長 / 旋轉半徑 if (eT >= 0) { // 旋轉中心在右邊 >> 旋轉角度要取負值 ePhi = -ePhi; } if (a_CMotorInfo.tMotorData.lMotorPower < 0) { // 馬達反轉 角度也要取負號 ePhi = -ePhi; } ePhiRad = ePhi * 180 / Math.PI; tNextPosition = rtVectorOP_2D.Rotate(a_CurrentPos, tRotateCenter, ePhi); } else { // 直行模式 ePhi = 0; ePhiRad = ePhi * 180 / Math.PI; eSpeed = (a_tCarData.eCarTireSpeedLeft + a_tCarData.eCarTireSpeedRight) / 2; eDistance = eSpeed * (1 / FREQUENCY); // distance = V x T if (a_CMotorInfo.tMotorData.lMotorPower >= 0) { tNextPosition.eX = a_CurrentPos.eX + eDistance * tV_Car.eX; tNextPosition.eY = a_CurrentPos.eY + eDistance * tV_Car.eY; } else { tNextPosition.eX = a_CurrentPos.eX - eDistance * tV_Car.eX; tNextPosition.eY = a_CurrentPos.eY - eDistance * tV_Car.eY; } } a_CMotorInfo.tMotorData.eDeltaAngle = ePhiRad; return tNextPosition; }
/// <summary> test function for coordinate predict </summary> /// <param name="a_tCarData">[IN] car data information </param> /// <param name="a_CMotorInfo">[IN] motor control class </param> public static void Test_Predict(rtCarData a_tCarData, ref rtMotorCtrl a_CMotorInfo) { if (a_CMotorInfo.lCntTest > 0) { a_CMotorInfo.ePredictErrorTest = rtVectorOP_2D.GetDistance(a_tCarData.tPosition, a_CMotorInfo.tNextPositionTest); } else { a_CMotorInfo.ePredictErrorTest = 0; } a_CMotorInfo.tNextPositionTest = Coordinate_Predict(a_tCarData.tPosition, a_tCarData, a_CMotorInfo, (1 / FREQUENCY)); a_CMotorInfo.lCntTest++; }
/// <summary> Determine Motor Angle By Path Error </summary> /// <param name="a_tPathVector">[IN] Path Vector </param> /// <param name="a_tCarData">[IN] car data </param> /// <returns> Motor Angle </returns> public double DedtermineMotorAngleByPathError(rtVector a_tPathVector, rtCarData a_tCarData) { double eMototAngle = 0, eTargetCarAngleOffset = 0, eCarCenterSpeed = 0, eDeltaCarAngle = 0, eCarAngle = 0; rtVector tTargetCarVector = new rtVector(); #if rtAGV_BACK_MODE eCarAngle = a_tCarData.eAngle; if (tMotorData.bBackWard) { // 倒退走 >> 在某些情況下 要用車尾方向來計算 eCarAngle = (eCarAngle > 180) ? eCarAngle - 180 : eCarAngle + 180; } #endif // 算出要跟路徑的夾角 eTargetCarAngleOffset = PathAngleOffsetCal(tMotorData.ePathError, tMotorCfg.tPID_ThetaOffsetCoe); tMotorData.Debug_TargetAngleOffset1 = eTargetCarAngleOffset; // 算出兩輪中心的速度 (以考慮正走反走的狀況: 車速的正負號) eCarCenterSpeed = CarCenterSpeedCal(a_tCarData); tMotorData.Debug_CenterSpeed = eCarCenterSpeed; #if rtAGV_DEBUG_OFFSET_MODIFY // 根據車速調整跟路徑的夾角 eTargetCarAngleOffset = TargetAngleOffsetModify(tMotorData.ePathError, eCarCenterSpeed, eTargetCarAngleOffset); #endif tMotorData.Debug_TargetAngleOffset2 = eTargetCarAngleOffset; // 算出目標車身角度 (vector format) tTargetCarVector = rtVectorOP_2D.Rotate(a_tPathVector, new rtVector(0, 0), eTargetCarAngleOffset * Math.PI / 180); // 算出目標車身角度與當下車身角度的差距 eDeltaCarAngle = rtAngleDiff.GetAngleDiff(tTargetCarVector, eCarAngle); #if rtAGV_BACK_MODE if (tMotorData.bBackWard) { // 倒退走 >> 同樣角度差距下 正走跟反走 車輪要打的方向相反 eDeltaCarAngle = -eDeltaCarAngle; } #endif tMotorData.Debug_eDeltaCarAngle = eDeltaCarAngle; // 用角度差距算出 適當的車輪馬達轉角 eMototAngle = MotorAngleCal(eDeltaCarAngle, 0, tMotorCfg.tPID_MotorAngleCoe); // 加上之前的角度 offset eMototAngle = eMototAngle + tMotorData.lTargetAngle; return eMototAngle; }
/// <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> 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> Car direction Alignment </summary> /// <param name="a_eTargetAngle">[IN] Target Angle of car </param> /// <param name="a_tCarData">[IN] car data </param> /// <returns> true: finish false: not yet </returns> public bool CarAngleAlignment(double a_eTargetAngle, rtCarData a_tCarData) { bool bMatched = false; double eAngleError = 0; double eAngleDelay = 0; // 確認輪胎有轉到90度 eAngleError = rtAngleDiff.GetAngleDiff(a_eTargetAngle, a_tCarData.eAngle); if (Math.Abs(eAngleError) <= ANGLE_MATCH_TH || bAlignmentCarAngleMatch) { bAlignmentCarAngleMatch = true; // 代表已轉正 >>正在迴正輪胎 tMotorData.lMotorPower = 0; tMotorData.lMotorAngle = 0; // 車輪轉回0度才結束 if (Math.Abs(a_tCarData.eWheelAngle) < ANGLE_MATCH_TH) { bAlignmentCarAngleMatch = false; // 結束時要復原這個旗標 or not bMatched = true; } else { bMatched = false; } } else { if(eAngleError > 0) { tMotorData.lMotorAngle = ANGLE_ROTATION; } else { tMotorData.lMotorAngle = -ANGLE_ROTATION; } eAngleDelay = tMotorData.lMotorAngle - a_tCarData.eWheelAngle; if(Math.Abs(eAngleDelay) < ANGLE_MATCH_TH) { if (Math.Abs(eAngleError) > ALIGNMENT_SPEED_ANGLE_TH) { tMotorData.lMotorPower = TURN_POWER; } else { tMotorData.lMotorPower = MIN_POWER; } } else { tMotorData.lMotorPower = 0; } bMatched = false; } return bMatched; }
/// <summary> Target Angle Calculation </summary> /// <param name="a_tCarData">[IN] car data </param> /// <param name="a_lTurnRadius">[IN] Turn Radius </param> /// <returns> Target Angle of car </returns> public static double TargetAngle_Cal(rtCarData a_tCarData, int a_lTurnRadius) { double eTargetAngle = 0, eLengthM2C = 0, eTanTheta = 0; eLengthM2C = rtVectorOP_2D.GetDistance(a_tCarData.tMotorPosition, a_tCarData.tPosition); eTanTheta = a_lTurnRadius / eLengthM2C; eTargetAngle = Math.Atan(eTanTheta); eTargetAngle = 90 - (eTargetAngle * 180 / Math.PI); return eTargetAngle; }
/// <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; }
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); } }
/// <summary> Motion Predict of coordinate </summary> /// <param name="a_tCarData">[IN] data for car </param> /// <param name="a_CMotorInfo">[IN] motor control class </param> /// <param name="a_eDeltaTime">[IN] the time interval </param> /// <param name="a_tCarCoordinate">[OUT] predicted coordinate </param> /// <param name="a_eCarAngle">[IN] car direction </param> public static void Motion_Predict(rtCarData a_tCarData, rtMotorCtrl a_CMotorInfo, double a_eDeltaTime, out rtVector a_tCarCoordinate, out double a_eCarAngle) { a_eCarAngle = 0; a_tCarCoordinate = new rtVector(); a_tCarCoordinate = Coordinate_Predict(a_tCarData.tPosition, a_tCarData, a_CMotorInfo, a_eDeltaTime); a_eCarAngle = a_tCarData.eAngle + a_CMotorInfo.tMotorData.eDeltaAngle; }