// Linked data structure
struct State {
  unsigned long out;        // 2-bit output 
  unsigned long delay;      // time to delay in 10ms
  unsigned long next[4];};  // Next if 2-bit input is 0-3
typedef const struct State StateType;
typedef StateType *StatePtr;
#define Center 0
#define Left 1
#define Right 2
StateType fsm[3]={
  {0x03, 1, { Right, Left,   Right,  Center }},  // Center
  {0x02, 1, { Left,  Center, Right,  Center }},  // Left
  {0x01, 1, { Right, Left,   Center, Center }}   // Right
};
 
unsigned long S;       // index to the current state 
unsigned long Input; 
unsigned long Output;
int main(void){ 
  Robot_Init();
  SysTick_Init();   // Program 10.2
  S = Center; 

  while(1){
    Output = fsm[S].out;      // set output from FSM
    Robot_Output(Output);     // do output to two motors
    SysTick_Wait10ms(fsm[S].delay); // wait
    Input = Robot_Input();    // read sensors
    S = fsm[S].next[Input];   // next depends on input and state 
  }
}