private void SimulateReal_SimulateMode(SectionLine sectionLine)
{
if (sectionLine == null)
{
return;
}
double commandEncoder = localData.MoveControlData.MoveCommand.CommandEncoder + localData.MoveControlData.MotionControlData.SimulateLineVelocity * localData.MoveControlData.MoveControlConfig.TimeValueConfig.IntervalTimeList[EnumIntervalTimeType.ThreadSleepTime] / 1000;
commandEncoder -= sectionLine.EncoderAddSectionDistanceStart;
if (!sectionLine.SectionDirFlag)
{
commandEncoder = -commandEncoder;
}
MapAGVPosition fakeReal = new MapAGVPosition();
double percent = commandEncoder / sectionLine.Section.Distance;
fakeReal.Angle = computeFunction.GetCurrectAngle(sectionLine.Section.FromVehicleAngle +
computeFunction.GetCurrectAngle(sectionLine.Section.ToVehicleAngle - sectionLine.Section.FromVehicleAngle) * percent);
fakeReal.Position = new MapPosition(sectionLine.Section.FromAddress.AGVPosition.Position.X +
(sectionLine.Section.ToAddress.AGVPosition.Position.X -
sectionLine.Section.FromAddress.AGVPosition.Position.X) * percent,
sectionLine.Section.FromAddress.AGVPosition.Position.Y +
(sectionLine.Section.ToAddress.AGVPosition.Position.Y -
sectionLine.Section.FromAddress.AGVPosition.Position.Y) * percent);
localData.MoveControlData.MotionControlData.LineVelocity = localData.MoveControlData.MotionControlData.SimulateLineVelocity;
localData.MoveControlData.MotionControlData.MoveStatus = (localData.MoveControlData.MotionControlData.SimulateLineVelocity != 0) ? EnumAxisMoveStatus.Move : EnumAxisMoveStatus.Stop;
localData.Real = fakeReal;
}
/// <summary>
/// Creates a new <see cref="IIniDocument"/> using data from a <see cref="TextReader"/>.
/// </summary>
/// <param name='reader'>
/// A <see cref="TextReader"/> that will supply the input data.
/// </param>
public static IIniDocument Read(TextReader reader)
{
IIniDocument output = new IniDocument();
IIniSection currentSection = output;
bool endOfStream = false;
int lineNumber = 0;
while (!endOfStream)
{
string currentLine = reader.ReadLine();
Match currentMatch;
lineNumber++;
// If we get a null response then we are at the end of the stream and can exit
if (currentLine == null)
{
endOfStream = true;
continue;
}
// We silently skip over empty lines
if (EmptyLine.Match(currentLine).Success || CommentLine.Match(currentLine).Success)
{
continue;
}
// If we find a 'new section' line then we create a new section and begin dealing with it
currentMatch = SectionLine.Match(currentLine);
if (currentMatch.Success)
{
string sectionName = currentMatch.Groups[1].Value.Trim();
currentSection = new IniSection();
output.Sections.Add(sectionName, currentSection);
continue;
}
// If we find a value line then we store it within the current section.
currentMatch = ValueLine.Match(currentLine);
if (currentMatch.Success)
{
string key = currentMatch.Groups[1].Value.Trim();
string value = currentMatch.Groups[2].Value.Trim();
currentSection[key] = value;
continue;
}
throw new FormatException(String.Format("Invalid INI data at line {0}.", lineNumber));
}
return(output);
}
public MapPosition GetTransferPosition(SectionLine section, MapPosition position)
{
if (section == null || position == null)
{
return(null);
}
double x = position.X - section.Start.AGVPosition.Position.X;
double y = position.Y - section.Start.AGVPosition.Position.Y;
double newX = x * section.CosTheta + y * section.SinTheta;
double newY = -x * section.SinTheta + y * section.CosTheta;
return(new MapPosition(newX, newY));
}
public static List <SectionLine> ToList(List <Line> lines, Guid sId, string sectionName, string sectionType)
{
var sectionLineList = new List <SectionLine>();
if (lines != null)
{
foreach (var line in lines)
{
var dt = line.dataTable;
for (int i = 0; i < dt.Rows.Count; i++)
{
var li = new SectionLine
{
Id = Guid.NewGuid(),
LineName = line.name,
SectionId = sId,
SectionName = sectionName,
SectionType = sectionType,
CreateTime = DateTime.Now
};
if (dt.Rows[i][0] != null && !string.IsNullOrEmpty(dt.Rows[i][0].ToString()))
{
li.RecordTime = DateTime.Parse(dt.Rows[i][0].ToString());
}
li.Result1 = dt.Rows[i][1] != null && !string.IsNullOrEmpty(dt.Rows[i][1].ToString()) ? double.Parse(dt.Rows[i][1].ToString()) : 0;
li.Result2 = dt.Rows[i][2] != null && !string.IsNullOrEmpty(dt.Rows[i][2].ToString()) ? double.Parse(dt.Rows[i][2].ToString()) : 0;
li.Result3 = dt.Rows[i][3] != null && !string.IsNullOrEmpty(dt.Rows[i][3].ToString()) ? double.Parse(dt.Rows[i][3].ToString()) : 0;
li.AvgResult = dt.Rows[i][4] != null && !string.IsNullOrEmpty(dt.Rows[i][4].ToString()) ? double.Parse(dt.Rows[i][4].ToString()) : 0;
li.EquipResult = dt.Rows[i][5] != null && !string.IsNullOrEmpty(dt.Rows[i][5].ToString()) ? double.Parse(dt.Rows[i][5].ToString()) : 0;
li.Temperature = dt.Rows[i][6] != null && !string.IsNullOrEmpty(dt.Rows[i][6].ToString()) ? double.Parse(dt.Rows[i][6].ToString()) : 0;
li.TempResult = dt.Rows[i][7] != null && !string.IsNullOrEmpty(dt.Rows[i][7].ToString()) ? double.Parse(dt.Rows[i][7].ToString()) : 0;
li.LastDiffResult = dt.Rows[i][8] != null && !string.IsNullOrEmpty(dt.Rows[i][8].ToString()) ? double.Parse(dt.Rows[i][8].ToString()) : 0;
li.DisSpeed = dt.Rows[i][9] != null && !string.IsNullOrEmpty(dt.Rows[i][9].ToString()) ? double.Parse(dt.Rows[i][9].ToString()) : 0;
li.CumSpeed = dt.Rows[i][10] != null && !string.IsNullOrEmpty(dt.Rows[i][10].ToString()) ? double.Parse(dt.Rows[i][10].ToString()) : 0;
sectionLineList.Add(li);
}
}
}
return(sectionLineList);
}
public void UpdateAllData(SectionLine sectionLine)
{
thetaSectionDeviation = null;
if (localData.MoveControlData.MoveCommand != null)
{
if (localData.SimulateMode)
{
SimulateReal_SimulateMode(sectionLine);
}
UpdateAddressSection_MoveControlMovingFlow(sectionLine);
}
else
{
if (localData.AutoManual == EnumAutoState.Manual)
{
UpdateAddressSection_ManualMode();
}
}
localData.MoveControlData.ThetaSectionDeviation = thetaSectionDeviation;
}
private void UpdateAddressSection_MoveControlMovingFlow(SectionLine sectionLine)
{
if (sectionLine == null || localData.Real == null)
{
return;
}
VehicleLocation newVehicleLocation = new VehicleLocation();
if (localData.MoveControlData.MoveCommand.CommandStatus == EnumMoveCommandStartStatus.Reporting)
{
newVehicleLocation.LastAddress = localData.Location.LastAddress;
newVehicleLocation.InAddress = localData.Location.InAddress;
}
else
{
if (localData.TheMapInfo.AllAddress.ContainsKey(sectionLine.Start.Id))
{
newVehicleLocation.LastAddress = sectionLine.Start.Id;
}
else
{
newVehicleLocation.LastAddress = localData.Location.LastAddress;
}
}
newVehicleLocation.NowSection = sectionLine.Section.Id;
MapPosition temp;
if (localData.MoveControlData.LocateControlData.LocateAGVPosition != null)
{
temp = computeFunction.GetTransferPosition(sectionLine.Section, localData.MoveControlData.LocateControlData.LocateAGVPosition.AGVPosition.Position);
}
else
{
temp = computeFunction.GetTransferPosition(sectionLine.Section, localData.Real.Position);
}
//MapPosition temp = computeFunction.GetTransferPosition(sectionLine.Section, localData.Real.Position);
if (temp.X < 0)
{
newVehicleLocation.DistanceFormSectionHead = 0;
}
else if (temp.X > sectionLine.Section.Distance)
{
newVehicleLocation.DistanceFormSectionHead = sectionLine.Section.Distance;
}
else
{
newVehicleLocation.DistanceFormSectionHead = temp.X;
}
if (Math.Abs(temp.X) <= localData.MoveControlData.MoveControlConfig.InPositionRange)
{
newVehicleLocation.InAddress = true;
}
else
{
newVehicleLocation.InAddress = false;
}
if (localData.MoveControlData.MoveCommand.MoveStatus == EnumMoveStatus.Moving)
{
double sectionDeviation = temp.Y;
double theta = computeFunction.GetCurrectAngle(localData.Real.Angle - sectionLine.Section.FromVehicleAngle);
thetaSectionDeviation = new ThetaSectionDeviation(theta, sectionDeviation);
}
localData.MoveControlData.MoveCommand.CommandEncoder = sectionLine.EncoderAddSectionDistanceStart + (sectionLine.SectionDirFlag ? temp.X : -temp.X);
localData.Location = newVehicleLocation;
localData.LastAGVPosition = localData.Real;
}
protected void SetFindSectionData()
{
SectionLine slamSection;
SLAMTransfer slamTransfer;
MapAddress from;
MapAddress to;
double sectionAngle;
double sectionDistance;
foreach (MapSection section in localData.TheMapInfo.AllSection.Values)
{
if (slamPosition.ContainsKey(section.FromAddress.Id) && slamPosition[section.FromAddress.Id] != null &&
slamPosition.ContainsKey(section.ToAddress.Id) && slamPosition[section.ToAddress.Id] != null)
{
from = new MapAddress();
from.Id = section.FromAddress.Id;
from.AGVPosition = slamPosition[section.FromAddress.Id];
to = new MapAddress();
to.Id = section.ToAddress.Id;
to.AGVPosition = slamPosition[section.ToAddress.Id];
sectionAngle = computeFunction.ComputeAngle(from.AGVPosition, to.AGVPosition);
sectionDistance = computeFunction.GetDistanceFormTwoAGVPosition(from.AGVPosition, to.AGVPosition);
slamSection = new SectionLine(section, from, to, sectionAngle, sectionDistance, 0, true, 0);
findSectionList.Add(section.Id, slamSection);
slamTransfer = new SLAMTransfer();
slamTransfer.Step1Offset = slamPosition[section.FromAddress.Id].Position;
sectionSLAMTransferLit.Add(section.Id, GetTransferBySection(section));
//sectionSLAMTransferLit.Add(section.Id, GetTransferByFourPosition(section.FromAddress.AGVPosition.Position, section.ToAddress.AGVPosition.Position,
// slamPosition[section.FromAddress.Id].Position, slamPosition[section.ToAddress.Id].Position, section.FromVehicleAngle, section.ToVehicleAngle));
SLAMTransfer temp = sectionSLAMTransferLit[section.Id];
WriteLog(7, "", String.Concat("Section : ", section.Id,
", Step1Offset (", computeFunction.GetMapPositionString(temp.Step1Offset, "0"),
"), Step2Cos : ", temp.Step2Cos.ToString("0.0"),
", Step2Sin : ", temp.Step2Sin.ToString("0.0"),
", Step3Mag : ", temp.Step3Mag.ToString("0.0"),
", Step4Offset (", computeFunction.GetMapPositionString(temp.Step4Offset, "0"),
"), ThetaOffset : ", temp.ThetaOffset.ToString("0.0"),
", ThetaOffsetStart : ", temp.ThetaOffsetStart.ToString("0.0"),
", ThetaOffsetEnd : ", temp.ThetaOffsetEnd.ToString("0.0"),
", Distance : ", temp.Distance.ToString("0.0")));
}
}
List <string> tempList;
foreach (MapSection section in localData.TheMapInfo.AllSection.Values)
{
if (findSectionList.ContainsKey(section.Id))
{
tempList = new List <string>();
for (int i = 0; i < section.NearbySection.Count; i++)
{
if (findSectionList.ContainsKey(section.NearbySection[i].Id))
{
tempList.Add(section.NearbySection[i].Id);
}
}
sectionConnectSectionList.Add(section.Id, tempList);
}
}
}
