Maszyna stanów do robota miniSUMO

Powoli i do przodu idzie mi uczenie się programowania mikrokontrolerów AVR. Na razie moim środowiskiem uruchomieniowym jest robot miniSUMO.

Logika w głównej pętli wykonawczej będzie oparta o prostą maszynę stanów. Napisanie i wrzucenie maszyny stanów na uC na początek nie jest dobrym pomysłem, ponieważ nie ma widocznego sprzężenia zwrotnego. Postanowiłem więc napisać prostą aplikację testową.

Graf maszyny stanów

Podstawowa struktura programu
1. Inicjalizacja licznika pomocniczego i ustawienie początkowego stanu.
2. Wejście w pętlę nieskończoną do stanu początkowego.
3. W danym stanie, wypisanie informacji o danym stanie. Oczekiwanie na zdarzenie w pętli while. Tutaj też na potrzeby aplikacji testowej jest zwiększany licznik pomocniczy, gdy osiągnie odpowiednią wartość, odpowiednie zdarzenie jest ustawiane, które przerwie pętlę oczekiwania.
4. Jak przyjdzie zdarzenie, sprawdzenie czy dany stan czeka na to konkretne zdarzenie. Wyzerowanie zmiennej ze zdarzeniami i zmiana stanu.
5. Kolejny obrót pętli głównej wskakuje w odpowiedni stan.

Kod aplikacji

#include <stdio.h>
#include <stdlib.h>

#define WAIT_COUNT 2

#define STATE1 1
#define STATE2 2
#define STATE3 3

#define EVENT0 0
#define EVENT0_M 0x1
#define EVENT1 1
#define EVENT1_M 0x2

int state = 0; int events = 0;

int main(void) {
  int count = 0, toggle_events = 0;

  state = STATE1;

  while(1) {
    switch(state) {
      case STATE1: {
          printf("State1\n");
          count = 0;

          while(!(events & EVENT0_M)) {
            printf("%d: I'm in state1. Waiting for Event0.\n", count);
            sleep(1);
            count++;

            if(count == WAIT_COUNT) {
              events = 1<<EVENT0;
            }
          }
          if(events & EVENT0_M) {
            printf("Event0 received, changing to state2.\n\n");
            state = STATE2;
            events = 0;
          }
          break;
        }
        case STATE2: {
            printf("State2\n");
            count = 0;

            while(!(events & EVENT1_M)) {
              printf("%d: I'm in state2. Waiting for Event1.\n", count);
              sleep(1);
              count++;

              if(count == WAIT_COUNT) {
                events = 1<<EVENT1;
              }
            }
            if(events & EVENT1_M) {
              printf("Event1 received, changing to state3.\n\n");
              state = STATE3;
              events = 0;
            }
            break;
          }
          case STATE3: {
            printf("State3\n");
            count = 0;

            while(!(events & (EVENT0_M | EVENT1_M))) {
              printf("%d: I'm in state3."
                     " Waiting for Event1 or Event2.\n", count);
              sleep(1);
              count++;

              if(count == WAIT_COUNT) {
                events = (toggle_events) ? 1<<EVENT0 : 1<<EVENT1;
              }
            }

            if(events & EVENT0_M) {
              printf("Event0 received, changing to state1.\n\n");
              state = STATE1;
              toggle_events = 0;
              events = 0;
            }
            else if(events & EVENT1_M) {
              printf("Event1 received changing, to state2.\n\n");
              state = STATE2;
              toggle_events = 1;
              events = 0;
            }
            break;
          }
          default: {
            printf("Error. Undefined state. Exit.\n");
            exit(-1);
            break;
          }
        }
  }

  return 0;
}

Opis działania
W każdym ze stanów jest pętla while, w której oczekujemy na konkretne zdarzenie. Ze strony implementacji jest to operacja maskowania bitowego zmiennej zawierającej zdarzenia z maską zdarzenia. Po przerwaniu pętli sprawdzane jest, które zdarzenie zakończyło oczekiwanie i podjęte są odpowiednie kroki. W tym miejscu jest zerowana (dla uproszczenia, powinno się w sumie zerować tylko dane zdarzenie) zmienna zawierająca zdarzenia i ustawiany odpowiedni stan. Licznik pomocniczy jest zwiększany i jak będzie równy czasowi oczekiwania, to ustawiamy zdarzenie, które przerwie pętle. Jest to sztuczny zabieg, dzięki któremu mogę zasymulować ustawianie zdarzeń. Na wejściu do każdego stanu, licznik pomocniczy jest zerowany. W stanie 3, w zależności od zmiennej ‘toggle_events’ jest ustawiane inne zdarzenie, co powoduje, że z tego stanu są dwie ścieżki wyjścia, do stanu 1 i 2.

Kompilacja i uruchomienie powyższego

gcc -o test main.c && ./test

Wynik działania programu

State1
0: I'm in state1. Waiting for Event0.
1: I'm in state1. Waiting for Event0.
Event0 received, changing to state2.

State2
0: I'm in state2. Waiting for Event1.
1: I'm in state2. Waiting for Event1.
Event1 received, changing to state3.

State3
0: I'm in state3. Waiting for Event1 or Event2.
1: I'm in state3. Waiting for Event1 or Event2.
Event1 received changing, to state2.

State2
0: I'm in state2. Waiting for Event1.
1: I'm in state2. Waiting for Event1.
Event1 received, changing to state3.

State3
0: I'm in state3. Waiting for Event1 or Event2.
1: I'm in state3. Waiting for Event1 or Event2.
Event0 received, changing to state1.

State1
0: I'm in state1. Waiting for Event0.
1: I'm in state1. Waiting for Event0.
Event0 received, changing to state2.

Podsumowanie
Jak widać aplikacja jest prosta i spełnia swoje zadania. Stany są odpowiednio przełączane przez wystąpienie zdarzeń. W prawdziwym kodzie do robota, zdarzenia będą ustawiane albo po czasie albo po odczytaniu stanu czujników. Zdarzenie czasowe i odczyt czujników odbywać się będą w funkcji obsługi przerwania wywołanego przez timer0.

Advertisements

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s