Saturday, August 25, 2012

State Machine Function Calls

I had a project which controlled up to six motors. The motors ran asynchronously and were controlled via a CAN bus. To make the system more deterministic and simpler, no RTOS was used. Everything was controlled by state machines. Logically we could consider each motor having its own "thread" and its own collection of state machines. But since all of the motors performed identical operations they could share code even though each could be executing different parts of that code. Only the data for each motor had to be unique -- like homing states or motor positions.

State machines can result in some ugly code if you're not careful. They're really nothing but glorified GOTO statements. To make the code more maintainable and more efficient I wanted to simulate nested function calls. And I wanted each function call to behave like a blocking call. But each call was actually going to be to yet another state machine.

You can't block a state machine. It kind of defeats the purpose. If one motor "thread" was blocked then all "threads" stop. None of the other motors could proceed with their states. State machines have to keep running.

Let's say StateMachine1 calls StateMachine2 which calls StateMachine3 and this machine is in a waiting mode. Then on each cycle we have to traverse from SM1 down to SM3, test a condition, then back out. Normally there's no good reason to do this. SM1 is simply waiting for SM3 to finish. Why bother calling SM1 and SM2 when we know we're in SM3? It would be better if an outer loop simply proceeds directly to SM3.

I came up with a simple way to do this. Each motor "thread" is really a data structure with some global information and a reference to be used for some local information. At a minimum globally we need a function address which is the currently executing state machine, and the current state of that machine. At a minimum the local information, which could almost be considered stack information, has the parent's state machine and its state. The outermost loop simply executes an indirect function call to the appropriate machine passing a pointer to that thread's global data structure. The state machine uses the permanent thread number to reference that thread's local data for that machine. Each state machine contains its data and only knows about its data. When one state machine "calls" another, the calling code copies the parent's address and state into the child's data structure and puts the child's address into the common data structure. In other words, each "thread" has a global data structure plus a changeable portion which is local only to the currently executing state machine. Each state machine has its own data structure for each possible thread. In my case that was six data structure, one for each motor. So each motor is assigned a thread number and this thread number is used as an index into its private data area in each state machine.

Note that the state machines must have two system-known states (or cases). SM_INITIALIZE is used to load the global area's local data pointer. SM_ENTRY is always the first state of the machine on a call, that is, it's the logical entry point. Internal states begin at START_INTERNAL_STATES.

Here is sm.h which defines the global data structures:
 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
// sm.h

typedef void (*tStateMachine)( void* thread );

typedef struct{
  tStateMachine  caller;
  int            callerstate;
  void *         callerslocal;
} tLocalData;


typedef struct{
  tStateMachine  currentmachine;  // executable function
  int            state;     // execute this state in the function
  tLocalData *   local;     // points to the current local data to use while executing
  tLocalData *   retlocal;    // saved pointer to child's data... this may be used to return data to parent from child
  int            id;     // id is set once and never changes
  int            status;    // can return status
} dThread, *tThread;


enum {
       SM_INITIALIZE=1,
       SM_ENTRY
     };

#define EXECUTE(thread) (*thread->currentmachine)(thread)

#define CALL_THEN_RETURNTO(sm, ret)  gp->state=ret;   \
        CallStateMachine( gp, (tStateMachine ) &sm )

#define START_INTERNAL_STATES 100

Here is sm.c which contains the core setup, call, and return functions:
 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
// ----- fail-safe -------------------------------------------------------------------

static void nocaller( void * thread )
{
  // should never get here!
}

//============================================================================
//=======================[ StartStateMachine ]================================
//============================================================================

// This is used to setup the top-most state machine. It will "call" other
// state-machines in its thread.  

static void StartThread( tThread thread, tStateMachine targetmachine, int id )
{
   thread->state=SM_INITIALIZE;          // each s-m must support this
   thread->id=id;                        // an instance identification for the target state-machine
   (*targetmachine)( thread );           // get the pointer to local data
   thread->local->caller=&nocaller;      // a dummy caller
   thread->local->callerstate=0;         // state is irrelevant
   thread->currentmachine=targetmachine; // this is the top-most state-machine
   thread->state=SM_ENTRY;               // this is where we always start
}

//============================================================================
//=======================[ CallStateMachine ]=================================
//============================================================================

// This saves the state of the parent state-machine into the
// child's data area. When the child eventually returns this
// saved status will allow the parent to resume execution at
// its next state. When a child is called, no code in the
// parent is executed until the child returns. So this scheme
// behaves like a regular function call.
//
// The local pointer is changed by this call. This pointer always
// points to the child's local data. And since the parent will 
// regain control immediately after CallStateMachine() returns, 
// but prior to the child's actual execution, the parent may pass
// parameters to the child through this local data structure. 
// When this is required, both parent and child know the structure
// of the child's data area. This is handled by selective 
// "switched" includes of a common ".h" file. 

static void CallStateMachine( tThread thread, tStateMachine targetmachine )
{
   int savestate;
   tLocalData * savelocal;
      
   savestate=thread->state;               // temporarily save the state
   savelocal=thread->local;               // temporarily save local data ptr
   
   thread->state=SM_INITIALIZE;
   (*targetmachine)( thread );            // get the pointer to the child's local data
                                          // thread->local is now changed
   thread->local->caller=thread->currentmachine; // save the parent's address so we can return
   thread->local->callerstate=savestate;  // also remember the parent's state (where we will return)
   thread->local->callerslocal=savelocal; // also remember the parent's local data
   thread->currentmachine=targetmachine;  // set the new (child) state-machine
   thread->state=SM_ENTRY;                // this is where we always start a child
}

//============================================================================
//=======================[ ReturnToCaller ]===================================
//============================================================================

static void ReturnToCaller( tThread p )
{
  p->currentmachine = p->local->caller; // return to the previous (parent) machine
  p->state=p->local->callerstate;       // restore the parent's state
  p->retlocal=p->local;
  p->local=p->local->callerslocal;      // restore pointer to parent's local data
  // Parent's status can be set in every state-machine.
  // Data is still saved in p->local, now moved to p->retlocal, so
  // if need be, we can return data since the parent
  // now has a pointer to the child's local (updated) data.
}

And here is a simple test program which is mostly stubbed out.
  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
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
enum {
       MOTOR1,
       MOTOR2,
       MOTOR3,
       MOTOR4,
       MOTOR5,
       MOTOR6,
       MAX_ID  // # of instances needed
     };

// prototypes
static void InitializeMotor( tThread gp );
static void SendBatch( tThread gp );
static void TransferData( tThread gp );
static void MotorThread( tThread gp );  // top state machine

static int done=1; // dummy value
static int error=0; // dummy value
static char motorparams[]={ 1,2,3,4,0 }; // dummy values
static char motorposition[]={ 5,6,7,8,0 }; // dummy values

#define TRANSF_ERROR 1

//------ MotorThread --------------------------------------------------------------------------
//
// This handles position updates.
// Plus, this starts EPOS and starts homing.

static void MotorThread( tThread gp )
{
   // internal states
   enum {
          STARTUP=START_INTERNAL_STATES,
          MOTORIDLE,
          MOTORMOVE,
        };

   typedef struct {
      tStateMachine caller;
      int callerstate;
      void * callerslocal;
   } tMotorLocals;
   
   #define LOCAL_BATCH
     #include "smlocals.h"

   static tMotorLocals local[MAX_ID]; // local instance variables
   tMotorLocals * lp; // pointer to the local data
   int gotdata=0;

   lp=&local[gp->id]; // first, load pointer to the local data

   switch( gp->state ) // then just jump to the current state
   {
      case SM_INITIALIZE:
        gp->local=(tLocalData*) lp;  // this is always required
        break;

      case SM_ENTRY:  // always required as first state
     
      case STARTUP:
        CALL_THEN_RETURNTO( InitializeMotor, MOTORIDLE );
        break;
               
      case MOTORIDLE:
        // call something to check for position input
        if( gotdata ) gp->state=MOTORMOVE;
        break;
        
      case MOTORMOVE:
        // let another state machine take over ...
        // ... then resume at the idle state
        CALL_THEN_RETURNTO( SendBatch, MOTORIDLE ); // call and advance
        ( (tBatchLocals*) gp->local )->packet=motorposition;  // can pass variables
        ( (tBatchLocals*) gp->local )->currentitem=0;
        break;
         
   } // end switch (state)
}

//------ InitializeMotor --------------------------------------------------------------------------
//
// This sends motor parameters to the motor controller.
// For example, Maxon EPOS controllers need to know what
// motors they are controlling, speeds, currents, and PID loops, etc.

static void InitializeMotor( tThread gp )
{
   // internal states
   enum {
          STARTUP_1=START_INTERNAL_STATES, // internal states begin after ...
          STARTUP_2,        // ... common states so there's no overlap
          STARTUP_DONE
        };

  // setup the data structures this state machine will use   
  #define LOCAL_BATCH
  #define LOCAL_STARTUP
    #include "smlocals.h"

   static tStartupLocals local[MAX_ID];  // allocate the local instances
   tStartupLocals * lp; // use lp as a pointer to local data

   lp=&local[gp->id];   // always setup the local pointer first

   switch( gp->state )  // execute the current state
   {
      case SM_INITIALIZE:
        gp->local=(tLocalData*) lp; // sets the pointer to instance data
        break;

      case SM_ENTRY:
        gp->state=STARTUP_1;        // this is how to advance to the next state
        break;
        
      case STARTUP_1:
        if( done ) gp->state=STARTUP_2;  // or advance on a flag
        break;

      case STARTUP_2:
        CALL_THEN_RETURNTO( SendBatch, STARTUP_DONE ); // call and advance
        ( (tBatchLocals*) gp->local )->packet=motorparams;  // can pass variables
        ( (tBatchLocals*) gp->local )->currentitem=0;
        break;
        
      case STARTUP_DONE:
        ReturnToCaller( gp ); // transfers back to the parent state machine
        break;

   } // end switch (state)
}

// ----- SendBatch ----------------------------------------------------------------------------------
//
// This will send a batch of data to the motor controller.
// Typically this will be on CAN or serial and will consist
// of a batch of 1 or more packets of some protocol.

static void SendBatch( tThread gp )
{
   // internal states
   enum { B_DONE=START_INTERNAL_STATES, // all packets were sent
          B_START_TRANSACTION,  // sends data to a motor controller
          B_START_ITEM,
          B_SENT
        };

  #define LOCAL_BATCH
  #define LOCAL_TRANSF
   #include "smlocals.h"
   
   static tBatchLocals local[MAX_ID];
   tBatchLocals * lp;
   int more=0;

   lp=&local[gp->id];

   switch( gp->state )
   {
      case SM_INITIALIZE:
        gp->local=(tLocalData*) lp;
        break;
      
      case SM_ENTRY:

      case B_START_ITEM:
         // setup stuff here like buffers, packet pointers
         gp->state=B_START_TRANSACTION; // next state
         break;

      case B_START_TRANSACTION:         // starts tramsmitting 1 packet
         CALL_THEN_RETURNTO( TransferData, B_SENT );
         ( (tTransfLocals*) gp->local )->retries=3; // pass variables
         break;

      case B_SENT:
         // add code to check if more packets need sending
         if( more ) gp->state=B_START_ITEM;
         if( done ) gp->state=B_DONE;              // next state
         break;
         
      case B_DONE:
         ReturnToCaller( gp );
         break;

   } // end switch (machine state)
}

// ----- TransferData -------------------------------------------------------------------------------
//
// This will send a batch of data to the motor controller.
// Typically this will be on CAN or serial and will consist
// of a batch of 1 or more packets of some protocol.

static void TransferData( tThread gp )
{
   // internal states
   enum { T_DONE=START_INTERNAL_STATES, // all packets were sent
          T_START_ITEM,         // starts new transaction based on options
          T_TX,
          T_ACK,
          T_RETRY,              // previous operation failed, retrying
        };

  #define LOCAL_TRANSF
    #include "smlocals.h"
   
   static tTransfLocals local[MAX_ID];
   tTransfLocals * lp;

   lp=&local[gp->id];

   switch( gp->state )
   {
      case SM_INITIALIZE:
        gp->local=(tLocalData*) lp;
        break;
      
      case SM_ENTRY:
        gp->status=0; // clear status;

      case T_TX:
         // call a function or yet another state machine to start
         // sending the data
         gp->state=T_ACK; // next state
         break;

      case T_ACK:
         // maybe wait on an ack, then:
         if( done ) gp->state=T_DONE;
         if( error ) gp->state=T_RETRY;
         break;

      case T_RETRY:
        if( lp->retries ) // see if any reties remain
         {
           lp->retries--;  // yes, do another try
           gp->state=T_TX;
         }
         else
         {
           // no reties reain, set some error condition and exit
           gp->status=TRANSF_ERROR;  // global status is easy to set
           gp->state=T_DONE;
         }
         break;
         
      case T_DONE:
         ReturnToCaller( gp );
         break;

   } // end switch (machine state)
}

static dThread Motor1Thread;
static tThread pMotor1Thread=&Motor1Thread;
static dThread Motor2Thread;
static tThread pMotor2Thread=&Motor2Thread;
static dThread Motor3Thread;
static tThread pMotor3Thread=&Motor3Thread;
static dThread Motor4Thread;
static tThread pMotor4Thread=&Motor4Thread;
static dThread Motor5Thread;
static tThread pMotor5Thread=&Motor5Thread;
static dThread Motor6Thread;
static tThread pMotor6Thread=&Motor6Thread;

// This is the main (outermost) loop. All we need to do is call 
// this periodically to advance all state machines.  Probably at 
// least one communication state machine must be added to handle
// the serial port or other communication method.

static void ExecuteAllStates( void )
{
  EXECUTE( pMotor1Thread );
  EXECUTE( pMotor2Thread );
  EXECUTE( pMotor3Thread );
  EXECUTE( pMotor4Thread );
  EXECUTE( pMotor5Thread );
  EXECUTE( pMotor6Thread );
}

static void InitializeTest( void )
{
  StartThread( pMotor1Thread, (tStateMachine) &MotorThread, MOTOR1 );
  StartThread( pMotor2Thread, (tStateMachine) &MotorThread, MOTOR2 );
  StartThread( pMotor3Thread, (tStateMachine) &MotorThread, MOTOR3 );
  StartThread( pMotor4Thread, (tStateMachine) &MotorThread, MOTOR4 );
  StartThread( pMotor5Thread, (tStateMachine) &MotorThread, MOTOR5 );
  StartThread( pMotor6Thread, (tStateMachine) &MotorThread, MOTOR6 );
}


InitializeTest should be called once then ExecuteAllStates should be called in an infinite loop after that.

As I hope you can see, no matter how deep we nest the state machine calls we never pay for it with a time penalty. Execution always proceeds directly to the currently executing state. We merely load a couple of pointers and execute a case in a switch statement. The biggest penalty is with the call itself but this occurs only once to get into that state machine. While we're there that penalty disappears.


Finally, here is smlocals.h which is where I defined the local data structures:
 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
#ifdef LOCAL_STARTUP

   typedef struct {
      tStateMachine caller;
      int callerstate;
      void * callerslocal;
   } tStartupLocals;

#endif 
#undef LOCAL_STARTUP


#ifdef LOCAL_BATCH

   typedef struct {
      tStateMachine caller;
      int callerstate;
      void * callerslocal;
   char * packet;
      int       currentitem;
   } tBatchLocals;

#endif 
#undef LOCAL_BATCH

#ifdef LOCAL_TRANSF

   typedef struct {
      tStateMachine caller;
      int callerstate;
      void * callerslocal;
   int retries;
   int status;
   char * buffer;
   int count;
   } tTransfLocals;

#endif 
#undef LOCAL_TRANSF

This method of defining local data structures is a bit uncommon and may be unnecessary. As a policy I try to hide data from functions which don't need to know about that data. It may be overkill. Basically, within functions (state machines) I switch in the appropriate structure with one or more #define statements. These flags enable only the structures used by that function -- that it, its own and others it may need to know to pass variables to something it calls.

-- Don Jindra


Index

Created August 2012