首次上传
This commit is contained in:
+217
@@ -0,0 +1,217 @@
|
||||
using Aitex.Core.Common.DeviceData;
|
||||
using Aitex.Core.RT.DataCenter;
|
||||
using Aitex.Core.RT.Device.Devices;
|
||||
using Aitex.Core.RT.Device.Unit;
|
||||
using Aitex.Core.Util;
|
||||
using System.Threading;
|
||||
|
||||
namespace MECF.Framework.RT.EquipmentLibrary.HardwareUnits.WalkingAixs
|
||||
{
|
||||
public class IOWalkingAxisHH :WalkingAxisBaseDevice
|
||||
{
|
||||
private R_TRIG _trigMonitorError = new R_TRIG();
|
||||
public IOWalkingAxisHH(string module,string name,IoSensor[] dis,IoTrigger[] dos):base(module,name)
|
||||
{
|
||||
_diPosFeedback0 = dis[0];
|
||||
_diPosFeedback1 = dis[1];
|
||||
_diPosFeedback2 = dis[2];
|
||||
|
||||
_diReady = dis[3];
|
||||
_diOnTarget = dis[4];
|
||||
_diOnError = dis[5];
|
||||
_diOnLeftLimit = dis[6];
|
||||
_diOnRightLimit = dis[7];
|
||||
_diHomeSensor = dis[8];
|
||||
|
||||
_doStartMoving = dos[0];
|
||||
_doPosSet0 = dos[1];
|
||||
_doPosSet1 = dos[2];
|
||||
_doPosSet2 = dos[3];
|
||||
|
||||
_doHome = dos[4];
|
||||
_doSetFree = dos[5];
|
||||
_doStop = dos[6];
|
||||
_doResetAlarm = dos[7];
|
||||
_doJogFwd = dos[8];
|
||||
_doJogRev = dos[9];
|
||||
|
||||
DATA.Subscribe($"{Module}.{Name}.DeviceData", () => DeviceData);
|
||||
}
|
||||
private readonly IoSensor _diPosFeedback0;
|
||||
private readonly IoSensor _diPosFeedback1;
|
||||
private readonly IoSensor _diPosFeedback2;
|
||||
private readonly IoSensor _diReady;
|
||||
private readonly IoSensor _diOnTarget;
|
||||
private readonly IoSensor _diOnError;
|
||||
private readonly IoSensor _diOnLeftLimit;
|
||||
private readonly IoSensor _diOnRightLimit;
|
||||
private readonly IoSensor _diHomeSensor;
|
||||
|
||||
private readonly IoTrigger _doStartMoving;
|
||||
private readonly IoTrigger _doPosSet0;
|
||||
private readonly IoTrigger _doPosSet1;
|
||||
private readonly IoTrigger _doPosSet2;
|
||||
|
||||
private readonly IoTrigger _doHome;
|
||||
private readonly IoTrigger _doSetFree;
|
||||
private readonly IoTrigger _doStop;
|
||||
private readonly IoTrigger _doResetAlarm;
|
||||
private readonly IoTrigger _doJogFwd;
|
||||
private readonly IoTrigger _doJogRev;
|
||||
|
||||
public override void Monitor()
|
||||
{
|
||||
_trigMonitorError.CLK = _diOnError.Value;
|
||||
if (_trigMonitorError.Q)
|
||||
OnError(null);
|
||||
}
|
||||
|
||||
protected override bool fStop(object[] param)
|
||||
{
|
||||
|
||||
_doStartMoving.SetTrigger(false, out _);
|
||||
_doPosSet0.SetTrigger(false, out _);
|
||||
_doPosSet1.SetTrigger(false, out _);
|
||||
_doPosSet2.SetTrigger(false, out _);
|
||||
_doHome.SetTrigger(false, out _);
|
||||
_doSetFree.SetTrigger(false, out _);
|
||||
_doStop.SetTrigger(true, out _);
|
||||
_doResetAlarm.SetTrigger(false, out _);
|
||||
_doJogFwd.SetTrigger(false, out _);
|
||||
_doJogRev.SetTrigger(false, out _);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
return base.fStop(param);
|
||||
}
|
||||
|
||||
protected override bool fMonitorInit(object[] param)
|
||||
{
|
||||
|
||||
if(_diHomeSensor.Value && _diOnTarget.Value && _diHomeSensor.Value && _diReady.Value)
|
||||
OnActionDone(param);
|
||||
return true;
|
||||
}
|
||||
|
||||
protected override bool fMonitorMove(object[] param)
|
||||
{
|
||||
if (_diOnTarget.Value && _diReady.Value && GetCurrentStation() == TargetStation)
|
||||
OnActionDone(param);
|
||||
|
||||
return true; ;
|
||||
}
|
||||
|
||||
protected override bool fStartInit(object[] param)
|
||||
{
|
||||
_doStop.SetTrigger(false, out _);
|
||||
_doJogRev.SetTrigger(false, out _);
|
||||
_doJogFwd.SetTrigger(false, out _);
|
||||
_doSetFree.SetTrigger(false, out _);
|
||||
_doResetAlarm.SetTrigger(false, out _);
|
||||
_doStartMoving.SetTrigger(false, out _);
|
||||
_doHome.SetTrigger(true,out _);
|
||||
return true;
|
||||
}
|
||||
protected override bool fInitComplete(object[] param)
|
||||
{
|
||||
_doHome.SetTrigger(false, out _);
|
||||
return true;
|
||||
}
|
||||
|
||||
protected override bool fStartMove(object[] param)
|
||||
{
|
||||
int stationIndex = (int)param[0];
|
||||
if (stationIndex > 7 || stationIndex <0) return false;
|
||||
_doPosSet0.SetTrigger((stationIndex & 0x1) == 0x1, out _);
|
||||
_doPosSet1.SetTrigger((stationIndex & 0x2) == 0x2, out _);
|
||||
_doPosSet2.SetTrigger((stationIndex & 0x4) == 0x4, out _);
|
||||
Thread.Sleep(200);
|
||||
_doStartMoving.SetTrigger(true, out _);
|
||||
Thread.Sleep(500);
|
||||
return true;
|
||||
|
||||
}
|
||||
protected override bool fMoveComplete(object[] param)
|
||||
{
|
||||
_doStartMoving.SetTrigger(false, out _);
|
||||
_doPosSet0.SetTrigger(false, out _);
|
||||
_doPosSet1.SetTrigger(false, out _);
|
||||
_doPosSet2.SetTrigger(false, out _);
|
||||
return true;
|
||||
}
|
||||
|
||||
protected override bool fStartReset(object[] param)
|
||||
{
|
||||
_doStartMoving.SetTrigger(false, out _);
|
||||
_doPosSet0.SetTrigger(false, out _);
|
||||
_doPosSet1.SetTrigger(false, out _);
|
||||
_doPosSet2.SetTrigger(false, out _);
|
||||
_doHome.SetTrigger(false, out _);
|
||||
_doSetFree.SetTrigger(false, out _);
|
||||
_doStop.SetTrigger(false, out _);
|
||||
//_doResetAlarm.SetTrigger(false, out _);
|
||||
_doJogFwd.SetTrigger(false, out _);
|
||||
_doJogRev.SetTrigger(false, out _);
|
||||
_doResetAlarm.SetTrigger(true, out _);
|
||||
Thread.Sleep(1000);
|
||||
_doResetAlarm.SetTrigger(false, out _);
|
||||
return true;
|
||||
}
|
||||
|
||||
public override bool IsReady()
|
||||
{
|
||||
if (_diOnError.Value) return false;
|
||||
if (_diOnLeftLimit.Value) return false;
|
||||
if (_diOnRightLimit.Value) return false;
|
||||
return base.IsReady();
|
||||
}
|
||||
public override int GetCurrentStation()
|
||||
{
|
||||
return (_diPosFeedback0.Value ? 1 : 0) + (_diPosFeedback1.Value ? 2 : 0) +
|
||||
(_diPosFeedback2.Value ? 4 : 0);
|
||||
|
||||
|
||||
}
|
||||
private AITServoMotorData DeviceData
|
||||
{
|
||||
get
|
||||
{
|
||||
return new AITServoMotorData()
|
||||
{
|
||||
DiPosFeedBack1 = _diPosFeedback0.Value,
|
||||
DiPosFeedBack2 = _diPosFeedback1.Value,
|
||||
DiPosFeedBack3 = _diPosFeedback2.Value,
|
||||
DiReady = _diReady.Value,
|
||||
DiOnTarget = _diOnTarget.Value,
|
||||
DiOnError = _diOnError.Value,
|
||||
DiOnLeftLimit = _diOnLeftLimit.Value,
|
||||
DiOnRightLimit = _diOnRightLimit.Value,
|
||||
DiOnHomeSensor = _diHomeSensor.Value,
|
||||
|
||||
DoStart = _doStartMoving.Value,
|
||||
DoPos1 = _doPosSet0.Value,
|
||||
DoPos2 = _doPosSet1.Value,
|
||||
DoPos3 = _doPosSet2.Value,
|
||||
DoHomeOn = _doHome.Value,
|
||||
DoFreeOn = _doSetFree.Value,
|
||||
DoStop = _doStop.Value,
|
||||
DoReset = _doResetAlarm.Value,
|
||||
DoJogFwd = _doJogFwd.Value,
|
||||
DoJogRev = _doJogRev.Value,
|
||||
|
||||
CurrentStatus = DeviceState.ToString(),
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//State = _state,
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
+256
@@ -0,0 +1,256 @@
|
||||
using Aitex.Core.RT.Device;
|
||||
using Aitex.Core.RT.Event;
|
||||
using Aitex.Core.RT.Fsm;
|
||||
using Aitex.Core.RT.DataCenter;
|
||||
using System;
|
||||
using Aitex.Core.RT.OperationCenter;
|
||||
using MECF.Framework.Common.Event;
|
||||
|
||||
namespace MECF.Framework.RT.EquipmentLibrary.HardwareUnits.WalkingAixs
|
||||
{
|
||||
public abstract class WalkingAxisBaseDevice : Entity, IDevice
|
||||
{
|
||||
public bool IsBusy { get; private set; }
|
||||
protected WalkingAxisBaseDevice(string module, string name)
|
||||
: base()
|
||||
{
|
||||
Module = module;
|
||||
Name = name;
|
||||
InitializeMotion();
|
||||
|
||||
}
|
||||
|
||||
private void InitializeMotion()
|
||||
{
|
||||
BuildTransitionTable();
|
||||
SubscribeDataVariable();
|
||||
|
||||
SubscribeOperation();
|
||||
Running = true;
|
||||
|
||||
}
|
||||
|
||||
public event Action<string, AlarmEventItem> OnDeviceAlarmStateChanged;
|
||||
public bool HasAlarm { get; }
|
||||
|
||||
public string Module { get; set; }
|
||||
public string Name { get; set ; }
|
||||
public WalkingAxisState DeviceState { get => (WalkingAxisState)fsm.State; }
|
||||
public abstract int GetCurrentStation();
|
||||
|
||||
public bool IsArrivedTarget
|
||||
{
|
||||
get { return (TargetStation == GetCurrentStation()); }
|
||||
}
|
||||
public int TargetStation { get; private set; }
|
||||
public abstract void Monitor();
|
||||
|
||||
public void Reset()
|
||||
{
|
||||
|
||||
}
|
||||
public enum WalkingAxisMsg
|
||||
{
|
||||
Reset,
|
||||
Init,
|
||||
StartMove,
|
||||
Stop,
|
||||
Error,
|
||||
ActionDone,
|
||||
}
|
||||
public enum WalkingAxisState
|
||||
{
|
||||
Init,
|
||||
Initializing,
|
||||
Idle,
|
||||
Moving,
|
||||
ERROR,
|
||||
Stopped,
|
||||
Busy,
|
||||
}
|
||||
|
||||
|
||||
private void BuildTransitionTable()
|
||||
{
|
||||
fsm = new StateMachine<WalkingAxisBaseDevice>(Module + Name + ".StateMachine", (int)WalkingAxisState.Init, 50);
|
||||
|
||||
AnyStateTransition(WalkingAxisMsg.Error, fError, WalkingAxisState.ERROR);
|
||||
AnyStateTransition(WalkingAxisMsg.Stop, fStop, WalkingAxisState.Stopped);
|
||||
|
||||
Transition(WalkingAxisState.Init, WalkingAxisMsg.Init, fStartInit, WalkingAxisState.Initializing);
|
||||
Transition(WalkingAxisState.Initializing, FSM_MSG.TIMER, fMonitorInit, WalkingAxisState.Initializing);
|
||||
Transition(WalkingAxisState.Initializing, WalkingAxisMsg.ActionDone, fInitComplete, WalkingAxisState.Idle);
|
||||
|
||||
Transition(WalkingAxisState.Idle, WalkingAxisMsg.Init, fStartInit, WalkingAxisState.Initializing);
|
||||
Transition(WalkingAxisState.Idle, WalkingAxisMsg.StartMove, fStartMove, WalkingAxisState.Moving);
|
||||
|
||||
Transition(WalkingAxisState.Moving, FSM_MSG.TIMER, fMonitorMove, WalkingAxisState.Moving);
|
||||
Transition(WalkingAxisState.Moving, WalkingAxisMsg.ActionDone, fMoveComplete, WalkingAxisState.Idle);
|
||||
|
||||
Transition(WalkingAxisState.ERROR, WalkingAxisMsg.Reset, fStartReset, WalkingAxisState.Init);
|
||||
Transition(WalkingAxisState.Stopped, WalkingAxisMsg.Reset, fStartReset, WalkingAxisState.Init);
|
||||
Transition(WalkingAxisState.Idle, WalkingAxisMsg.Reset, fStartReset, WalkingAxisState.Idle);
|
||||
Transition(WalkingAxisState.Init, WalkingAxisMsg.Reset, fStartReset, WalkingAxisState.Idle);
|
||||
|
||||
|
||||
}
|
||||
|
||||
public virtual bool IsReady()
|
||||
{
|
||||
return ((!IsBusy) && (DeviceState == WalkingAxisState.Idle));
|
||||
}
|
||||
|
||||
private void SubscribeDataVariable()
|
||||
{
|
||||
DATA.Subscribe($"{Module}.{Name}.State", () => DeviceState.ToString());
|
||||
}
|
||||
private void SubscribeOperation()
|
||||
{
|
||||
OP.Subscribe($"{Module}.{Name}.Home", (string cmd, object[] param) =>
|
||||
{
|
||||
string reason = "";
|
||||
if (!Home(null))
|
||||
{
|
||||
|
||||
EV.PostWarningLog(Module, $"{Name} can not home, {reason}");
|
||||
return false;
|
||||
}
|
||||
EV.PostInfoLog(Module, $"{Name} home");
|
||||
return true;
|
||||
});
|
||||
|
||||
OP.Subscribe($"{Module}.{Name}.Reset", (string cmd, object[] param) =>
|
||||
{
|
||||
string reason = "";
|
||||
if (!Reset(null))
|
||||
{
|
||||
EV.PostWarningLog(Module, $"{Name} can not clear alarm, {reason}");
|
||||
return false;
|
||||
}
|
||||
EV.PostInfoLog(Module, $"{Name} reset alarm");
|
||||
return true;
|
||||
});
|
||||
OP.Subscribe($"{Module}.{Name}.MoveTo", (string cmd, object[] param) =>
|
||||
{
|
||||
string reason = "";
|
||||
if (!MoveTo(param))
|
||||
{
|
||||
EV.PostWarningLog(Module, $"{Name} can not move to {param[0].ToString()}, {reason}");
|
||||
return false;
|
||||
}
|
||||
EV.PostInfoLog(Module, $"{Name} move to {param[0]}");
|
||||
return true;
|
||||
});
|
||||
OP.Subscribe($"{Module}.{Name}.Stop", (string cmd, object[] param) =>
|
||||
{
|
||||
string reason = "";
|
||||
if (!Stop(null))
|
||||
{
|
||||
|
||||
EV.PostWarningLog(Module, $"{Name} can not Stop, {reason}");
|
||||
return false;
|
||||
}
|
||||
EV.PostInfoLog(Module, $"{Name} Stop");
|
||||
return true;
|
||||
});
|
||||
|
||||
}
|
||||
protected virtual bool fStop(object[] param)
|
||||
{
|
||||
IsBusy = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
protected abstract bool fStartReset(object[] param);
|
||||
|
||||
protected abstract bool fMonitorMove(object[] param);
|
||||
protected abstract bool fStartMove(object[] param);
|
||||
|
||||
|
||||
protected abstract bool fMonitorInit(object[] param);
|
||||
protected abstract bool fStartInit(object[] param);
|
||||
|
||||
protected virtual bool fError(object[] param)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
protected virtual bool fInitComplete(object[] param)
|
||||
{
|
||||
IsBusy = false;
|
||||
return true;
|
||||
}
|
||||
protected virtual bool fMoveComplete(object[] param)
|
||||
{
|
||||
IsBusy = false;
|
||||
return true;
|
||||
}
|
||||
public virtual bool Home(object[] param)
|
||||
{
|
||||
IsBusy = true;
|
||||
return CheckToPostMessage((int)WalkingAxisMsg.Init, param);
|
||||
}
|
||||
public virtual bool MoveTo(object[] param)
|
||||
{
|
||||
int temp;
|
||||
if (int.TryParse(param[0].ToString(), out temp))
|
||||
{
|
||||
TargetStation = temp;
|
||||
IsBusy = true;
|
||||
return CheckToPostMessage((int)WalkingAxisMsg.StartMove, new object[] { temp});
|
||||
}
|
||||
return false;
|
||||
|
||||
|
||||
}
|
||||
public virtual bool Reset(object[] param)
|
||||
{
|
||||
IsBusy = false;
|
||||
return CheckToPostMessage((int)WalkingAxisMsg.Reset, param);
|
||||
}
|
||||
public virtual bool Stop(object[] param)
|
||||
{
|
||||
IsBusy = true;
|
||||
return CheckToPostMessage((int)WalkingAxisMsg.Stop, param);
|
||||
}
|
||||
|
||||
|
||||
public virtual bool OnActionDone(object[] param)
|
||||
{
|
||||
IsBusy = false;
|
||||
return CheckToPostMessage((int)WalkingAxisMsg.ActionDone, param);
|
||||
}
|
||||
public virtual bool OnError(object[] param)
|
||||
{
|
||||
return CheckToPostMessage((int)WalkingAxisMsg.Error, param);
|
||||
}
|
||||
|
||||
public bool CheckToPostMessage(int msg, params object[] args)
|
||||
{
|
||||
if (!fsm.FindTransition(fsm.State, msg))
|
||||
{
|
||||
EV.PostWarningLog(Name, $"{Name} is in { (WalkingAxisState)fsm.State} state,can not do {(WalkingAxisMsg)msg}");
|
||||
return false;
|
||||
}
|
||||
|
||||
fsm.PostMsg(msg, args);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public bool Check(int msg, out string reason, params object[] args)
|
||||
{
|
||||
if (!fsm.FindTransition(fsm.State, msg))
|
||||
{
|
||||
reason = String.Format("{0} is in {1} state,can not do {2}", Name, (WalkingAxisState)fsm.State, (WalkingAxisMsg)msg);
|
||||
return false;
|
||||
}
|
||||
|
||||
reason = "";
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user