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