2023-03-03 15:42:13 +08:00
|
|
|
|
using Aitex.Core.RT.Log;
|
|
|
|
|
using Aitex.Core.RT.Routine;
|
|
|
|
|
using Aitex.Core.RT.SCCore;
|
2023-04-13 15:35:13 +08:00
|
|
|
|
using SicModules.LLs.Routines.Base;
|
|
|
|
|
|
|
|
|
|
namespace SicModules.LLs.Routines
|
2023-03-03 15:42:13 +08:00
|
|
|
|
{
|
2024-06-13 18:13:59 +08:00
|
|
|
|
public class LoadRotationHomeRoutine : LoadLockBaseRoutine
|
2023-03-03 15:42:13 +08:00
|
|
|
|
{
|
|
|
|
|
enum RoutineStep
|
|
|
|
|
{
|
2024-06-07 17:28:20 +08:00
|
|
|
|
TimeDelay1,
|
|
|
|
|
TrayClamp,
|
|
|
|
|
TrayUnClamp,
|
2023-03-03 15:42:13 +08:00
|
|
|
|
ResetRotationServo,
|
|
|
|
|
RotationServoOn,
|
|
|
|
|
MoveRelativeHome,
|
|
|
|
|
WaitMoveDone,
|
|
|
|
|
MoveHomeOffset,
|
|
|
|
|
TimeDelay2,
|
|
|
|
|
WaitMoveDone2,
|
|
|
|
|
}
|
|
|
|
|
private float _homeOffset;
|
|
|
|
|
|
2024-06-07 17:28:20 +08:00
|
|
|
|
private LoadLockTrayClawRoutine _trayClamp = new LoadLockTrayClawRoutine();
|
|
|
|
|
private LoadLockTrayClawRoutine _trayUnClamp = new LoadLockTrayClawRoutine();
|
|
|
|
|
|
2024-06-13 18:13:59 +08:00
|
|
|
|
public LoadRotationHomeRoutine()
|
2023-03-03 15:42:13 +08:00
|
|
|
|
{
|
|
|
|
|
Name = "Home";
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
public override Result Start(params object[] objs)
|
|
|
|
|
{
|
|
|
|
|
Reset();
|
|
|
|
|
|
|
|
|
|
_homeOffset = (float)SC.GetConfigItem($"LoadLock.LoadRotation.HomeOffset").DoubleValue;
|
|
|
|
|
|
2024-06-07 17:28:20 +08:00
|
|
|
|
_trayClamp.Init(true);
|
|
|
|
|
_trayUnClamp.Init(false);
|
|
|
|
|
|
2023-03-03 15:42:13 +08:00
|
|
|
|
Notify("Start");
|
|
|
|
|
|
|
|
|
|
return Result.RUN;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
public override Result Monitor()
|
|
|
|
|
{
|
|
|
|
|
try
|
|
|
|
|
{
|
|
|
|
|
if (SC.GetValue<bool>($"System.IsSimulatorMode"))
|
|
|
|
|
{
|
|
|
|
|
return Result.DONE;
|
|
|
|
|
}
|
|
|
|
|
|
2024-06-07 17:28:20 +08:00
|
|
|
|
//对中
|
|
|
|
|
ExecuteRoutine((int)RoutineStep.TrayClamp, _trayClamp); //夹爪关闭
|
|
|
|
|
TimeDelay((int)RoutineStep.TimeDelay1, 1);//延迟1s
|
|
|
|
|
ExecuteRoutine((int)RoutineStep.TrayUnClamp, _trayUnClamp); //夹爪打开
|
|
|
|
|
|
2023-03-03 15:42:13 +08:00
|
|
|
|
ResetRotationServo((int)RoutineStep.ResetRotationServo, 3);
|
|
|
|
|
|
|
|
|
|
RotationServoOn((int)RoutineStep.RotationServoOn, 3);
|
|
|
|
|
|
|
|
|
|
MoveRelativeHome((int)RoutineStep.MoveRelativeHome, 10);
|
2024-06-13 18:13:59 +08:00
|
|
|
|
//TimeDelay((int)RoutineStep.TimeDelay2, 3);
|
|
|
|
|
//WaitLoadRotationDone((int)RoutineStep.WaitMoveDone, 60);
|
2023-03-03 15:42:13 +08:00
|
|
|
|
|
|
|
|
|
//取消Offset
|
|
|
|
|
//MoveLoadRotationHomeOffset((int)RoutineStep.MoveHomeOffset, _homeOffset, 10);
|
|
|
|
|
//TimeDelay((int)RoutineStep.TimeDelay2, 1);
|
|
|
|
|
//WaitLoadRotationDone((int)RoutineStep.WaitMoveDone2, 60);
|
|
|
|
|
}
|
|
|
|
|
catch (RoutineBreakException)
|
|
|
|
|
{
|
|
|
|
|
return Result.RUN;
|
|
|
|
|
}
|
|
|
|
|
catch (RoutineFaildException ex)
|
|
|
|
|
{
|
|
|
|
|
LOG.Error(ex.ToString());
|
|
|
|
|
return Result.FAIL;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
Notify("Finished");
|
|
|
|
|
|
|
|
|
|
return Result.DONE;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
public override void Abort()
|
|
|
|
|
{
|
|
|
|
|
base.Abort();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
}
|