|
@@ -27,7 +27,8 @@ namespace Venus_RT.Modules
|
|
|
{
|
|
|
Unknown,
|
|
|
Init,
|
|
|
- Initializing,
|
|
|
+ Initializing,
|
|
|
+ InitializingRB,
|
|
|
Idle,
|
|
|
Error,
|
|
|
Pumping,
|
|
@@ -51,7 +52,8 @@ namespace Venus_RT.Modules
|
|
|
|
|
|
public enum MSG
|
|
|
{
|
|
|
- Home,
|
|
|
+ Home,
|
|
|
+ RobotHome,
|
|
|
Online,
|
|
|
Offline,
|
|
|
Pump,
|
|
@@ -173,7 +175,8 @@ namespace Venus_RT.Modules
|
|
|
|
|
|
protected override bool Init()
|
|
|
{
|
|
|
- OP.Subscribe("TM.Home", (cmd, args) => CheckToPostMessage((int)MSG.Home));
|
|
|
+ OP.Subscribe("TM.Home", (cmd, args) => CheckToPostMessage((int)MSG.Home, args));
|
|
|
+ OP.Subscribe("TM.RobotHome", (cmd, args) => CheckToPostMessage((int)MSG.RobotHome, args));
|
|
|
OP.Subscribe($"TM.{RtOperation.LLPick}", (cmd, args) => CheckToPostMessage((int)MSG.Pick, args));
|
|
|
OP.Subscribe($"TM.{RtOperation.LLPlace}", (cmd, args) => CheckToPostMessage((int)MSG.Place, args));
|
|
|
OP.Subscribe($"TM.{RtOperation.PMPick}", (cmd, args) => CheckToPostMessage((int)MSG.PMPick, args));
|
|
@@ -232,8 +235,12 @@ namespace Venus_RT.Modules
|
|
|
Transition(STATE.Initializing, FSM_MSG.TIMER, fnHoming, STATE.Idle);
|
|
|
Transition(STATE.Initializing, MSG.Abort, FnAbortExtend, STATE.Idle);
|
|
|
|
|
|
- Transition(STATE.Idle, FSM_MSG.TIMER, fnMonitor, STATE.Idle);
|
|
|
- Transition(STATE.Init, FSM_MSG.TIMER, fnMonitor, STATE.Init);
|
|
|
+ // Robot Home
|
|
|
+ Transition(STATE.Idle, MSG.RobotHome, fnHome, STATE.InitializingRB);
|
|
|
+ Transition(STATE.InitializingRB, FSM_MSG.TIMER, fnHoming, STATE.Idle);
|
|
|
+ Transition(STATE.InitializingRB, MSG.Abort, FnAbortExtend, STATE.Idle);
|
|
|
+ //Transition(STATE.Idle, FSM_MSG.TIMER, fnMonitor, STATE.Idle);
|
|
|
+ //Transition(STATE.Init, FSM_MSG.TIMER, fnMonitor, STATE.Init);
|
|
|
|
|
|
// Vent sequence
|
|
|
Transition(STATE.Idle, MSG.Vent, FnStartVent, STATE.Venting);
|
|
@@ -343,7 +350,12 @@ namespace Venus_RT.Modules
|
|
|
|
|
|
private bool fnHome(object[] param)
|
|
|
{
|
|
|
- return _homeRoutine.Start() == RState.Running;
|
|
|
+ if (fsm.State == (int)STATE.Init && param.Length > 0)//带参home
|
|
|
+ {
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ return _homeRoutine.Start(param) == RState.Running;
|
|
|
}
|
|
|
|
|
|
private bool fnHoming(object[] param)
|