Created
April 13, 2018 22:04
-
-
Save ozel/9c5c71e209e670a4a38061cd7896890a to your computer and use it in GitHub Desktop.
OpenAuto FSM demo code
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// This file is part of arduino-fsm. | |
// | |
// arduino-fsm is free software: you can redistribute it and/or modify it under | |
// the terms of the GNU Lesser General Public License as published by the Free | |
// Software Foundation, either version 3 of the License, or (at your option) | |
// any later version. | |
// | |
// arduino-fsm is distributed in the hope that it will be useful, but WITHOUT | |
// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or | |
// FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License | |
// for more details. | |
// | |
// You should have received a copy of the GNU Lesser General Public License | |
// along with arduino-fsm. If not, see <http://www.gnu.org/licenses/>. | |
#include "Fsm.h" | |
State::State(void (*on_enter)(), void (*on_state)(), void (*on_exit)()) | |
: on_enter(on_enter), | |
on_state(on_state), | |
on_exit(on_exit) | |
{ | |
} | |
Fsm::Fsm(State* initial_state) | |
: m_current_state(initial_state), | |
m_transitions(NULL), | |
m_num_transitions(0), | |
m_num_timed_transitions(0), | |
m_initialized(false) | |
{ | |
} | |
Fsm::~Fsm() | |
{ | |
free(m_transitions); | |
free(m_timed_transitions); | |
m_transitions = NULL; | |
m_timed_transitions = NULL; | |
} | |
void Fsm::add_transition(State* state_from, State* state_to, int event, | |
void (*on_transition)()) | |
{ | |
if (state_from == NULL || state_to == NULL) | |
return; | |
Transition transition = Fsm::create_transition(state_from, state_to, event, | |
on_transition); | |
m_transitions = (Transition*) realloc(m_transitions, (m_num_transitions + 1) | |
* sizeof(Transition)); | |
m_transitions[m_num_transitions] = transition; | |
m_num_transitions++; | |
} | |
void Fsm::add_timed_transition(State* state_from, State* state_to, | |
unsigned long interval, void (*on_transition)()) | |
{ | |
if (state_from == NULL || state_to == NULL) | |
return; | |
Transition transition = Fsm::create_transition(state_from, state_to, 0, | |
on_transition); | |
TimedTransition timed_transition; | |
timed_transition.transition = transition; | |
timed_transition.start = 0; | |
timed_transition.interval = interval; | |
m_timed_transitions = (TimedTransition*) realloc( | |
m_timed_transitions, (m_num_timed_transitions + 1) * sizeof(TimedTransition)); | |
m_timed_transitions[m_num_timed_transitions] = timed_transition; | |
m_num_timed_transitions++; | |
} | |
Fsm::Transition Fsm::create_transition(State* state_from, State* state_to, | |
int event, void (*on_transition)()) | |
{ | |
Transition t; | |
t.state_from = state_from; | |
t.state_to = state_to; | |
t.event = event; | |
t.on_transition = on_transition; | |
return t; | |
} | |
void Fsm::trigger(int event) | |
{ | |
if (m_initialized) | |
{ | |
// Find the transition with the current state and given event. | |
for (int i = 0; i < m_num_transitions; ++i) | |
{ | |
if (m_transitions[i].state_from == m_current_state && | |
m_transitions[i].event == event) | |
{ | |
Fsm::make_transition(&(m_transitions[i])); | |
return; | |
} | |
} | |
} | |
} | |
void Fsm::check_timed_transitions() | |
{ | |
for (int i = 0; i < m_num_timed_transitions; ++i) | |
{ | |
TimedTransition* transition = &m_timed_transitions[i]; | |
if (transition->transition.state_from == m_current_state) | |
{ | |
if (transition->start == 0) | |
{ | |
transition->start = millis(); | |
} | |
else{ | |
unsigned long now = millis(); | |
if (now - transition->start >= transition->interval) | |
{ | |
Fsm::make_transition(&(transition->transition)); | |
transition->start = 0; | |
} | |
} | |
} | |
} | |
} | |
void Fsm::run_machine() | |
{ | |
// first run must exec first state "on_enter" | |
if (!m_initialized) | |
{ | |
m_initialized = true; | |
if (m_current_state->on_enter != NULL) | |
m_current_state->on_enter(); | |
} | |
if (m_current_state->on_state != NULL) | |
m_current_state->on_state(); | |
Fsm::check_timed_transitions(); | |
} | |
void Fsm::make_transition(Transition* transition) | |
{ | |
// Execute the handlers in the correct order. | |
if (transition->state_from->on_exit != NULL) | |
transition->state_from->on_exit(); | |
if (transition->on_transition != NULL) | |
transition->on_transition(); | |
if (transition->state_to->on_enter != NULL) | |
transition->state_to->on_enter(); | |
m_current_state = transition->state_to; | |
//Initialice all timed transitions from m_current_state | |
unsigned long now = millis(); | |
for (int i = 0; i < m_num_timed_transitions; ++i) | |
{ | |
TimedTransition* ttransition = &m_timed_transitions[i]; | |
if (ttransition->transition.state_from == m_current_state) | |
ttransition->start = now; | |
} | |
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// This file is part of arduino-fsm. | |
// | |
// arduino-fsm is free software: you can redistribute it and/or modify it under | |
// the terms of the GNU Lesser General Public License as published by the Free | |
// Software Foundation, either version 3 of the License, or (at your option) | |
// any later version. | |
// | |
// arduino-fsm is distributed in the hope that it will be useful, but WITHOUT | |
// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or | |
// FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License | |
// for more details. | |
// | |
// You should have received a copy of the GNU Lesser General Public License | |
// along with arduino-fsm. If not, see <http://www.gnu.org/licenses/>. | |
#ifndef FSM_H | |
#define FSM_H | |
#if defined(ARDUINO) && ARDUINO >= 100 | |
#include <Arduino.h> | |
#else | |
#include <WProgram.h> | |
#endif | |
struct State | |
{ | |
State(void (*on_enter)(), void (*on_state)(), void (*on_exit)()); | |
void (*on_enter)(); | |
void (*on_state)(); | |
void (*on_exit)(); | |
}; | |
class Fsm | |
{ | |
public: | |
Fsm(State* initial_state); | |
~Fsm(); | |
void add_transition(State* state_from, State* state_to, int event, | |
void (*on_transition)()); | |
void add_timed_transition(State* state_from, State* state_to, | |
unsigned long interval, void (*on_transition)()); | |
void check_timed_transitions(); | |
void trigger(int event); | |
void run_machine(); | |
private: | |
struct Transition | |
{ | |
State* state_from; | |
State* state_to; | |
int event; | |
void (*on_transition)(); | |
}; | |
struct TimedTransition | |
{ | |
Transition transition; | |
unsigned long start; | |
unsigned long interval; | |
}; | |
static Transition create_transition(State* state_from, State* state_to, | |
int event, void (*on_transition)()); | |
void make_transition(Transition* transition); | |
private: | |
State* m_current_state; | |
Transition* m_transitions; | |
int m_num_transitions; | |
TimedTransition* m_timed_transitions; | |
int m_num_timed_transitions; | |
bool m_initialized; | |
}; | |
#endif |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#include "Fsm.h" | |
/* | |
* FSM Library sample with user and timed | |
* transitions. | |
* Uses a button and Arduino builtin led, | |
* button can be replaced just grounding | |
* pin. | |
*/ | |
// Used pins | |
#define LED_PIN D7 | |
#define BUTTON_PIN D6 | |
//Events | |
#define BOOKING_RECEIVED 0 | |
#define RESERVATION_STARTED 1 | |
#define RESERVATION_ENDED 2 | |
/* state 1: led off | |
* state 2: led on | |
* transition from s1 to s2 on button press | |
* transition back from s2 to s1 after 3 seconds or button press | |
*/ | |
State state_waiting(&enter_waiting, &check_server, NULL); | |
State state_reserved(&enter_reserved, &check_reservations, NULL); | |
State state_active(&enter_active, &check_endOfReservation, NULL); | |
// State state_led_off(&led_off, &check_button, NULL); | |
// State state_led_on(&led_on, &check_button, NULL); | |
Fsm fsm(&state_waiting); | |
static int lastButtonState = HIGH; | |
void enter_waiting() | |
{ | |
Serial.println("WAITING - for new reservation from server"); | |
digitalWrite(LED_PIN, LOW); | |
} | |
void enter_reserved() | |
{ | |
Serial.println("RESERVED - waiting for reservation to start"); | |
digitalWrite(LED_PIN, HIGH); | |
} | |
void enter_active() | |
{ | |
Serial.println("ACTIVE - reservation active"); | |
digitalWrite(LED_PIN, HIGH); | |
} | |
void check_server() | |
{ | |
int buttonState = digitalRead(BUTTON_PIN); | |
if (buttonState == LOW && lastButtonState == HIGH) { | |
Serial.println("received reservation"); | |
fsm.trigger(BOOKING_RECEIVED); | |
lastButtonState = LOW; | |
} else { | |
lastButtonState = buttonState; | |
} | |
} | |
void check_reservations() | |
{ | |
int buttonState = digitalRead(BUTTON_PIN); | |
static int state=HIGH; | |
state = !state; | |
digitalWrite(LED_PIN, state); | |
delay(50); //FXIME: this is just for demo, production grade code should | |
//handle a blinking LED with by a separate FSM and timed transitions! | |
if (buttonState == LOW && lastButtonState == HIGH) { | |
Serial.println("reservation activated"); | |
fsm.trigger(RESERVATION_STARTED); | |
lastButtonState = LOW; | |
} else { | |
lastButtonState = buttonState; | |
} | |
} | |
void check_endOfReservation() | |
{ | |
int buttonState = digitalRead(BUTTON_PIN); | |
if (buttonState == LOW && lastButtonState == HIGH) { | |
Serial.println("reservation ended early"); | |
fsm.trigger(RESERVATION_ENDED); | |
lastButtonState = LOW; | |
} else { | |
lastButtonState = buttonState; | |
} | |
} | |
void expired(){ | |
Serial.println("reservation expired"); | |
} | |
// standard arduino functions | |
void setup() | |
{ | |
Serial.begin(9600); | |
pinMode(LED_PIN, OUTPUT); | |
pinMode(BUTTON_PIN, INPUT); | |
fsm.add_transition(&state_waiting, &state_reserved, BOOKING_RECEIVED, NULL); | |
fsm.add_transition(&state_reserved, &state_active, RESERVATION_STARTED, NULL); | |
fsm.add_transition(&state_active, &state_waiting, RESERVATION_ENDED, NULL); | |
fsm.add_timed_transition(&state_active, &state_waiting, 5000, expired); | |
Serial.println("Setup finished"); | |
} | |
void loop() | |
{ | |
// Call fsm run | |
fsm.run_machine(); | |
delay(200); | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
the FSM lib comes from here: https://github.com/jonblack/arduino-fsm