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);
}
}
private void DiffTimeForAlignment(ref rtVector PredictPosition, ref double PredictAngle, rtMotorCtrl Data)
{
TimeSpan DisTime = DateTime.Now - GlobalVar.NavTimeStamp;
LocateData.tPosition.eX = GlobalVar.CurrentPosition.LocationX;
LocateData.tPosition.eY = GlobalVar.CurrentPosition.LocationY;
LocateData.eAngle = GlobalVar.CurrentPosition.Direction;
LocateData.eWheelAngle = GlobalVar.RealMotorAngle;
a_tAGV_Data.tCarInfo = LocateData;
rtMotorCtrl.Motion_Predict(a_tAGV_Data.tCarInfo, Data, DisTime.TotalMilliseconds / 1000, out PredictPosition, out PredictAngle);
}
/// <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> 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;
}
