Navigation State Machine Pseudo-Code 

{
  Declare local variable ReturnEvent
  assume no errors
  
  switch based on current state
  {
If current state is initial Psedudo State
          only respond to EF_Init
          {
              Initialize PWM hardware
              now put the machine into the actual initial state
              Set both motors off
              Start with robot stationary              
              Initialize robot at starting positions
              Query starting orientation from game play state machine
              Record starting time
              Initialize interrupts
              Start clock to delay first dead reckoning calculation                
           }
      Break switch on event type

      If current state is running
          Base response on event type
          {
            If timeout
               {
                  Save values of encoder counts before calculations
                  Record timestep in timer tics
                  Record starting time for next calculation
                  Calculate timestep in seconds
                  Calculate Angular speed of left wheel in degrees per second
                  Calculate Angular speed of right wheel in degrees per second
                  ReStart timer for RPM updates
                  Update last encoder counts for next calculation
               }
               Break on Timeout
                               
            If the reckoning interrupt timer expired, time to recalculate robot position
               {
                  Save values of encoder counts before calculations
                  Record timestep in timer tics
                  Record starting time for next calculation
                  Calculate timestep in seconds
                  
                  Query new desired wheel speed values from DrivingSM
                  Calculate Angular speed of left wheel in degrees per second
                  Calculate Angular speed of right wheel in degrees per second
                  Use Mobile robot kinematics to calculate forward and angular robot velocity
                  Update robot positions/orientation based on velocities and current position/orientation
                  Keep orientation value between 0 and 360
                  
                  if navigation start delay hasn’t passed yet
                  {
                     Give time for motors to stabilize before putting robot on ground and starting dead reckoning calculations
                  }                   
                  If the motors arent moving
                  {
                     if it should be moving
                     {
                       Increment number of periods with motor stall
                       if motorStallCount > motorStallThreshold
                       {
                        Post motor stall event to gameplay state machine
                        Reset motor stall count value
                       }
                     }
                  }
                  
                  Update last encoder counts for next calculation
               }
          Break on Reckoning Update
          
          If need to read ostart from gameplayHSM
          {
             Query starting orientation from game play state machine
          }
          Break on Reset orientation
          
          If the gameplay state machine has new info and wants to reset the location
          {
             Query new values from gameplay state machine and set as current location
          }
          Break on Reset Location
               
               
          }  end switch on Event type
      break;
        

  } end switch on Current State
  return ReturnEvent;
}

void interrupt _Vec_tim0ch6 LeftEncoderTimer(void)
{
   
   If Return 1 if PORTT 3 is high, 0 if low
   {
      Use quadrature to increment/decrement encoder count
   } 
   Clear flag on T2
} 


void interrupt _Vec_tim1ch4 RightEncoderTimer(void)
{
   
   If Return 1 if PORTT 5 is high, 0 if low
   {
      Use quadrature to increment/decrement encoder count
   } 
   Clear flag on T4
} 

void interrupt _Vec_tim1ch6 MotorSpeedControlTimer(void)
{
   Increment timer compare
   Clear flag on T6
   EnableInterrupts;

   Calculate speed error
   Add on error from this time step to integrated error
   Update duty cycles with PI control
   Caps and anti-windup on left motor
   Caps and antiwindup on right motor
   Update PWM output on U0 for left motor
   Update PWM output on U1 for right motor
   
}  

void interrupt _Vec_tim1ch7 ReckoningUpdateTimer(void)
{
   Clear interrupt flag
   Set up next compare event

   EnableInterrupts;
   
   Post update events to navigation and driving state machines 
}