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_ManualMode()
{
if (localData.Real != null && localData.LastAGVPosition != null)
{
double distance = computeFunction.GetTwoPositionDistance(localData.Real, localData.LastAGVPosition);
if (distance <= localData.MoveControlData.MoveControlConfig.InPositionRange && Math.Abs(computeFunction.GetCurrectAngle(localData.Real.Angle - localData.LastAGVPosition.Angle)) <= localData.MoveControlData.MoveControlConfig.SectionAllowDeltaTheta)
{
lastFindSectionTime = DateTime.Now;
string sectionID = localData.Location.NowSection;
if (localData.TheMapInfo.AllSection.ContainsKey(sectionID))
{
if (localData.TheMapInfo.AllSection[sectionID].FromVehicleAngle == localData.TheMapInfo.AllSection[sectionID].ToVehicleAngle)
{
MapPosition temp = computeFunction.GetTransferPosition(localData.TheMapInfo.AllSection[sectionID], localData.Real.Position);
double sectionDeviation = temp.Y;
double theta = computeFunction.GetCurrectAngle(localData.Real.Angle - localData.TheMapInfo.AllSection[sectionID].FromVehicleAngle);
thetaSectionDeviation = new ThetaSectionDeviation(theta, sectionDeviation);
}
}
return;
}
}
if (localData.Real == null)
{
if (localData.MoveControlData.MoveControlConfig.LosePositionSetNullAddressSection)
{
localData.Location = new VehicleLocation();
}
}
else
{
if ((DateTime.Now - lastFindSectionTime).TotalMilliseconds > localData.MoveControlData.MoveControlConfig.TimeValueConfig.IntervalTimeList[EnumIntervalTimeType.ManualFindSectionInterval])
{
lastFindSectionTime = DateTime.Now;
if (findSectionThread == null || !findSectionThread.IsAlive)
{
findSectionThread = new Thread(FindSectionThread);
findSectionThread.Start();
}
else
{
WriteLog(3, "", String.Concat("Find now Section Interval : ", localData.MoveControlData.MoveControlConfig.TimeValueConfig.IntervalTimeList[EnumIntervalTimeType.ManualFindSectionInterval].ToString("0"), ", 上一次搜尋還未結束"));
}
}
else
{
VehicleLocation oldLocate = localData.Location;
if (localData.TheMapInfo.AllSection.ContainsKey(oldLocate.NowSection))
{
VehicleLocation newLocate = new VehicleLocation();
newLocate.NowSection = oldLocate.NowSection;
MapPosition temp = computeFunction.GetTransferPosition(localData.TheMapInfo.AllSection[oldLocate.NowSection], localData.Real.Position);
if (Math.Abs(temp.X) <= localData.MoveControlData.MoveControlConfig.InPositionRange)
{
newLocate.InAddress = true;
newLocate.LastAddress = localData.TheMapInfo.AllSection[oldLocate.NowSection].FromAddress.Id;
}
else if (Math.Abs(localData.TheMapInfo.AllSection[oldLocate.NowSection].Distance - temp.X) <= localData.MoveControlData.MoveControlConfig.InPositionRange)
{
newLocate.InAddress = true;
newLocate.LastAddress = localData.TheMapInfo.AllSection[oldLocate.NowSection].ToAddress.Id;
}
else
{
newLocate.LastAddress = oldLocate.LastAddress;
}
if (temp.X < 0)
{
temp.X = 0;
}
else if (temp.X > localData.TheMapInfo.AllSection[oldLocate.NowSection].Distance)
{
temp.X = localData.TheMapInfo.AllSection[oldLocate.NowSection].Distance;
}
newLocate.DistanceFormSectionHead = temp.X;
double sectionDeviation = temp.Y;
double theta = computeFunction.GetCurrectAngle(localData.Real.Angle - localData.TheMapInfo.AllSection[oldLocate.NowSection].FromVehicleAngle);
thetaSectionDeviation = new ThetaSectionDeviation(theta, sectionDeviation);
localData.Location = newLocate;
}
}
}
}
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;
}
private void UpdateMoveControlJogPitch()
{
if (localData.MoveControlData.MotionControlData.JoystickMode)
{
label_JogPitch_JoystickValue.Text = "開啟中";
label_JogPitch_JoystickValue.ForeColor = Color.Green;
}
else
{
label_JogPitch_JoystickValue.Text = "關閉中";
label_JogPitch_JoystickValue.ForeColor = Color.Red;
}
pB_JogPitch_AllServoOn.BackColor = (mainFlow.MoveControl.MotionControl.AllServoOn ? Color.Green : Color.Transparent);
pB_JogPitch_AllServoOff.BackColor = (mainFlow.MoveControl.MotionControl.AllServoOff ? Color.Red : Color.Transparent);
label_JogPitch_LineVelocityValue.Text = localData.MoveControlData.MotionControlData.JoystickLineAxisData.Velocity.ToString("0");
label_JogPitch_LineAccValue.Text = localData.MoveControlData.MotionControlData.JoystickLineAxisData.Acceleration.ToString("0");
label_JogPitch_LineDecValue.Text = localData.MoveControlData.MotionControlData.JoystickLineAxisData.Deceleration.ToString("0");
label_JogPitch_ThetaVelocityValue.Text = localData.MoveControlData.MotionControlData.JoystickThetaAxisData.Velocity.ToString("0");
label_JogPitch_ThetaAccValue.Text = localData.MoveControlData.MotionControlData.JoystickThetaAxisData.Acceleration.ToString("0");
label_JogPitch_ThetaDecValue.Text = localData.MoveControlData.MotionControlData.JoystickThetaAxisData.Deceleration.ToString("0");
ThetaSectionDeviation temp = localData.MoveControlData.ThetaSectionDeviation;
if (temp != null)
{
label_JogPitch_SectionDeviationValue.Text = temp.SectionDeviation.ToString("0.0");
label_JogPitch_SectionThetaValue.Text = temp.Theta.ToString("0.0");
if (Math.Abs(temp.SectionDeviation) <= localData.MoveControlData.MoveControlConfig.Safety[EnumMoveControlSafetyType.OntimeReviseSectionDeviationLine].Range / 2)
{
label_JogPitch_SectionDeviationValue.ForeColor = Color.Green;
}
else
{
label_JogPitch_SectionDeviationValue.ForeColor = Color.Red;
}
if (Math.Abs(temp.Theta) <= localData.MoveControlData.MoveControlConfig.Safety[EnumMoveControlSafetyType.OntimeReviseTheta].Range / 2)
{
label_JogPitch_SectionThetaValue.ForeColor = Color.Green;
}
else
{
label_JogPitch_SectionThetaValue.ForeColor = Color.Red;
}
}
else
{
label_JogPitch_SectionDeviationValue.Text = "---";
label_JogPitch_SectionDeviationValue.ForeColor = Color.Red;
label_JogPitch_SectionThetaValue.Text = "---";
label_JogPitch_SectionThetaValue.ForeColor = Color.Red;
}
if (localData.MoveControlData.MoveControlCanAuto)
{
label_JogPitch_CanAuto.Text = "可以Auto";
label_JogPitch_CanAuto.ForeColor = Color.Green;
label_JogPitch_CantAutoReason.Text = "";
}
else
{
label_JogPitch_CanAuto.Text = "不能Auto";
label_JogPitch_CanAuto.ForeColor = Color.Red;
label_JogPitch_CantAutoReason.Text = localData.MoveControlData.MoveControlCantAutoReason;
}
label_JogPitch_RealPositionValue.Text = computeFunction.GetMapAGVPositionStringWithAngle(localData.Real);
}
