public bool GetMovingAngleAfterRTurn(AGVTurnParameter turn, MapAddress start, MapAddress end, ref double newAGVMovingAngle, ref string errorMessage)
{
double rTurnSectionAngle = ComputeAngle(start.AGVPosition, end.AGVPosition);
double agvGoAngle = start.AGVPosition.Angle;
bool dirFlag = true;
if (Math.Abs(GetCurrectAngle(agvGoAngle - rTurnSectionAngle)) >= 90)
{
dirFlag = false;
agvGoAngle = GetCurrectAngle(agvGoAngle - 180);
}
double delta = GetCurrectAngle(rTurnSectionAngle - agvGoAngle);
double endAngle = GetCurrectAngle(rTurnSectionAngle + delta);
if (!dirFlag)
{
endAngle = GetCurrectAngle(endAngle - 180);
}
newAGVMovingAngle = GetCurrectAngle(end.AGVPosition.Angle + (dirFlag ? 0 : 180));
if (Math.Abs(GetCurrectAngle(endAngle - end.AGVPosition.Angle)) > turn.SectionAngleChangeMax)
{
errorMessage = String.Concat("turnType : ", turn.TurnName,
", start (", GetMapAGVPositionString(start.AGVPosition), ") VehicleHeadAngle = ", start.AGVPosition.Angle.ToString("0"),
", end ", GetMapAGVPositionString(end.AGVPosition), " VehicleHeadAngle = ", end.AGVPosition.Angle.ToString("0"),
" 判定角度變化與Config不符合");
return(false);
}
return(true);
}
protected AGVTurnParameter ReadTurnXML(XmlElement element)
{
AGVTurnParameter temp = new AGVTurnParameter();
foreach (XmlNode item in element.ChildNodes)
{
switch (item.Name)
{
case "R":
temp.R = double.Parse(item.InnerText);
break;
case "Velocity":
temp.Velocity = double.Parse(item.InnerText);
break;
case "VChangeSafetyDistance":
temp.VChangeSafetyDistance = double.Parse(item.InnerText);
break;
case "SectionAngleChangeMin":
temp.SectionAngleChangeMin = double.Parse(item.InnerText);
break;
case "SectionAngleChangeMax":
temp.SectionAngleChangeMax = double.Parse(item.InnerText);
break;
case "SafetyVelocityRange":
temp.SafetyVelocityRange = double.Parse(item.InnerText);
break;
case "RTurnDistanceRange":
temp.RTurnDistanceRange = double.Parse(item.InnerText);
break;
default:
break;
}
}
return(temp);
}
