Add State Machine

CMakeLists.txt

@@ -10,7 +10,7 @@ add_subdirectory(AutonomousMode)
 add_subdirectory(Communication)
 add_subdirectory(Socket)
 
-add_executable(lfr_central main.cpp)
+add_executable(lfr_central main.cpp lfr_state_machine.cpp)
 
 target_include_directories(lfr_central PRIVATE ${lfr_image_processing_SOURCE_DIRS})
 target_include_directories(lfr_central PRIVATE ${lfr_uart_SOURCE_DIRS})

lfr_state_machine.cpp

@@ -0,0 +1,54 @@
+#include "lfr_state_machine.h"
+
+/* void State::Idle::toggle(LFR_StateMachine* stateMachine)
+{
+    stateMachine->setState()
+} */
+
+void State::Idle::enter(LFR_StateMachine* stateMachine)
+{
+
+}
+
+void State::Idle::exit(LFR_StateMachine* stateMachine)
+{
+    
+}
+
+LFR_State& State::Idle::getInstance()
+{
+    static State::Idle singleton;
+    return singleton;
+}
+
+void State::Autonomous::enter(LFR_StateMachine* stateMachine)
+{
+
+}
+
+void State::Autonomous::exit(LFR_StateMachine* stateMachine)
+{
+    stateMachine->setState(State::Idle::getInstance());
+}
+
+LFR_State& State::Autonomous::getInstance()
+{
+    static State::Autonomous singleton;
+    return singleton;
+}
+
+void State::Manual::enter(LFR_StateMachine* stateMachine)
+{
+
+}
+
+void State::Manual::exit(LFR_StateMachine* stateMachine)
+{
+    stateMachine->setState(State::Idle::getInstance());
+}
+
+LFR_State& State::Manual::getInstance()
+{
+    static State::Manual singleton;
+    return singleton;
+}

lfr_state_machine.h

@@ -0,0 +1,69 @@
+#pragma once
+
+class LFR_StateMachine;
+
+class LFR_State
+{
+ public:
+ virtual void enter(LFR_StateMachine* stateMachine) = 0;
+ virtual void exit(LFR_StateMachine* stateMachine) = 0;
+ virtual ~LFR_State() {}
+};
+
+
+namespace State
+{
+    class Idle: public LFR_State
+    {
+        Idle() {}
+        Idle(const Idle& other) {};
+        Idle& operator= (const Idle& other);
+
+    public:
+        void enter(LFR_StateMachine*);
+        void exit(LFR_StateMachine*);
+        static LFR_State& getInstance();
+    };
+
+
+    class Autonomous: public LFR_State
+    {
+        Autonomous() {}
+        Autonomous(const Autonomous& other) {};
+        Autonomous& operator= (const Autonomous& other);
+
+    public:
+        void enter(LFR_StateMachine*);
+        void exit(LFR_StateMachine*);
+        static LFR_State& getInstance();
+    };
+
+
+    class Manual: public LFR_State
+    {
+        Manual() {}
+        Manual(const Manual& other) {};
+        Manual& operator= (const Manual& other);
+
+    public:
+        void enter(LFR_StateMachine*);
+        void exit(LFR_StateMachine*);
+        static LFR_State& getInstance();
+    };
+}
+
+class LFR_StateMachine
+{
+    LFR_State* currentState;
+public:
+    LFR_StateMachine(){
+        setState(State::Idle::getInstance());
+    }
+    inline LFR_State* getCurrentState() const {return currentState;}
+    void setState(LFR_State& newState)
+    {
+        currentState->exit(this);
+        currentState = &newState;
+        currentState->enter(this);
+    }
+};