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
No comments:
Post a Comment