Created
March 7, 2020 13:19
-
-
Save jpf91/16c3cfb324dbfb39adea50410c01842d to your computer and use it in GitHub Desktop.
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
import std.meta, std.traits; | |
version = DebugPrint; | |
version (DebugPrint) | |
{ | |
import std.experimental.logger; | |
alias log = infof; | |
extern(C) uint _currTime() | |
{ | |
import std.datetime; | |
static SysTime first; | |
if (first == SysTime.init) | |
first = Clock.currTime; | |
return cast(uint)(Clock.currTime - first).total!"msecs"; | |
} | |
extern(C) void _sideEffect() {} | |
} | |
else | |
{ | |
void log(Args...)(Args args) | |
{ | |
} | |
extern(C) uint _currTime(); | |
extern(C) void _sideEffect(); | |
} | |
// https://github.com/Superstar64/tagged_union/blob/master/source/tagged_union.d | |
struct TaggedUnion(Types_...) | |
{ | |
alias Types = Types_; | |
private size_t index = size_t.max; | |
union | |
{ | |
private Types data; | |
} | |
this(T)(T type) if (IndexOf!T != -1) | |
{ | |
this = type; | |
} | |
auto ref opAssign(T)(T type) if (IndexOf!T != -1) | |
{ | |
set!T(type); | |
} | |
auto opEquals(const typeof(this) other) const | |
{ | |
if (id == other.id) | |
{ | |
if (id == size_t.max) | |
return true; | |
foreach (c, Type; Types) | |
{ | |
if (id == c) | |
{ | |
return getID!c == other.getID!c; | |
} | |
} | |
} | |
return false; | |
} | |
auto opEquals(T)(const T type) const if (IndexOf!T != -1) | |
{ | |
if (id == IndexOf!T) | |
{ | |
return type == get!T; | |
} | |
return false; | |
} | |
@property: | |
auto id() const | |
{ | |
return index; | |
} | |
pure @trusted auto ref getID(size_t id)() inout if (id < Types.length) | |
{ | |
// Create too much code... | |
//assert(index == id); | |
return data[id]; | |
} | |
pure @trusted auto ref setID(size_t id)(Types[id] type) if (id < Types.length) | |
{ | |
index = id; | |
data[id] = type; | |
} | |
pure @safe bool isType(Type)() inout if (IndexOf!Type != -1) | |
{ | |
return IndexOf!Type == id; | |
} | |
alias peek = isType; | |
pure @safe auto ref get(Type)() inout if (IndexOf!Type != -1) | |
{ | |
return getID!(IndexOf!Type); | |
} | |
alias get(size_t i) = getID!i; | |
pure @safe auto ref set(Type)(Type type) if (IndexOf!Type != -1) | |
{ | |
return setID!(IndexOf!Type)(type); | |
} | |
alias set(size_t i) = setID!i; | |
string toString() | |
{ | |
foreach (c, Type; Types) | |
{ | |
if (id == c) | |
{ | |
return Type.stringof; | |
} | |
} | |
return "invalid"; | |
} | |
template IndexOf(Type) | |
{ | |
import std.typetuple : staticIndexOf; | |
enum IndexOf = staticIndexOf!(Type, Types); | |
} | |
} | |
void updateMembers(T)(ref T parent, uint now) | |
{ | |
// Any members with update functions? | |
foreach(m; __traits(allMembers, T)) | |
{ | |
static if(__traits(compiles, __traits(getMember, parent, m).update(0))) | |
__traits(getMember, parent, m).update(now); | |
} | |
} | |
mixin template FSMLogic(FSM) | |
{ | |
alias State = TaggedUnion!(FSM.States); | |
State _state, _nextState; | |
size_t prevState; | |
bool onEnter = false; | |
uint now = 0; | |
bool _events = false; | |
void _enter() | |
{ | |
log("State change: %s => %s", _state, _nextState); | |
prevState = _state.id; | |
_state = _nextState; | |
_nextState = State.init; | |
onEnter = true; | |
} | |
static void updateState(T)(ref T state, uint now) | |
{ | |
// If State has an update function, call it | |
static if (__traits(compiles, state.update(0))) | |
state.update(now); | |
// Any members with update functions? | |
updateMembers(state, now); | |
} | |
void enterState(T)() | |
{ | |
// Don't want any recursion here, so temporarily store state | |
_nextState = T.init; | |
} | |
static size_t stateID(T)() | |
{ | |
return State.IndexOf!T; | |
} | |
void begin() | |
{ | |
enterState!(FSM.InitialState); | |
} | |
void update() | |
{ | |
now = _currTime(); | |
onEnter = false; | |
if (_nextState.id != State.init.id) | |
_enter(); | |
// Call state handler | |
sw: switch(_state.id) | |
{ | |
foreach(Type; State.Types) | |
{ | |
case stateID!(Type): | |
// Check if handler even exists | |
foreach (Overload; __traits(getOverloads, FSM, "onState")) | |
{ | |
static if (is(Parameters!Overload == AliasSeq!(Type))) | |
{ | |
onState(_state.get!Type); | |
} | |
} | |
// Update state | |
updateState(_state.get!Type, now); | |
break sw; | |
} | |
default: | |
break; | |
} | |
} | |
} | |
struct EdgeEvent | |
{ | |
private: | |
ubyte _state = 0; | |
enum EventBit | |
{ | |
event = 1, | |
happened = 2 | |
} | |
public: | |
void setEvent() | |
{ | |
_state |= (EventBit.event | EventBit.happened); | |
} | |
@property bool event() | |
{ | |
const result = _state & EventBit.event ? true : false; | |
_state &= ~EventBit.event; | |
return result; | |
} | |
// had event at least once | |
@property bool happened() | |
{ | |
return _state & EventBit.happened ? true : false; | |
} | |
} | |
struct LevelEvent | |
{ | |
private: | |
ubyte _state = 0; | |
enum EventBit | |
{ | |
active = 1, | |
rose = 2, | |
fell = 4, | |
happened = 8 | |
} | |
public: | |
void setActive() | |
{ | |
if (!active) | |
_state |= (EventBit.rose | EventBit.happened | EventBit.active); | |
} | |
void setInActive() | |
{ | |
if (active) | |
{ | |
_state |= EventBit.fell; | |
_state &= ~EventBit.active; | |
} | |
} | |
@property bool rose() | |
{ | |
const result = _state & EventBit.rose ? true : false; | |
_state &= ~EventBit.rose; | |
return result; | |
} | |
@property bool fell() | |
{ | |
const result = _state & EventBit.fell ? true : false; | |
_state &= ~EventBit.fell; | |
return result; | |
} | |
@property bool edge() | |
{ | |
auto result = rose(); | |
result |= fell(); // No short-circuit || here, must always clear both flags! | |
return result; | |
} | |
@property bool active() | |
{ | |
return _state & EventBit.active ? true : false; | |
} | |
// was active at least once | |
@property bool happened() | |
{ | |
return _state & EventBit.happened ? true : false; | |
} | |
} | |
// Real code starts here | |
struct Timer | |
{ | |
uint _target = uint.max; | |
EdgeEvent alarm; | |
alias alarm this; | |
this(uint now, uint duration) | |
{ | |
_target = now + duration; | |
} | |
void update(uint now) | |
{ | |
log("%s (%s)", now, _target); | |
if (now > _target) | |
{ | |
_target = uint.max; | |
alarm.setEvent(); | |
} | |
} | |
} | |
struct Button(alias Addr) | |
{ | |
LevelEvent pressed; | |
alias pressed this; | |
void update(uint now) | |
{ | |
import core.volatile; | |
version (DebugPrint) | |
{ | |
static bool last = false; | |
auto val = !last; | |
last = val; | |
} | |
else | |
auto val = volatileLoad(Addr) & 0b1; | |
if (val) | |
pressed.setActive(); | |
else | |
pressed.setInActive(); | |
} | |
} | |
struct Motor | |
{ | |
private: | |
ubyte _target, _current; | |
uint _last; | |
void setAngle() | |
{ | |
import core.volatile; | |
version (DebugPrint) | |
log("set motor angle to %s", _current); | |
else | |
volatileStore(cast(ubyte*)0xDEADBEAF, _current); | |
} | |
public: | |
EdgeEvent moved; | |
void update(uint now) | |
{ | |
const angularV = 2048; // degree per sec | |
const delta = ((now - _last) * angularV) / 1000; | |
if (delta > 0) | |
{ | |
if (_target > _current) | |
{ | |
if (delta >= _target - _current) | |
{ | |
_current = _target; | |
moved.setEvent(); | |
} | |
else | |
{ | |
_current += delta; | |
} | |
setAngle(); | |
} | |
else if(_target < _current) | |
{ | |
if (delta >= _current - _target) | |
{ | |
_current = _target; | |
moved.setEvent(); | |
} | |
else | |
{ | |
_current -= delta; | |
} | |
setAngle(); | |
} | |
_last = now; | |
} | |
} | |
void moveAngle(ubyte target) | |
{ | |
_target = target; | |
} | |
} | |
enum ubyte* START_BUTTON = cast(ubyte*)0xDEADBEAF; | |
struct Phase1 | |
{ | |
Timer timeout; | |
Motor motor; | |
Button!START_BUTTON startButton; | |
ubyte buttonCount = 5; | |
} | |
struct Phase2 | |
{ | |
} | |
struct FSM | |
{ | |
private: | |
alias States = AliasSeq!(Phase1, Phase2); | |
alias InitialState = Phase1; | |
mixin FSMLogic!(typeof(this)); | |
public: | |
void onState(ref Phase1 phase1) | |
{ | |
log("onState:Phase1"); | |
if (onEnter) | |
phase1.timeout = Timer(now, 5); | |
// Only executed once per event, even for level triggered. Cleared on check. | |
if (phase1.timeout.event) | |
{ | |
log("phase1 timout elapsed. Setting motor to 90 deg"); | |
phase1.motor.moveAngle(90); | |
} | |
// Executed if phase1.motor.moved.event happened at least once this state | |
if (phase1.motor.moved.happened) | |
{ | |
log("motor happened"); | |
// Only on rise | |
if (phase1.startButton.rose) | |
{ | |
phase1.buttonCount--; | |
log("button count %s", phase1.buttonCount); | |
if (phase1.buttonCount == 0) | |
enterState!Phase2; | |
} | |
} | |
} | |
void onState(ref Phase2 phase2) | |
{ | |
log("onState:Phase2"); | |
if (onEnter) | |
{ | |
log("Entered Phase2"); | |
_sideEffect(); | |
if (prevState == stateID!Phase1) | |
{ | |
log("Previous phase was Phase1, goto Phase1"); | |
enterState!Phase1; | |
} | |
} | |
} | |
} | |
void main() | |
{ | |
FSM fsm; | |
fsm.begin(); | |
size_t iterations = 0; | |
while(true) | |
{ | |
fsm.update(); | |
version (DebugPrint) | |
{ | |
// This is not an embedded system, we don't want 100% CPU... | |
import core.thread : Thread; | |
import std.datetime : msecs; | |
Thread.sleep(1.msecs); | |
if (++iterations == 100) | |
return; | |
} | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment