Skip to content

Instantly share code, notes, and snippets.

@jpf91 jpf91/fsm2.d

Created Mar 7, 2020
Embed
What would you like to do?
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
You can’t perform that action at this time.