Finite State Machine in RAPID

RobotStudio
1

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


# MODULE FiniteStateMachine
    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

ENDMODULE

# MODULE FiniteStateMachine_States
    !
    ! 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.
    !

ENDMODULE

# MODULE Helper    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
ENDMODULE
2

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:


MODULE simple
    VAR num SimpleState;

    CONST num ESTOP:=1;
    CONST num RESET:=2;
    CONST num INITIALIZE:=3;
    CONST num PICKANDPLACE:=4;
    CONST num DONE:=5;
    CONST num SSTOP:=6;

    LOCAL PROC main()
        SimpleState:=ESTOP;
        WHILE TRUE DO
            SimpleStateMachine;
        ENDWHILE
    ENDPROC

    LOCAL PROC SimpleStateMachine()
        TEST SimpleState
        CASE ESTOP:
            FSM_OutputLogic "EStop";
            SimpleState:=RESET;
        CASE RESET:
            FSM_OutputLogic "RESET";
            SimpleState:=INITIALIZE;
        CASE INITIALIZE:
            FSM_OutputLogic "INITIALIZE";
            SimpleState:=PICKANDPLACE;
        CASE PICKANDPLACE:
            FSM_OutputLogic "PICKANDPLACE";
            SimpleState:=DONE;
        CASE DONE:
            FSM_OutputLogic "DONE";
            SimpleState:=SSTOP;
        CASE SSTOP:
            FSM_OutputLogic "STOP";
            SimpleState:=SSTOP;
        DEFAULT:
        ENDTEST
    ENDPROC

    LOCAL PROC FSM_OutputLogic(string stateName)
        VAR string call_routine;

        call_routine:="FiniteStateMachine_States:FSM_State_"+stateName;
        %call_routine %;
    ENDPROC

ENDMODULE

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:


    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

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?

3

Hope my code looks better in you browser that mine :confused: :cry:

4

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.