goalfinding_sm Module

Files:

Interface

Function: StartGoalFindingSM
Arguments: None.
Returns: None.
Does any required initialization for the goalfinding state machine.

Function: RunGoalFindingSM
Arguments: Event_t: the event to process.
Returns: Event_t: an event to return.
Runs the tasks for the goalfinding state machine.

Function: QueryGoalFindingSM
Arguments: None.
Returns: GoalFindingState_t The current state of the goalfinding state machine.
Runs the tasks for the goalfinding state machine.

Pseudo-Code

StartGoalFindingSM
  Initialize module variable CurrentState to FINDING_GOAL

  Call RunGoalFindingSM(EV_ENTRY)

RunGoalFindingSM(Takes an Event_t and returns new Event_t)
  Initialize MakeTransition to FALSE
  Set NextState to CurrentState

  Switch between values of CurrentState:

    If value is FINDING_GOAL:
      Call DuringFindingGoal(CurrentEvent) and store the return in CurrentEvent

      Switch between values of CurrentEvent:
        if value is EV_AT_GOAL_DIRECTION:
          Set NextState to HEADING_TO_GOAL
          Set MakeTransition to TRUE
        Endif
      Endswitch
    Endif

    If value is HEADING_TO_GOAL:
      Call DuringHeadingToGoal(CurrentEvent) and store the return in CurrentEvent

      Switch between values of CurrentEvent:
        if value is EV_HIT_GREEN_PAINT:
          Set NextState to ORIENTING_IN_GOAL
          Set MakeTransition to TRUE
        Endif
      Endswitch
    Endif

    If value is ORIENTING_IN_GOAL:
      Call DuringOrientingInGoal(CurrentEvent) and store the return in CurrentEvent

      Switch between values of CurrentEvent:
        if value is EV_IN_GOAL:
          Set NextState to WAITING
          Set MakeTransition to TRUE
        Endif
      Endswitch
    Endif

    If value is WAITING:
      Call DuringWaiting(CurrentEvent) and store the return in CurrentEvent

      Switch between values of CurrentEvent:
        if value is EV_MISALIGNED:
          Set NextState to ORIENTING_IN_GOAL
          Set MakeTransition to TRUE
        Endif
      Endswitch
    Endif

  Endswitch

  If MakeTransition is TRUE:
    Call RunGoalFindingSM(EV_EXIT)
    Set CurrentState to NextState
    Call RunGoalFindingSM(EV_ENTRY)
  Endif

  return CurrentEvent

QueryGoalFindingSM(Returns current GoalFindingState_t)
  return module variable CurrentState

DuringFindingGoal(Takes current event Event_t, returns new event Event_t)
  If the event is EV_ENTRY:
    Turn left
  Endif

  Return same event

DuringHeadingToGoal(Takes current event Event_t, returns new event Event_t)
  If the event is not EV_EXIT or EV_ENTRY:
    If event is EV_HIT_BLACK_PAINT:
      Go straight slowly
    Else:
      Go straight
    Endif
  Endif

  Return same event

DuringOrientingInGoal(Takes current event Event_t, returns new event Event_t)
  If the event is not EV_EXIT or EV_ENTRY:
    If both the front left and right tape sensors do not read green paint
     or both the front left and right tape sensors do read green paint:
      Move forward
    Else if the front left tape sensor reads green paint
     and the right front tape sensor reads black:
      Turn left
    Else if the front left tape sensor reads black
     and the right front tape sensor reads green paint:
      Turn right
    Endif
  Endif

  Return same event

DuringWaiting(Takes current event Event_t, returns new event Event_t)
  If the event is EV_ENTRY:
    Do not move
  Endif

  Return same event