414 lines
18 KiB
C#
414 lines
18 KiB
C#
using ACS.SPiiPlusNET;
|
|
using Aurora;
|
|
using Aurora.Api;
|
|
using Aurora.Api.Devices;
|
|
using Aurora.View;
|
|
using AuroraACS;
|
|
using AuroraHeightSensor;
|
|
using AuroraLaser;
|
|
using AuroraLaser.Trumpf;
|
|
using AuroraMassFlowMeter;
|
|
using AuroraMotionBoard.Ajin;
|
|
using AuroraOCT.Trumpf;
|
|
using Hermes;
|
|
using ILSOS_PrismaticGun.Alarm;
|
|
using ILSOS_PrismaticGun.Device;
|
|
using ILSOS_PrismaticGun.Device.ACS;
|
|
using ILSOS_PrismaticGun.Process;
|
|
using ILSOS_PrismaticGun.Recipes;
|
|
using ILSOS_PrismaticGun.Sequences.SubSequence.StageA;
|
|
using ILSOS_PrismaticGun.Sequences.SubSequence.StageB;
|
|
using ILSOS_PrismaticGun.Settings;
|
|
using ILSOS_PrismaticGun.Tools;
|
|
using ILSOS_PrismaticGun.Views.Recipe;
|
|
using OpenCvSharp.Dnn;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Diagnostics;
|
|
using System.Threading;
|
|
using System.Windows;
|
|
using VisionCommon;
|
|
using WeldingCommon;
|
|
using WeldingCommon.Alarm;
|
|
using WeldingCommon.Data;
|
|
using WeldingCommon.Sequences;
|
|
using WeldingCommon.Settings;
|
|
using static AuroraOCT.Trumpf.OCT;
|
|
using static WeldingCommon.Devices.PLCController;
|
|
|
|
|
|
namespace ILSOS_PrismaticGun.Sequences
|
|
{
|
|
class SeqMotorInit
|
|
{
|
|
static readonly Logger Log = new Logger("MotorInitSequence");
|
|
const int SEQ_TIMEOUT = 120 * 1000;
|
|
const int RESET_TIMEOUT = 10 * 1000;
|
|
|
|
const int MOTION_TIMEOUT = 10 * 1000;
|
|
const int MOTION_INTERVAL = 20;
|
|
|
|
const string SEQ_ENTER = "SEQ_ENTER";
|
|
const string SEQ_CHECK_INIT_CONDITION = "Check Init Condition";
|
|
const string SEQ_PREPARE_MOTION = "Prepare Motion - JigUp";
|
|
const string SEQ_RE_SCAN = "ReScan";
|
|
const string SEQ_INIT_Z_AXIS = "Init Z Axis";
|
|
const string SEQ_ACS_ALARM_RESET = "ACS Alarm Reset";
|
|
const string SEQ_INIT_ACS_AXIS = "Init ACS X,Y Axis";
|
|
const string SEQ_MOTION_FORK = "Fork Safety Position Move";
|
|
const string SEQ_MOVE_SAFETY_POS_STAGE_A = "Move Safety Pos Stage A";
|
|
const string SEQ_MOVE_SAFETY_POS_STAGE_B = "Move Safety Pos Stage B";
|
|
const string SEQ_MOTION_JOIN = "Join Safety Position Move";
|
|
const string SEQ_INIT_COMPLETE = "Init Complete";
|
|
|
|
private static PLCControllerPrismaticGun _Plc;
|
|
private static PrismaticGunACS _acs;
|
|
private static IPrismaticGunACS _motion;
|
|
private static PrismaticGunACSIO _io;
|
|
|
|
private static AJINMotor _StageAZ;
|
|
private static AJINMotor _StageBZ;
|
|
|
|
private static AJINDevice _Ajin;
|
|
|
|
|
|
public static SequenceBuilder Builder()
|
|
{
|
|
SequenceBuilder sb = new SequenceBuilder(1000, true);
|
|
|
|
sb.AddAction(SEQ_ENTER, Sequence_Enter);
|
|
sb.AddAction(SEQ_CHECK_INIT_CONDITION, CheckInitCondition);
|
|
sb.AddAction(SEQ_PREPARE_MOTION, PrepareMotion);
|
|
sb.AddAction(SEQ_RE_SCAN, AjinRescan);
|
|
sb.AddAction(SEQ_INIT_Z_AXIS, ZAxisInit);
|
|
sb.AddAction(SEQ_ACS_ALARM_RESET, ACSAlarmReset);
|
|
sb.AddAction(SEQ_INIT_ACS_AXIS, ACSAxisInit);
|
|
sb.AddFork(SEQ_MOTION_FORK);
|
|
sb.AddSubsequence(SEQ_MOVE_SAFETY_POS_STAGE_A, SeqTeachingPosMoveStageA.Builder());
|
|
sb.AddSubsequence(SEQ_MOVE_SAFETY_POS_STAGE_B, SeqTeachingPosMoveStageB.Builder());
|
|
sb.AddFork(SEQ_MOTION_JOIN);
|
|
sb.AddAction(SEQ_INIT_COMPLETE, InitComplete);
|
|
|
|
sb.OnStopped(OnStopped);
|
|
sb.OnForceError(OnForceError);
|
|
|
|
sb.Flows(sb.Init > sb[SEQ_ENTER] > sb[SEQ_CHECK_INIT_CONDITION] > sb[SEQ_PREPARE_MOTION] >
|
|
sb[SEQ_RE_SCAN] > sb[SEQ_INIT_Z_AXIS] > sb[SEQ_ACS_ALARM_RESET] > sb[SEQ_INIT_ACS_AXIS] > sb[SEQ_MOTION_FORK]);
|
|
sb.Flows(sb[SEQ_MOTION_FORK] > sb[SEQ_MOVE_SAFETY_POS_STAGE_A] > sb[SEQ_MOTION_JOIN]);
|
|
sb.Flows(sb[SEQ_MOTION_FORK] > sb[SEQ_MOVE_SAFETY_POS_STAGE_B] > sb[SEQ_MOTION_JOIN]);
|
|
sb.Flows(sb[SEQ_MOTION_JOIN] > sb[SEQ_INIT_COMPLETE] > sb.Final);
|
|
return sb;
|
|
}
|
|
|
|
private static SequenceActionResult Sequence_Enter(SequenceContext context, string id, SequenceActionResult param)
|
|
{
|
|
if (_motion is null)
|
|
_motion = _acs = context.Devices[DeviceIds.ACS] as PrismaticGunACS;
|
|
|
|
if (_io is null)
|
|
_io = context.Devices[DeviceIds.IODevice] as PrismaticGunACSIO;
|
|
|
|
if (_Ajin is null)
|
|
_Ajin = context.Devices[CommonDeviceIds.AJIN] as AJINDevice;
|
|
|
|
if (_StageAZ is null)
|
|
_StageAZ = context.Devices[MotorIds.ALineZ] as AJINMotor;
|
|
|
|
if (_StageBZ is null)
|
|
_StageBZ = context.Devices[MotorIds.BLineZ] as AJINMotor;
|
|
|
|
if (_Plc is null)
|
|
_Plc = context.Devices[DeviceIds.PLCDevice] as PLCControllerPrismaticGun;
|
|
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
private static SequenceActionResult CheckInitCondition(SequenceContext context, string id, SequenceActionResult param)
|
|
{
|
|
if (!((_motion as PrismaticGunACS).IsActivated)) {
|
|
context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.STAGE_A_DEACTIVATED_DISCONNECTED);
|
|
context.PutValue<string>("AlarmMessage",
|
|
$"[ACS activated({(_motion as PrismaticGunACS).IsActivated})]");
|
|
return null;
|
|
}
|
|
|
|
if (!(_io.IsActivated && _io.IsConnected())) {
|
|
Log.E();
|
|
context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.DEVICE_ACSIO_DEACTIVATED_DISCONNECTED);
|
|
context.PutValue<string>("AlarmMessage",
|
|
$"[IO activated({_io.IsActivated}) / connected({_io.IsConnected()})]");
|
|
return null;
|
|
}
|
|
|
|
if (!(_Plc.IsActivated && _Plc.IsConnected)) {
|
|
context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.DEVICE_PLC_DEACTIVATED_DISCONNECTED);
|
|
context.PutValue<string>("AlarmMessage",
|
|
$"[PLC activated({_Plc.IsActivated}) / connected({_Plc.IsConnected})]");
|
|
return null;
|
|
}
|
|
|
|
if(!_Plc.ReadBit(PLCControllerPrismaticGun.eBitInput.PLC_STAGE_A_LOAD_ENABLE).data) {
|
|
context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.STAGE_A_PLC_ENABLE_TIMEOUT);
|
|
context.PutValue<string>("AlarmMessage",
|
|
$"Stage A Load Enable이 꺼져있습니다.");
|
|
return null;
|
|
}
|
|
|
|
if (!_Plc.ReadBit(PLCControllerPrismaticGun.eBitInput.PLC_STAGE_B_LOAD_ENABLE).data) {
|
|
context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.STAGE_B_PLC_ENABLE_TIMEOUT);
|
|
context.PutValue<string>("AlarmMessage",
|
|
$"Stage B Load Enable이 꺼져있습니다.");
|
|
return null;
|
|
}
|
|
|
|
var InbufferPalletExist = _Plc.ExistPalletInBuffer(eStage.A) || _Plc.ExistRealPalletInBuffer(eStage.A);
|
|
if (InbufferPalletExist) {
|
|
context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.STAGE_A_MISMACHING_CELL_EXIST_DATA);
|
|
context.PutValue<string>("AlarmMessage",
|
|
$"Stage A In-buffer에 데이터({_Plc.ExistPalletInBuffer(eStage.A)}) 또는 실물({_Plc.ExistRealPalletInBuffer(eStage.A)})이 있습니다. 실물과 데이터를 지운 후 진행하세요");
|
|
return null;
|
|
}
|
|
|
|
InbufferPalletExist = _Plc.ExistPalletInBuffer(eStage.B) || _Plc.ExistRealPalletInBuffer(eStage.B);
|
|
if (InbufferPalletExist) {
|
|
context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.STAGE_B_MISMACHING_CELL_EXIST_DATA);
|
|
context.PutValue<string>("AlarmMessage",
|
|
$"Stage B In-buffer에 데이터({_Plc.ExistPalletInBuffer(eStage.B)}) 또는 실물({_Plc.ExistRealPalletInBuffer(eStage.B)})이 있습니다. 실물과 데이터를 지운 후 진행하세요");
|
|
return null;
|
|
}
|
|
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
private static SequenceActionResult PrepareMotion(SequenceContext context, string id, SequenceActionResult param)
|
|
{
|
|
if (!_io.CheckJigUp(eStage.A)) {
|
|
Log.V($"Stage A Jig Up Start");
|
|
_io.JigUp(eStage.A);
|
|
|
|
var res = context.RetryUntilSuccess(() => _io.CheckJigUp(eStage.A), MOTION_TIMEOUT, MOTION_INTERVAL);
|
|
if (res == E2.TIMEOUT) {
|
|
context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.STAGE_A_JIG_UP_TIMEOUT);
|
|
context.PutValue<string>("AlarmMessage", "Jig Up Check Timeout!");
|
|
return null;
|
|
} else if (res == E2.CANCELLED) {
|
|
context.PutValue<int>("AlarmCode", 0);
|
|
context.PutValue<string>("AlarmMessage", "Jig Up Check Cancel!");
|
|
return null;
|
|
}
|
|
Log.V($"Stage A Jig Up Complete");
|
|
}
|
|
|
|
if (!_io.CheckJigUp(eStage.B)) {
|
|
Log.V($"Stage B Jig Up Start");
|
|
_io.JigUp(eStage.B);
|
|
|
|
var res = context.RetryUntilSuccess(() => _io.CheckJigUp(eStage.B), MOTION_TIMEOUT, MOTION_INTERVAL);
|
|
if (res == E2.TIMEOUT) {
|
|
context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.STAGE_B_JIG_UP_TIMEOUT);
|
|
context.PutValue<string>("AlarmMessage", "Jig Up Check Timeout!");
|
|
return null;
|
|
} else if (res == E2.CANCELLED) {
|
|
context.PutValue<int>("AlarmCode", 0);
|
|
context.PutValue<string>("AlarmMessage", "Jig Up Check Cancel!");
|
|
return null;
|
|
}
|
|
Log.V($"Stage B Jig Up Complete");
|
|
}
|
|
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
private static SequenceActionResult AjinRescan(SequenceContext context, string id, SequenceActionResult param)
|
|
{
|
|
Log.V($"Ajin ReScan Start");
|
|
if(_Ajin.CheckDeviceLive()){
|
|
Log.I("Ajin Device HomeComplete Check OK - Rescan Skip");
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
if (!_Ajin.Rescan()) {
|
|
context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.DEVICE_MOTOR_DEACTIVATED);
|
|
context.PutValue<string>("AlarmMessage", "Rescan Command Fail");
|
|
return null;
|
|
}
|
|
|
|
if (!_Ajin.ReInitialize()) {
|
|
context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.DEVICE_MOTOR_DEACTIVATED);
|
|
context.PutValue<string>("AlarmMessage", "ReInitialize Fail");
|
|
return null;
|
|
}
|
|
|
|
if (_Ajin.LoadMotFile() != E1.OK) {
|
|
context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.DEVICE_MOTOR_DEACTIVATED);
|
|
context.PutValue<string>("AlarmMessage", ".mot File Load Fail");
|
|
return null;
|
|
}
|
|
var res = context.RetryUntilSuccess(() => _Ajin.CheckDeviceLive(), SEQ_TIMEOUT, MOTION_INTERVAL);
|
|
if (res == E2.TIMEOUT) {
|
|
context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.DEVICE_MOTOR_DEACTIVATED);
|
|
context.PutValue<string>("AlarmMessage", "[Ajin ReScan Check Fail Timeout]");
|
|
return null;
|
|
} else if (res == E2.CANCELLED) {
|
|
context.PutValue<int>("AlarmCode", 0);
|
|
context.PutValue<string>("AlarmMessage", "[Ajin ReScan Check Fail Cancel]");
|
|
return null;
|
|
}
|
|
|
|
Log.V($"Ajin ReScan End");
|
|
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
private static SequenceActionResult ZAxisInit(SequenceContext context, string id, SequenceActionResult param)
|
|
{
|
|
Log.V($"Ajin Z Axis Home Start");
|
|
|
|
if(!_StageAZ.IsServoOn())
|
|
_StageAZ.ServoOn(true);
|
|
|
|
if (!_StageBZ.IsServoOn())
|
|
_StageBZ.ServoOn(true);
|
|
|
|
Thread.Sleep(300);
|
|
|
|
if(!_StageAZ.IsHomeCompleted())
|
|
_StageAZ.DoHome();
|
|
|
|
if (!_StageBZ.IsHomeCompleted())
|
|
_StageBZ.DoHome();
|
|
|
|
var res = context.RetryUntilSuccess(() => _StageAZ.IsHomeCompleted(), SEQ_TIMEOUT, MOTION_INTERVAL);
|
|
if (res == E2.TIMEOUT) {
|
|
context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.STAGE_A_Z_MOVE_TIMEOUT);
|
|
context.PutValue<string>("AlarmMessage", "Ajin StageA Z Axis Home Timeout");
|
|
return null;
|
|
} else if (res == E2.CANCELLED) {
|
|
context.PutValue<int>("AlarmCode", 0);
|
|
context.PutValue<string>("AlarmMessage", "Ajin StageA Z Axis Home Cancel");
|
|
return null;
|
|
}
|
|
|
|
res = context.RetryUntilSuccess(() => _StageBZ.IsHomeCompleted(), SEQ_TIMEOUT, MOTION_INTERVAL);
|
|
if (res == E2.TIMEOUT) {
|
|
context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.STAGE_B_Z_MOVE_TIMEOUT);
|
|
context.PutValue<string>("AlarmMessage", "Ajin StageB Z Axis Home Timeout");
|
|
return null;
|
|
} else if (res == E2.CANCELLED) {
|
|
context.PutValue<int>("AlarmCode", 0);
|
|
context.PutValue<string>("AlarmMessage", "Ajin StageB Z Axis Home Cancel");
|
|
return null;
|
|
}
|
|
Log.V($"Ajin Z Axis Home End");
|
|
|
|
_StageAZ.Move(0.0, AuroraMotionBoard.Ajin.Speed.L, TargetPosType.ABS);
|
|
Log.V($"Move Z Position : 0.0");
|
|
|
|
_StageBZ.Move(0.0, AuroraMotionBoard.Ajin.Speed.L, TargetPosType.ABS);
|
|
Log.V($"Move Z Position : 0.0");
|
|
|
|
res = context.RetryUntilSuccess(() => _StageAZ.IsReachedTargetPosition(0.0), MOTION_TIMEOUT, MOTION_INTERVAL, true);
|
|
if (res == E2.TIMEOUT) {
|
|
Log.FuncE($"Z-Axis({Enum.GetName(typeof(eTeachingPosition), eTeachingPosition.SafetyPosition)}) Move Timeout!");
|
|
context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.STAGE_A_Z_MOVE_TIMEOUT);
|
|
return null;
|
|
} else if (res == E2.CANCELLED) {
|
|
Log.FuncE($"Z-Axis({Enum.GetName(typeof(eTeachingPosition), eTeachingPosition.SafetyPosition)}) Move Cancel!");
|
|
context.PutValue<int>("AlarmCode", 0);
|
|
return null;
|
|
}
|
|
|
|
res = context.RetryUntilSuccess(() => _StageBZ.IsReachedTargetPosition(0.0), MOTION_TIMEOUT, MOTION_INTERVAL, true);
|
|
if (res == E2.TIMEOUT) {
|
|
Log.FuncE($"Z-Axis(0.0) Move Timeout!");
|
|
context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.STAGE_B_Z_MOVE_TIMEOUT);
|
|
return null;
|
|
} else if (res == E2.CANCELLED) {
|
|
Log.FuncE($"Z-Axis(0.0) Move Cancel!");
|
|
context.PutValue<int>("AlarmCode", 0);
|
|
return null;
|
|
}
|
|
Log.V($"Move Z Position Complete : 0.0");
|
|
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
private static SequenceActionResult ACSAlarmReset(SequenceContext context, string id, SequenceActionResult param)
|
|
{
|
|
Log.V("ACS Alarm Reset Start");
|
|
_motion.ResetAlarm();
|
|
Thread.Sleep(1000);
|
|
|
|
var res = context.RetryUntilSuccess(() => !_acs.HasHeavyAlarm & !_acs.HasLightAlarm, RESET_TIMEOUT, MOTION_INTERVAL);
|
|
if (res == E2.TIMEOUT) {
|
|
context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.ACS_AXIS_INITIALIZE_FAIL);
|
|
context.PutValue<string>("AlarmMessage", "ACS Alarm Reset Fail");
|
|
return null;
|
|
} else if (res == E2.CANCELLED) {
|
|
context.PutValue<int>("AlarmCode", 0);
|
|
context.PutValue<string>("AlarmMessage", "ACS Alarm Reset Cancel");
|
|
return null;
|
|
}
|
|
Log.V("ACS Alarm Reset End");
|
|
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
private static SequenceActionResult ACSAxisInit(SequenceContext context, string id, SequenceActionResult param)
|
|
{
|
|
Log.V($"ACS Init Start");
|
|
_motion.AllAxisHomeStart();
|
|
Thread.Sleep(1000);
|
|
|
|
var res = context.RetryUntilSuccess(() => _motion.CheckAllAxisHome(), SEQ_TIMEOUT, MOTION_INTERVAL);
|
|
if (res == E2.TIMEOUT) {
|
|
context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.ACS_AXIS_INITIALIZE_FAIL);
|
|
context.PutValue<string>("AlarmMessage", "ACS Axis Home Timeout");
|
|
return null;
|
|
} else if (res == E2.CANCELLED) {
|
|
context.PutValue<int>("AlarmCode", 0);
|
|
context.PutValue<string>("AlarmMessage", "ACS Axis Home Cancel");
|
|
return null;
|
|
}
|
|
|
|
Log.V($"ACS Init End");
|
|
return SequenceAction.Ok(eTeachingPosition.SafetyPosition);
|
|
}
|
|
|
|
private static SequenceActionResult InitComplete(SequenceContext context, string id, SequenceActionResult param)
|
|
{
|
|
MessageUtil.ShowSuccessToast("Initialize Complete", Log);
|
|
return SequenceAction.Ok();
|
|
}
|
|
|
|
private static void OnStopped(SequenceContext context)
|
|
{
|
|
var AlarmCode = context.GetValue<int>("AlarmCode");
|
|
if (AlarmCode > 0)
|
|
Log.E(context.AlarmAccured(AlarmCode));
|
|
|
|
_motion.AllAxisHomeStop();
|
|
_StageAZ.Stop();
|
|
_StageBZ.Stop();
|
|
|
|
return;
|
|
}
|
|
|
|
private static void OnForceError(SequenceContext context, string arg2)
|
|
{
|
|
var AlarmCode = context.GetValue<int>("AlarmCode");
|
|
if (AlarmCode > 0)
|
|
Log.E(context.AlarmAccured(AlarmCode));
|
|
|
|
var message = context.GetValue<string>("AlarmMessage");
|
|
Application.Current.Dispatcher.Invoke(() => { MessageUtil.CreateAndShowMessageDialog("Initialize", message); });
|
|
|
|
_motion.AllAxisHomeStop();
|
|
_StageAZ.Stop();
|
|
_StageBZ.Stop();
|
|
|
|
return;
|
|
}
|
|
}
|
|
}
|