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