Finite State Machine in RAPID
in RobotStudio
Here's my attempt at implementing a Finite State Machine in RAPID. It's just a skeleton example with a few typical states and state transitions. Comments or suggestions are welcome.
MODULE MainModule PROC main() TPWriteProc "main","BEGIN"; FSM_Initialise; FSM_StateRegister; TPWriteProc "main","END"; ERROR TPWriteProc "main.ERROR","@\n:=ClkRead(clock1\HighRes); ENDPROC ENDMODULE
ALIAS num StateID;
ALIAS string StateName;
RECORD State
StateID id;
StateName name;
bool transition;
StateID next_state_id;
ENDRECORD
!
! State Table
!
CONST StateID FSM_MAX_STATES:=6;
VAR State States{FSM_MAX_STATES}:=
[
[1,"EStop",FALSE,2],
[2,"Reset",FALSE,3],
[3,"Initialise",FALSE,4],
[4,"PickAndPlace",FALSE,5],
[5,"Done",FALSE,6],
[6,"Stop",FALSE,6]
];
VAR State current_state;
VAR State next_state;
VAR bool FSM_run:=TRUE;
PROC FSM_Initialise()
current_state:=States{1};
next_state:=States{1};
ENDPROC
FUNC bool FSM_IsValidState(State s)
VAR bool valid:=FALSE;
IF s.id>=1 AND s.id<=FSM_MAX_STATES THEN
valid:=TRUE;
ELSE
TPWriteProc "FSM_IsValidState","Invalid state="\n:=s.id;
FSM_run:=FALSE;
Stop;
ENDIF
RETURN valid;
ENDFUNC
PROC FSM_NextStateLogic()
VAR bool valid_state:=FALSE;
VAR bool transition:=FALSE;
valid_state:=FSM_IsValidState(current_state);
IF valid_state THEN
transition:=current_state.transition;
IF transition THEN
! Reset transition flag of current state.
current_state.transition:=FALSE;
! Reset transition flag of next state.
States{current_state.next_state_id}.transition:=FALSE;
! Select next state.
current_state:=States{current_state.next_state_id};
ENDIF
ENDIF
ENDPROC
PROC FSM_OutputLogic()
VAR bool valid_state:=FALSE;
VAR string call_routine;
valid_state:=FSM_IsValidState(current_state);
IF valid_state THEN
!CallByVar "FSM_State",FSM_current_state;
call_routine:="FiniteStateMachine_States:FSM_State_"+current_state.name;
%call_routine %;
current_state.transition:=TRUE;
ENDIF
ERROR
IF ERRNO=ERR_CALLPROC THEN
TPWriteProc "FSM_OutputLogic","Routine not defined: "+call_routine;
Stop;
ENDIF
ENDPROC
PROC FSM_StateRegister()
WHILE FSM_run DO
TPWriteProc "FSM_StateRegister","cycle@"\n:=ClkRead(clock1\HighRes);
FSM_OutputLogic;
FSM_NextStateLogic;
ENDWHILE
Stop;
ENDPROC
ENDMODULEMODULE FiniteStateMachine
!
! BEGIN: Routines for the states defined in the state table.
!
PROC FSM_State_EStop()
ENDPROC
PROC FSM_State_Reset()
ENDPROC
PROC FSM_State_Initialise()
ENDPROC
PROC FSM_State_PickAndPlace()
ENDPROC
PROC FSM_State_Done()
ENDPROC
PROC FSM_State_Stop()
ENDPROC
!
! END: Routines for the states defined in the state table.
!
ENDMODULEMODULE FiniteStateMachine_States PROC TPWriteProc(string sProcName,string s,\num n|bool b|pos p|orient o|dnum d)
VAR string str;
str:=sProcName+" "+s;
IF Present(n) THEN
TPWrite str\num:=n;
ELSEIF Present(b) THEN
TPWrite str\bool:=b;
ELSEIF Present(p) THEN
TPWrite str\pos:=p;
ELSEIF Present(o) THEN
TPWrite str\orient:=o;
ELSEIF Present(d) THEN
TPWrite str\dnum:=d;
ELSE
TPWrite str;
ENDIF
ERROR
TPWrite "TPWriteProc.ERROR @"\num:=ClkRead(clock1\HighRes);
ENDPROC
ENDMODULEMODULE Helper0
Comments
-
Hello. Looks interesting.
I'm a little rusty, with the FSM theory, But I hope my comment makes sense.
As I see it, you code can more or less be rewritten to:<div>MODULE simple</div><div> VAR num SimpleState;</div><div><br></div><div> CONST num ESTOP:=1;</div><div> CONST num RESET:=2;</div><div> CONST num INITIALIZE:=3;</div><div> CONST num PICKANDPLACE:=4;</div><div> CONST num DONE:=5;</div><div> CONST num SSTOP:=6;</div><div><br></div><div> LOCAL PROC main()</div><div> SimpleState:=ESTOP;</div><div> WHILE TRUE DO</div><div> SimpleStateMachine;</div><div> ENDWHILE</div><div> ENDPROC</div><div><br></div><div> LOCAL PROC SimpleStateMachine()</div><div> TEST SimpleState</div><div> CASE ESTOP:</div><div> FSM_OutputLogic "EStop";</div><div> SimpleState:=RESET;</div><div> CASE RESET:</div><div> FSM_OutputLogic "RESET";</div><div> SimpleState:=INITIALIZE;</div><div> CASE INITIALIZE:</div><div> FSM_OutputLogic "INITIALIZE";</div><div> SimpleState:=PICKANDPLACE;</div><div> CASE PICKANDPLACE:</div><div> FSM_OutputLogic "PICKANDPLACE";</div><div> SimpleState:=DONE;</div><div> CASE DONE:</div><div> FSM_OutputLogic "DONE";</div><div> SimpleState:=SSTOP;</div><div> CASE SSTOP:</div><div> FSM_OutputLogic "STOP";</div><div> SimpleState:=SSTOP;</div><div> DEFAULT:</div><div> ENDTEST</div><div> ENDPROC</div><div><br></div><div> LOCAL PROC FSM_OutputLogic(string stateName)</div><div> VAR string call_routine;</div><div><br></div><div> call_routine:="FiniteStateMachine_States:FSM_State_"+stateName;</div><div> %call_routine %;</div><div> ENDPROC</div><div><br></div><div>ENDMODULE</div>
I understand you effort to write a proper FSM. But for that, I think you need a propor Transition Table, which I think is also the hard on to implement in RAPID.
But without the possibility to another state depending on the input, the state machine becomes kind of useless, for me at least.
So if you are able to implement a transition table, with input, that you would find in a real world, then I think it would be really nice.
a few more comments on more specific parts of you code:<div> PROC FSM_NextStateLogic()</div><div> VAR bool valid_state:=FALSE;</div><div> VAR bool transition:=FALSE;</div><div><br></div><div> valid_state:=FSM_IsValidState(current_state);</div><div><br></div><div> IF valid_state THEN</div><div> transition:=current_state.transition;</div><div> IF transition THEN</div><div><br></div><div> ! Reset transition flag of current state.</div><div> current_state.transition:=FALSE;</div><div><br></div><div> ! Reset transition flag of next state.</div><div> States{current_state.next_state_id}.transition:=FALSE;</div><div><br></div><div> ! Select next state.</div><div> current_state:=States{current_state.next_state_id};</div><div><br></div><div> ENDIF</div><div> ENDIF</div><div> ENDPROC</div>why validate the current state here? wouldn't be better to validate the next state?
And that do you use you transition flag for? as I see it you just set it before making the transition, instead of just doing the transition?
I think that there are missing some transition condition? or are that suppose to be in the state them self?0 -
Hope my code looks better in you browser that mine
0 -
I think you need a propor Transition Table, which I think is also the hard on to implement in RAPID.Good point. My overall aim is to separate the transition table from the state machine. The state table could be generated in your favourite logic programme, e.g MS Excel, and then output the table as a CSV file which could be read by the Rapid code during FSM_Initialise to initialise the State array VAR.
0
Categories
- All Categories
- 5.5K RobotStudio
- 395 UpFeed
- 18 Tutorials
- 13 RobotApps
- 297 PowerPacs
- 405 RobotStudio S4
- 1.8K Developer Tools
- 249 ScreenMaker
- 2.7K Robot Controller
- 310 IRC5
- 59 OmniCore
- 7 RCS (Realistic Controller Simulation)
- 785 RAPID Programming
- AppStudio
- 3 RobotStudio AR Viewer
- 18 Wizard Easy Programming
- 105 Collaborative Robots
- 4 Job listings