FredUtil-Arduino/fredStateMachine.h at master · fredlllll/FredUtil-Arduino · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
#ifndef FREDSTATEMACHINE_H
#define FREDSTATEMACHINE_H

//#define USEENTERFUNCTIONS
//#define USELEAVEFUNCTIONS

typedef int (*stateFunction)();
typedef void (*stateEnterLeaveHandler)(int);

const int noState = -1;

template<int maxstatecount> class fredStateMachine {
private:
#ifdef USEENTERFUNCTIONS
    stateEnterLeaveHandler stateEnterHandlers[statecount];
#endif
#ifdef USELEAVEFUNCTIONS
    stateEnterLeaveHandler stateLeaveHandlers[statecount];
#endif
    stateFunction stateFunctions[maxstatecount];
    volatile int _state;
    int statecount;
public:
    fredStateMachine() :
            _state(noState), statecount(0) {
    }

    void loop() {
        if (_state != noState) {
            auto stateFunc = stateFunctions[_state];
            if (stateFunc) {
                int newState = stateFunc();
                changeState(newState);
            }
        }
    }

    int getState() {
        return _state;
    }

    //directly setting the state, no enter or leave functions are called
    void setState(int state) {
        _state = state;
    }

    //changing state (if newState is not noState). this will call enter and leave functions
    void changeState(int newState) {
        if (newState != noState) {
#ifdef USELEAVEFUNCTIONS
            if (_state != noState) {
                stateEnterLeaveHandler leaveFunc = stateLeaveHandlers[_state];
                if (leaveFunc) {
                    leaveFunc(newState);
                }
            }
#endif
#ifdef USEENTERFUNCTIONS
            int oldState = _state;
#endif
            _state = newState;
#ifdef USEENTERFUNCTIONS
            stateEnterLeaveHandler enterFunc = stateEnterHandlers[_state];
            if (enterFunc) {
                enterFunc(oldState);
            }
#endif
        }
    }

    int addStateFunction(stateFunction function) {
        int index = statecount;
        statecount++;
        stateFunctions[index] = function;
        return index;
    }

#ifdef USEENTERFUNCTIONS
    void setStateEnterHandler(int state, stateEnterLeaveHandler handler) {
        stateEnterHandlers[state] = handler;
    }
#endif

#ifdef USELEAVEFUNCTIONS
    void setStateLeaveHandler(int state, stateEnterLeaveHandler handler) {
        stateLeaveHandlers[state] = handler;
    }
#endif
};
#endif