From af6b650dddec9169f0d45635494c627d28987365 Mon Sep 17 00:00:00 2001 From: Cantonplas Date: Sun, 4 Jan 2026 16:47:09 +0100 Subject: [PATCH 01/18] Full of errors, but moving forward --- Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp | 295 +++++++++---------- Src/ST-LIB_LOW/StateMachine/StateMachine.cpp | 22 +- 2 files changed, 142 insertions(+), 175 deletions(-) diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp index 1f8fa36c5..f9a5e57c6 100644 --- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp +++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp @@ -6,6 +6,12 @@ #include "C++Utilities/CppUtils.hpp" #include "ErrorHandler/ErrorHandler.hpp" #include "HALAL/HALAL.hpp" +#include +#include +#include +#include +#include + #ifdef STLIB_ETH #include "StateMachine/StateOrder.hpp" #endif @@ -17,204 +23,179 @@ using ms = std::chrono::milliseconds; using us = std::chrono::microseconds; using s = std::chrono::seconds; +using Callback = void (*)(); +using Guard = bool (*)(); + +static constexpr size_t NUMBER_OF_ACTIONS = 20; + enum AlarmType { LOW_PRECISION, MID_PRECISION, HIGH_PRECISION }; class TimedAction { public: - function action; + Callback action; uint32_t period; AlarmType alarm_precision; uint8_t id = -1; - bool is_on = false; TimedAction() = default; }; +template +struct Transition { + StateEnum target; + Guard predicate; +}; + +template +concept are_transitions = (std::same_as> && ...); + +template class State { -public: - vector cyclic_actions; - vector> on_enter_actions = {}; - vector> on_exit_actions = {}; - vector state_orders_ids = {}; +private: + std::array cyclic_actions; + std::array on_enter_actions = {}; + std::array on_exit_actions = {}; + [[no_unique_address]]std::array state_orders_ids = {}; + StateEnum state; + std::array, NTransitions> transitions; + public: + template + requires are_transitions + consteval State(StateEnum state, T... transitions) + : state(state), transitions({transitions...}) {} + + + consteval const StateEnum& get_state() const { return state; }; + consteval const auto& get_transitions() const { return transitions; }; + consteval const auto& get_cyclic_actions() const {return cyclic_actions}; + consteval const auto& get_enter_actions() const {return on_enter_actions}; + consteval const auto& get_exit_actions() const {return on_exit_actions}; - void enter(); - void exit(); template - TimedAction *add_new_timed_action(function action, - chrono::duration period, - AlarmType precision_type); + consteval TimedAction * + add_low_precision_cyclic_action(Callback action, + chrono::duration period); + template + consteval TimedAction * + add_mid_precision_cyclic_action(Callback action, + chrono::duration period); + template - TimedAction * - register_new_timed_action(function action, - chrono::duration period, - AlarmType precision_type); - void unregister_all_timed_actions(); + consteval TimedAction * + add_high_precision_cyclic_action(Callback action, + chrono::duration period); + + void enter(); + void exit(); + + void unregister_all_timed_actions(); void register_all_timed_actions(); - void unregister_timed_action(TimedAction *timed_action); - void erase_timed_action(TimedAction *timed_action); + void erase_timed_action(TimedAction* timed_action); void add_state_order(uint16_t id); }; + template -TimedAction * -State::register_new_timed_action(function action, - chrono::duration period, - AlarmType precision_type) { +consteval TimedAction * State::add_low_precision_cyclic_action(Callback action, + chrono::duration period) { TimedAction timed_action = {}; - timed_action.alarm_precision = precision_type; + timed_action.alarm_precision = LOW_PRECISION; timed_action.action = action; - uint32_t miliseconds = - chrono::duration_cast(period).count(); - uint32_t microseconds = - chrono::duration_cast(period).count(); - switch (precision_type) { - case LOW_PRECISION: - timed_action.period = miliseconds; - timed_action.id = Time::register_low_precision_alarm(miliseconds, action); - break; - case MID_PRECISION: - timed_action.period = microseconds; - timed_action.id = Time::register_mid_precision_alarm(microseconds, action); - break; - case HIGH_PRECISION: - timed_action.period = microseconds; - timed_action.id = Time::register_high_precision_alarm(microseconds, action); - break; - default: - ErrorHandler("Alarm Precision Type does not exist, AlarmType: %d", - precision_type); - break; - } - timed_action.is_on = true; + uint32_t miliseconds = chrono::duration_cast(period).count(); + timed_action.period = miliseconds; cyclic_actions.push_back(timed_action); return &cyclic_actions.back(); } template -TimedAction * -State::add_new_timed_action(function action, - chrono::duration period, - AlarmType precision_type) { +consteval TimedAction * State::add_mid_precision_cyclic_action(Callback action, + chrono::duration period) { TimedAction timed_action = {}; - timed_action.alarm_precision = precision_type; + timed_action.alarm_precision = MID_PRECISION; timed_action.action = action; - uint32_t miliseconds = - chrono::duration_cast(period).count(); - uint32_t microseconds = - chrono::duration_cast(period).count(); - switch (precision_type) { - case LOW_PRECISION: - timed_action.period = miliseconds; - break; - case MID_PRECISION: - timed_action.period = microseconds; - break; - case HIGH_PRECISION: - timed_action.period = microseconds; - break; - default: - ErrorHandler("Alarm Precision Type does not exist, AlarmType: %d", - precision_type); - return nullptr; - break; - } + uint32_t microseconds = chrono::duration_cast(period).count(); + timed_action.period = microseconds; cyclic_actions.push_back(timed_action); return &cyclic_actions.back(); } +template +consteval TimedAction * State::add_high_precision_cyclic_action(Callback action, + chrono::duration period) { + TimedAction timed_action = {}; + timed_action.alarm_precision = HIGH_PRECISION; + timed_action.action = action; + uint32_t microseconds = chrono::duration_cast(period).count(); + timed_action.period = microseconds; + cyclic_actions.push_back(timed_action); + return &cyclic_actions.back(); +} +template +struct is_state : std::false_type {}; + +template +struct is_state, StateEnum> : std::true_type {}; + +template +concept are_states = (is_state::value && ...); + +template class StateMachine { -public: - typedef uint8_t state_id; +private: + using Transitions = std::array, NTransitions>; + using TAssocs = std::array, NStates>; + + StateEnum current_state; + Transitions transitions; + TAssocs transitions_assoc; - state_id initial_state = 0; - state_id current_state = 0; +public: + bool is_on = true; StateMachine(); - StateMachine(state_id initial_state); - - void add_state(state_id state); - void add_transition(state_id old_state, state_id new_state, - function transition); - - template - TimedAction * - add_low_precision_cyclic_action(function action, - chrono::duration period, - state_id state); - template - TimedAction * - add_low_precision_cyclic_action(function action, - chrono::duration period, - vector states); - template - TimedAction * - add_low_precision_cyclic_action(function action, - chrono::duration period); + StateMachine(StateEnum initial_state); - template - TimedAction * - add_mid_precision_cyclic_action(function action, - chrono::duration period, - state_id state); - template - TimedAction * - add_mid_precision_cyclic_action(function action, - chrono::duration period, - vector states); - template - TimedAction * - add_mid_precision_cyclic_action(function action, - chrono::duration period); + consteval void add_state(StateEnum state){ + states[state] = State(state); + } + void add_transition(StateEnum old_state, StateEnum new_state, + Guard transition); - template - TimedAction * - add_high_precision_cyclic_action(function action, - chrono::duration period); - template - TimedAction * - add_high_precision_cyclic_action(function action, - chrono::duration period, - state_id state); - template - TimedAction * - add_high_precision_cyclic_action(function action, - chrono::duration period, - vector states); void remove_cyclic_action(TimedAction *timed_action); - void remove_cyclic_action(TimedAction *timed_action, state_id state); + void remove_cyclic_action(TimedAction *timed_action, StateEnum state); - void add_enter_action(function action); - void add_enter_action(function action, state_id state); + void add_enter_action(Callback action); + void add_enter_action(Callback action, StateEnum state); - void add_exit_action(function action); - void add_exit_action(function action, state_id state); + void add_exit_action(Callback action); + void add_exit_action(Callback action, StateEnum state); - void force_change_state(state_id new_state); + void force_change_state(StateEnum new_state); void check_transitions(); - void add_state_machine(StateMachine &state_machine, state_id state); + void add_state_machine(StateMachine &state_machine, StateEnum state); void refresh_state_orders(); - unordered_map &get_states(); + unordered_map &get_states(); #ifdef SIM_ON uint8_t get_id_in_shm(); #endif private: - unordered_map states; - unordered_map>> + unordered_map states; + unordered_map> transitions; - unordered_map nested_state_machine; + unordered_map nested_state_machine; - void enter_state(state_id new_state); - void exit_state(state_id old_state); - void register_all_timed_actions(state_id state); - void unregister_all_timed_actions(state_id state); + void enter_state(StateEnum new_state); + void exit_state(StateEnum old_state); + void register_all_timed_actions(StateEnum state); + void unregister_all_timed_actions(StateEnum state); #ifdef SIM_ON uint8_t state_machine_id_in_shm; #endif @@ -222,8 +203,8 @@ class StateMachine { template TimedAction *StateMachine::add_low_precision_cyclic_action( - function action, chrono::duration period, - state_id state) { + Callback action, chrono::duration period, + StateEnum state) { if (not states.contains(state)) { ErrorHandler("The state %d is not added to the state machine", state); return nullptr; @@ -247,10 +228,10 @@ TimedAction *StateMachine::add_low_precision_cyclic_action( template TimedAction *StateMachine::add_low_precision_cyclic_action( - function action, chrono::duration period, - vector states) { + Callback action, chrono::duration period, + vector states) { TimedAction *timed_action = nullptr; - for (state_id state : states) { + for (StateEnum state : states) { timed_action = add_low_precision_cyclic_action(action, period, state); } return timed_action; @@ -258,14 +239,14 @@ TimedAction *StateMachine::add_low_precision_cyclic_action( template TimedAction *StateMachine::add_low_precision_cyclic_action( - function action, chrono::duration period) { + Callback action, chrono::duration period) { return add_low_precision_cyclic_action(action, period, current_state); } template TimedAction *StateMachine::add_mid_precision_cyclic_action( - function action, chrono::duration period, - state_id state) { + Callback action, chrono::duration period, + StateEnum state) { if (not states.contains(state)) { ErrorHandler("The state %d is not added to the state machine", state); return nullptr; @@ -290,10 +271,10 @@ TimedAction *StateMachine::add_mid_precision_cyclic_action( template TimedAction *StateMachine::add_mid_precision_cyclic_action( - function action, chrono::duration period, - vector states) { + Callback action, chrono::duration period, + vector states) { TimedAction *timed_action = nullptr; - for (state_id state : states) { + for (StateEnum state : states) { timed_action = add_mid_precision_cyclic_action(action, period, state); } return timed_action; @@ -301,14 +282,14 @@ TimedAction *StateMachine::add_mid_precision_cyclic_action( template TimedAction *StateMachine::add_mid_precision_cyclic_action( - function action, chrono::duration period) { + Callback action, chrono::duration period) { return add_mid_precision_cyclic_action(action, period, current_state); } template TimedAction *StateMachine::add_high_precision_cyclic_action( - function action, chrono::duration period, - state_id state) { + Callback action, chrono::duration period, + StateEnum state) { if (not states.contains(state)) { ErrorHandler("The state %d is not added to the state machine", state); return nullptr; @@ -333,10 +314,10 @@ TimedAction *StateMachine::add_high_precision_cyclic_action( template TimedAction *StateMachine::add_high_precision_cyclic_action( - function action, chrono::duration period, - vector states) { + Callback action, chrono::duration period, + vector states) { TimedAction *timed_action = nullptr; - for (state_id state : states) { + for (StateEnum state : states) { timed_action = add_high_precision_cyclic_action(action, period, state); } return timed_action; @@ -344,6 +325,6 @@ TimedAction *StateMachine::add_high_precision_cyclic_action( template TimedAction *StateMachine::add_high_precision_cyclic_action( - function action, chrono::duration period) { + Callback action, chrono::duration period) { return add_high_precision_cyclic_action(action, period, current_state); } diff --git a/Src/ST-LIB_LOW/StateMachine/StateMachine.cpp b/Src/ST-LIB_LOW/StateMachine/StateMachine.cpp index fc8770749..b89c98f21 100644 --- a/Src/ST-LIB_LOW/StateMachine/StateMachine.cpp +++ b/Src/ST-LIB_LOW/StateMachine/StateMachine.cpp @@ -14,7 +14,9 @@ void State::enter() { action(); } #ifdef STLIB_ETH + if(Number_of_state_orderss != 0){ if(state_orders_ids.size() != 0) StateOrder::add_state_orders(state_orders_ids); + } #endif } @@ -23,28 +25,12 @@ void State::exit() { action(); } #ifdef STLIB_ETH + if(Number_of_state_orderss != 0){ if(state_orders_ids.size() != 0) StateOrder::remove_state_orders(state_orders_ids); + } #endif } -void State::unregister_timed_action(TimedAction* timed_action){ - switch (timed_action->alarm_precision) { - case LOW_PRECISION: - Time::unregister_low_precision_alarm(timed_action->id); - break; - case MID_PRECISION: - Time::unregister_mid_precision_alarm(timed_action->id); - break; - case HIGH_PRECISION: - Time::unregister_high_precision_alarm(timed_action->id); - break; - default: - ErrorHandler("Alarm Precision Type does not exist, AlarmType: %d", timed_action->alarm_precision); - return; - break; - } - timed_action->is_on = false; -} void State::erase_timed_action(TimedAction* timed_action){ if(timed_action->is_on) unregister_timed_action(timed_action); From a07cac2ac375e985e1d675f7fdc17253dea75027 Mon Sep 17 00:00:00 2001 From: Cantonplas Date: Sun, 4 Jan 2026 17:16:22 +0100 Subject: [PATCH 02/18] Avanzando bien fino, casi implementado --- Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp | 276 ++++++++----------- Src/ST-LIB_LOW/StateMachine/StateMachine.cpp | 21 +- 2 files changed, 118 insertions(+), 179 deletions(-) diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp index f9a5e57c6..89c4cb80a 100644 --- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp +++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp @@ -71,6 +71,17 @@ class State { consteval const auto& get_enter_actions() const {return on_enter_actions}; consteval const auto& get_exit_actions() const {return on_exit_actions}; + consteval void add_cyclic_action(TimedAction *timed_action); + + consteval void add_enter_action(Callback action){ + on_enter_actions.push_back(action); + }; + + consteval void add_exit_action(Callback action){ + on_exit_actions.push_back(action); + }; + + template consteval TimedAction * add_low_precision_cyclic_action(Callback action, @@ -90,7 +101,7 @@ class State { void unregister_all_timed_actions(); void register_all_timed_actions(); - void erase_timed_action(TimedAction* timed_action); + void remove_cyclic_action(TimedAction *timed_action); void add_state_order(uint16_t id); }; @@ -148,183 +159,130 @@ class StateMachine { StateEnum current_state; Transitions transitions; TAssocs transitions_assoc; + std::array, NStates> states; + unordered_map nested_state_machine; //ya veremos que hacemos con esto -public: - - bool is_on = true; - StateMachine(); - StateMachine(StateEnum initial_state); + consteval void process_state(auto state, size_t offset) { + for (const auto& t : state.get_transitions()) { + transitions[offset++] = t; + } + + transitions_assoc[static_cast(state.get_state())] = { + offset - state.get_transitions().size(), + state.get_transitions().size()}; + } - consteval void add_state(StateEnum state){ - states[state] = State(state); + void enter() { //ESTO ESTÁ EN LA CLASE STATE MOVER DE AQUI + auto& state = states[static_cast(current_state)]; + for (const auto& action : state.get_enter_actions()) { + action(); + } + register_all_timed_actions(current_state); } - void add_transition(StateEnum old_state, StateEnum new_state, - Guard transition); + void exit() { + unregister_all_timed_actions(current_state); + auto& state = states[static_cast(current_state)]; + for (const auto& action : state.get_exit_actions()) { + action(); + } + } - void remove_cyclic_action(TimedAction *timed_action); - void remove_cyclic_action(TimedAction *timed_action, StateEnum state); +public: + + bool is_on = true; - void add_enter_action(Callback action); - void add_enter_action(Callback action, StateEnum state); - void add_exit_action(Callback action); - void add_exit_action(Callback action, StateEnum state); + template + requires are_states + consteval StateMachine(StateEnum initial_state, S... states) { + current_state = initial_state; + size_t offset = 0; + ((process_state(states, offset), + offset += states.get_transitions().size()), + ...); + } + + + + void check_transitions() { + auto& [i, n] = transitions_assoc[static_cast(current_state)]; + for (auto index = i; index < i + n; ++index) { + const auto& t = transitions[index]; + if (t.predicate()) { + exit(); + current_state = t.target; + enter(); + break; + } + } + if (nested_state_machine.contains(current_state)) { + nested_state_machine[current_state]->check_transitions(); + } + }; + + void force_change_state(StateEnum new_state) { + exit(); + current_state = new_state; + enter(); + }; + + //Falta helper crear transiciones y crear estado + consteval void add_cyclic_action(TimedAction *timed_action, StateEnum state); //helpers que llamarán en tiempo de compilacion a las funciones de state + + void remove_cyclic_action(TimedAction *timed_action, StateEnum state); + consteval void add_enter_action(Callback action, StateEnum state){ + if (not states.contains(state)) { + throw "Error: The state is not added to the state machine"; + return; + } + states[state].on_enter_actions.push_back(action); + }; + + consteval void add_exit_action(Callback action, StateEnum state){ + if (not states.contains(state)) { + throw "Error: The state is not added to the state machine"; + return; + } + states[state].on_exit_actions.push_back(action); + }; + + void StateMachine::add_state_machine(StateMachine& state_machine, uint8_t state) { + if(nested_state_machine.contains(state)){ + ErrorHandler("Only one Nested State Machine can be added per state, tried to add to state: %d", state); + return; + } + + if(not state_machine.states[state_machine.current_state].cyclic_actions.empty()){ + ErrorHandler("Nested State Machine current state has actions registered, must be empty until nesting"); + } + + nested_state_machine[state] = &state_machine; + + if (current_state != state) { + state_machine.is_on = false; + } +} void force_change_state(StateEnum new_state); - void check_transitions(); void add_state_machine(StateMachine &state_machine, StateEnum state); - void refresh_state_orders(); + void StateMachine::refresh_state_orders(){ + #ifdef STLIB_ETH + if(states[current_state].state_orders_ids.size() != 0) StateOrder::add_state_orders(states[current_state].state_orders_ids); //Por ver + #endif + } - unordered_map &get_states(); + consteval std::array, NStates> states& get_states(){ + return states; + } #ifdef SIM_ON uint8_t get_id_in_shm(); -#endif - -private: - unordered_map states; - unordered_map> - transitions; - unordered_map nested_state_machine; - - void enter_state(StateEnum new_state); - void exit_state(StateEnum old_state); - void register_all_timed_actions(StateEnum state); - void unregister_all_timed_actions(StateEnum state); -#ifdef SIM_ON uint8_t state_machine_id_in_shm; #endif }; -template -TimedAction *StateMachine::add_low_precision_cyclic_action( - Callback action, chrono::duration period, - StateEnum state) { - if (not states.contains(state)) { - ErrorHandler("The state %d is not added to the state machine", state); - return nullptr; - } - - uint32_t microseconds = - (uint32_t)chrono::duration_cast(period).count(); - - if (microseconds % 1000 != 0) { - ErrorHandler("Low precision cyclic action does not have enough resolution " - "for the desired period, Desired period: %d uS", - microseconds); - return nullptr; - } - if (state == current_state && is_on) - return states[state].register_new_timed_action(action, period, - LOW_PRECISION); - else - return states[state].add_new_timed_action(action, period, LOW_PRECISION); -} - -template -TimedAction *StateMachine::add_low_precision_cyclic_action( - Callback action, chrono::duration period, - vector states) { - TimedAction *timed_action = nullptr; - for (StateEnum state : states) { - timed_action = add_low_precision_cyclic_action(action, period, state); - } - return timed_action; -} - -template -TimedAction *StateMachine::add_low_precision_cyclic_action( - Callback action, chrono::duration period) { - return add_low_precision_cyclic_action(action, period, current_state); -} - -template -TimedAction *StateMachine::add_mid_precision_cyclic_action( - Callback action, chrono::duration period, - StateEnum state) { - if (not states.contains(state)) { - ErrorHandler("The state %d is not added to the state machine", state); - return nullptr; - } - - uint32_t microseconds = - (uint32_t)chrono::duration_cast(period).count(); - - if (microseconds % 50 != 0) { - ErrorHandler("Mid precision cyclic action does not have enough resolution " - "for the desired period, Desired period: %d uS", - microseconds); - return nullptr; - } - - if (state == current_state && is_on) - return states[state].register_new_timed_action(action, period, - MID_PRECISION); - else - return states[state].add_new_timed_action(action, period, MID_PRECISION); -} - -template -TimedAction *StateMachine::add_mid_precision_cyclic_action( - Callback action, chrono::duration period, - vector states) { - TimedAction *timed_action = nullptr; - for (StateEnum state : states) { - timed_action = add_mid_precision_cyclic_action(action, period, state); - } - return timed_action; -} - -template -TimedAction *StateMachine::add_mid_precision_cyclic_action( - Callback action, chrono::duration period) { - return add_mid_precision_cyclic_action(action, period, current_state); -} - -template -TimedAction *StateMachine::add_high_precision_cyclic_action( - Callback action, chrono::duration period, - StateEnum state) { - if (not states.contains(state)) { - ErrorHandler("The state %d is not added to the state machine", state); - return nullptr; - } - - uint32_t microseconds = - (uint32_t)chrono::duration_cast(period).count(); - - if (microseconds < 1) { - ErrorHandler("High precision cyclic action does not have enough resolution " - "for the desired period, Desired period: %d uS", - microseconds); - return nullptr; - } - - if (state == current_state && is_on) - return states[state].register_new_timed_action(action, period, - HIGH_PRECISION); - else - return states[state].add_new_timed_action(action, period, HIGH_PRECISION); -} - -template -TimedAction *StateMachine::add_high_precision_cyclic_action( - Callback action, chrono::duration period, - vector states) { - TimedAction *timed_action = nullptr; - for (StateEnum state : states) { - timed_action = add_high_precision_cyclic_action(action, period, state); - } - return timed_action; -} - -template -TimedAction *StateMachine::add_high_precision_cyclic_action( - Callback action, chrono::duration period) { - return add_high_precision_cyclic_action(action, period, current_state); -} diff --git a/Src/ST-LIB_LOW/StateMachine/StateMachine.cpp b/Src/ST-LIB_LOW/StateMachine/StateMachine.cpp index b89c98f21..f0aa5a74b 100644 --- a/Src/ST-LIB_LOW/StateMachine/StateMachine.cpp +++ b/Src/ST-LIB_LOW/StateMachine/StateMachine.cpp @@ -233,22 +233,7 @@ void StateMachine::add_transition(uint8_t old_state, uint8_t new_state, * machine to the current StateMachine object. * @param state The state to which the nested state machine is being added. */ -void StateMachine::add_state_machine(StateMachine& state_machine, uint8_t state) { - if(nested_state_machine.contains(state)){ - ErrorHandler("Only one Nested State Machine can be added per state, tried to add to state: %d", state); - return; - } - - if(not state_machine.states[state_machine.current_state].cyclic_actions.empty()){ - ErrorHandler("Nested State Machine current state has actions registered, must be empty until nesting"); - } - nested_state_machine[state] = &state_machine; - - if (current_state != state) { - state_machine.is_on = false; - } -} /** * The function checks for state transitions and changes the current state if necessary, and also @@ -350,11 +335,7 @@ unordered_map& StateMachine::get_states(){ return states; } -void StateMachine::refresh_state_orders(){ -#ifdef STLIB_ETH - if(states[current_state].state_orders_ids.size() != 0) StateOrder::add_state_orders(states[current_state].state_orders_ids); -#endif -} + #ifdef SIM_ON uint8_t StateMachine::get_id_in_shm(){ From 2d10275c022a48294c73669d46254de2978f5926 Mon Sep 17 00:00:00 2001 From: Cantonplas Date: Sun, 4 Jan 2026 18:35:25 +0100 Subject: [PATCH 03/18] Vamos bien encaminados --- Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp | 230 +++++++++---- Src/ST-LIB_LOW/StateMachine/StateMachine.cpp | 344 ------------------- 2 files changed, 167 insertions(+), 407 deletions(-) delete mode 100644 Src/ST-LIB_LOW/StateMachine/StateMachine.cpp diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp index 89c4cb80a..b44d79a83 100644 --- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp +++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp @@ -32,10 +32,11 @@ enum AlarmType { LOW_PRECISION, MID_PRECISION, HIGH_PRECISION }; class TimedAction { public: - Callback action; - uint32_t period; - AlarmType alarm_precision; - uint8_t id = -1; + Callback action = nullptr; + uint32_t period = 0; + AlarmType alarm_precision = LOW_PRECISION; + uint8_t id = 255; + bool is_on = false; TimedAction() = default; }; @@ -67,80 +68,181 @@ class State { consteval const StateEnum& get_state() const { return state; }; consteval const auto& get_transitions() const { return transitions; }; - consteval const auto& get_cyclic_actions() const {return cyclic_actions}; - consteval const auto& get_enter_actions() const {return on_enter_actions}; - consteval const auto& get_exit_actions() const {return on_exit_actions}; + consteval const auto& get_cyclic_actions() const {return cyclic_actions;}; + consteval const auto& get_enter_actions() const {return on_enter_actions;}; + consteval const auto& get_exit_actions() const {return on_exit_actions;}; consteval void add_cyclic_action(TimedAction *timed_action); consteval void add_enter_action(Callback action){ - on_enter_actions.push_back(action); + for(auto& slot : on_enter_actions){ + if(slot == nullptr){ + slot = action; + return; + } + } }; consteval void add_exit_action(Callback action){ - on_exit_actions.push_back(action); + for(auto& slot : on_exit_actions){ + if(slot == nullptr){ + slot = action; + return; + } + } }; + void enter(){ + for (const auto& action : on_enter_actions) { + if(action) action(); + } + register_all_timed_actions(); + }; + void exit(){ + unregister_all_timed_actions(); + for (const auto& action : on_exit_actions) { + if(action) action(); + } + }; - template - consteval TimedAction * - add_low_precision_cyclic_action(Callback action, - chrono::duration period); - template - consteval TimedAction * - add_mid_precision_cyclic_action(Callback action, - chrono::duration period); - - template - consteval TimedAction * - add_high_precision_cyclic_action(Callback action, - chrono::duration period); + void unregister_all_timed_actions(){ + for(TimedAction& timed_action : cyclic_actions){ + if(timed_action.action == nullptr) continue; + if(!timed_action.is_on) continue; + switch(timed_action.alarm_precision){ + case LOW_PRECISION: + Time::unregister_low_precision_alarm(timed_action.id); + break; + case MID_PRECISION: + Time::unregister_mid_precision_alarm(timed_action.id); + break; + case HIGH_PRECISION: + Time::unregister_high_precision_alarm(timed_action.id); + break; + default: + ErrorHandler("Cannot unregister timed action with erroneus alarm precision, Alarm Precision Type: %d", timed_action.alarm_precision); + return; + break; + } + timed_action.is_on = false; + } + }; + + void register_all_timed_actions(){ + for(TimedAction& timed_action : cyclic_actions){ + if(timed_action.action == nullptr) continue; + switch(timed_action.alarm_precision){ + case LOW_PRECISION: + timed_action.id = Time::register_low_precision_alarm(timed_action.period, timed_action.action); + break; + case MID_PRECISION: + timed_action.id = Time::register_mid_precision_alarm(timed_action.period, timed_action.action); + break; + case HIGH_PRECISION: + timed_action.id = Time::register_high_precision_alarm(timed_action.period, timed_action.action); + break; + default: + ErrorHandler("Cannot register timed action with erroneus alarm precision, Alarm Precision Type: %d", timed_action.alarm_precision); + return; + break; + } + timed_action.is_on = true; + } + }; - void enter(); - void exit(); + void remove_cyclic_action(TimedAction *timed_action){ + if(timed_action->is_on) unregister_timed_action(timed_action); + for(auto& slot : cyclic_actions){ + if(&slot == timed_action){ + slot = TimedAction{}; + return; + } + } + }; - void unregister_all_timed_actions(); - void register_all_timed_actions(); - void remove_cyclic_action(TimedAction *timed_action); - void add_state_order(uint16_t id); -}; + void unregister_timed_action(TimedAction* timed_action){ + switch(timed_action->alarm_precision){ + case LOW_PRECISION: + Time::unregister_low_precision_alarm(timed_action->id); + break; + case MID_PRECISION: + Time::unregister_mid_precision_alarm(timed_action->id); + break; + case HIGH_PRECISION: + Time::unregister_high_precision_alarm(timed_action->id); + break; + default: + ErrorHandler("Cannot unregister timed action with erroneus alarm precision, Alarm Precision Type: %d", timed_action->alarm_precision); + return; + break; + } + timed_action->is_on = false; + }; + void add_state_order(uint16_t id){ + state_orders_ids.push_back(id); + }; -template -consteval TimedAction * State::add_low_precision_cyclic_action(Callback action, - chrono::duration period) { + template + consteval TimedAction * + add_low_precision_cyclic_action(Callback action, + chrono::duration period){ TimedAction timed_action = {}; timed_action.alarm_precision = LOW_PRECISION; timed_action.action = action; uint32_t miliseconds = chrono::duration_cast(period).count(); timed_action.period = miliseconds; - cyclic_actions.push_back(timed_action); - return &cyclic_actions.back(); -} + + for(auto& slot : cyclic_actions){ + if(slot.action == nullptr){ + slot = timed_action; + return &slot; + } + } + return nullptr; + }; -template -consteval TimedAction * State::add_mid_precision_cyclic_action(Callback action, - chrono::duration period) { + template + consteval TimedAction * + add_mid_precision_cyclic_action(Callback action, + chrono::duration period){ TimedAction timed_action = {}; timed_action.alarm_precision = MID_PRECISION; timed_action.action = action; uint32_t microseconds = chrono::duration_cast(period).count(); timed_action.period = microseconds; - cyclic_actions.push_back(timed_action); - return &cyclic_actions.back(); -} + + for(auto& slot : cyclic_actions){ + if(slot.action == nullptr){ + slot = timed_action; + return &slot; + } + } + return nullptr; + }; -template -consteval TimedAction * State::add_high_precision_cyclic_action(Callback action, - chrono::duration period) { + template + consteval TimedAction * + add_high_precision_cyclic_action(Callback action, + chrono::duration period){ TimedAction timed_action = {}; timed_action.alarm_precision = HIGH_PRECISION; timed_action.action = action; uint32_t microseconds = chrono::duration_cast(period).count(); timed_action.period = microseconds; - cyclic_actions.push_back(timed_action); - return &cyclic_actions.back(); -} + + for(auto& slot : cyclic_actions){ + if(slot.action == nullptr){ + slot = timed_action; + return &slot; + } + } + return nullptr; + }; + +}; + + template struct is_state : std::false_type {}; @@ -174,26 +276,18 @@ class StateMachine { state.get_transitions().size()}; } - void enter() { //ESTO ESTÁ EN LA CLASE STATE MOVER DE AQUI + void enter() { auto& state = states[static_cast(current_state)]; - for (const auto& action : state.get_enter_actions()) { - action(); - } - register_all_timed_actions(current_state); + state.enter(); + } void exit() { - unregister_all_timed_actions(current_state); auto& state = states[static_cast(current_state)]; - for (const auto& action : state.get_exit_actions()) { - action(); - } + state.exit(); } public: - - bool is_on = true; - template requires are_states @@ -228,7 +322,7 @@ class StateMachine { current_state = new_state; enter(); }; - + //Falta helper crear transiciones y crear estado consteval void add_cyclic_action(TimedAction *timed_action, StateEnum state); //helpers que llamarán en tiempo de compilacion a las funciones de state @@ -250,7 +344,7 @@ class StateMachine { states[state].on_exit_actions.push_back(action); }; - void StateMachine::add_state_machine(StateMachine& state_machine, uint8_t state) { + void add_state_machine(StateMachine& state_machine, uint8_t state) { if(nested_state_machine.contains(state)){ ErrorHandler("Only one Nested State Machine can be added per state, tried to add to state: %d", state); return; @@ -266,11 +360,10 @@ class StateMachine { state_machine.is_on = false; } } - void force_change_state(StateEnum new_state); void add_state_machine(StateMachine &state_machine, StateEnum state); - void StateMachine::refresh_state_orders(){ + void refresh_state_orders(){ #ifdef STLIB_ETH if(states[current_state].state_orders_ids.size() != 0) StateOrder::add_state_orders(states[current_state].state_orders_ids); //Por ver #endif @@ -280,6 +373,17 @@ class StateMachine { return states; } + template + requires are_transitions + static consteval auto make_state(StateEnum state, Callback action, + Transitions... transitions) { + constexpr size_t NTransitions = sizeof...(transitions); + return State(state, action, + transitions...); + } + + + #ifdef SIM_ON uint8_t get_id_in_shm(); uint8_t state_machine_id_in_shm; diff --git a/Src/ST-LIB_LOW/StateMachine/StateMachine.cpp b/Src/ST-LIB_LOW/StateMachine/StateMachine.cpp deleted file mode 100644 index f0aa5a74b..000000000 --- a/Src/ST-LIB_LOW/StateMachine/StateMachine.cpp +++ /dev/null @@ -1,344 +0,0 @@ -/* - * Created by Alejandro - */ - -#include "StateMachine/StateMachine.hpp" -#include "ErrorHandler/ErrorHandler.hpp" - -#ifdef SIM_ON -#include "HALALMock/Services/SharedMemory/SharedMemory.hpp" -#endif - -void State::enter() { - for (function& action : on_enter_actions) { - action(); - } -#ifdef STLIB_ETH - if(Number_of_state_orderss != 0){ - if(state_orders_ids.size() != 0) StateOrder::add_state_orders(state_orders_ids); - } -#endif -} - -void State::exit() { - for (function& action : on_exit_actions) { - action(); - } -#ifdef STLIB_ETH - if(Number_of_state_orderss != 0){ - if(state_orders_ids.size() != 0) StateOrder::remove_state_orders(state_orders_ids); - } -#endif -} - - -void State::erase_timed_action(TimedAction* timed_action){ - if(timed_action->is_on) unregister_timed_action(timed_action); - cyclic_actions.erase(find_if(cyclic_actions.begin(), cyclic_actions.end(), [&](TimedAction& other){ - if(&other == timed_action) return true; - return false; - })); -} - -void State::unregister_all_timed_actions(){ - for(TimedAction& timed_action : cyclic_actions){ - if(!timed_action.is_on) continue; - switch(timed_action.alarm_precision){ - case LOW_PRECISION: - Time::unregister_low_precision_alarm(timed_action.id); - break; - case MID_PRECISION: - Time::unregister_mid_precision_alarm(timed_action.id); - break; - case HIGH_PRECISION: - Time::unregister_high_precision_alarm(timed_action.id); - break; - default: - ErrorHandler("Cannot unregister timed action with erroneus alarm precision, Alarm Precision Type: %d", timed_action.alarm_precision); - return; - break; - } - timed_action.is_on = false; - } -} - -void State::register_all_timed_actions(){ - for(TimedAction& timed_action : cyclic_actions){ - switch(timed_action.alarm_precision){ - case LOW_PRECISION: - timed_action.id = Time::register_low_precision_alarm(timed_action.period, timed_action.action); - break; - case MID_PRECISION: - timed_action.id = Time::register_mid_precision_alarm(timed_action.period, timed_action.action); - break; - case HIGH_PRECISION: - timed_action.id = Time::register_high_precision_alarm(timed_action.period, timed_action.action); - break; - default: - ErrorHandler("Cannot register timed action with erroneus alarm precision, Alarm Precision Type: %d", timed_action.alarm_precision); - return; - break; - } - timed_action.is_on = true; - } -} - -void State::add_state_order(uint16_t id){ - state_orders_ids.push_back(id); -} - -#ifdef SIM_ON -StateMachine::StateMachine(){ - state_machine_id_in_shm = ++(*SharedMemory::state_machine_count); - SharedMemory::update_current_state(state_machine_id_in_shm,initial_state); -} -#else -StateMachine::StateMachine(){} -#endif - -/** - * This is a constructor for a StateMachine object that initializes the initial state and creates a - * State object for it. - * - * @param initial_state The initial state parameter is an unsigned 8-bit integer that represents the - * starting state of the state machine. - */ -StateMachine::StateMachine(uint8_t initial_state) : - initial_state(initial_state), current_state(initial_state) { - add_state(initial_state); - enter_state(initial_state); - - #ifdef SIM_ON - state_machine_id_in_shm = ++(*SharedMemory::state_machine_count); - SharedMemory::update_current_state(state_machine_id_in_shm,initial_state); - #endif -} - -/** - * The function adds a state to a state machine and sets it as the initial and current state if it is - * the first state added. - * - * @param state The state to be added to the state machine. - * - * @return The function does not return anything. It has a void return type. - */ -void StateMachine::add_state(uint8_t state) { - if (states.contains(state)) { - ErrorHandler("The state %d is already added", state); - return; - } - - if (states.empty()) { - initial_state = state; - current_state = state; - } - states[state]; -} - -/** - * This function adds an action to be executed when entering the current state of a state machine. - * - * @param action The parameter "action" is a function pointer to a function that takes no arguments and - * returns nothing (void). It represents an action that should be executed when the current state of - * the state machine is entered. - */ -void StateMachine::add_enter_action(function action) { - if (not current_state) { - ErrorHandler("The state machine does not have a current state"); - return; - } - states[current_state].on_enter_actions.push_back(action); -} - -/** - * This function adds an action to be executed when entering a specific state in a state machine. - * - * @param action The action parameter is a function pointer to a function that takes no arguments and - * returns nothing. This function will be executed when the state machine transitions to the state - * specified by the state parameter. - * @param state The state parameter is an unsigned 8-bit integer that represents the state to which the - * enter action is being added. - */ -void StateMachine::add_enter_action(function action, uint8_t state) { - if (not states.contains(state)) { - ErrorHandler("The state %d is not added to the state machine", state); - return; - } - states[state].on_enter_actions.push_back(action); -} - -/** - * This function adds an exit action to the current state of a state machine. - * - * @param action `action` is a function pointer to a function that takes no arguments and returns - * nothing (`void`). It represents an action that should be executed when the state machine exits the - * current state and transitions to a new state. The `add_exit_action` function adds this action to the - * list of exit actions for - */ -void StateMachine::add_exit_action(function action) { - if (not current_state) { - ErrorHandler("The state machine does not have a current state"); - return; - } - states[current_state].on_exit_actions.push_back(action); -} - -/** - * This function adds an exit action to a specific state in a state machine. - * - * @param action The action parameter is a function pointer to a function that takes no arguments and - * returns nothing. This function represents the action that will be executed when the state machine - * exits the specified state. - * @param state The state parameter is an unsigned 8-bit integer that represents the state to which the - * exit action is being added. - */ -void StateMachine::add_exit_action(function action, uint8_t state) { - if (not states.contains(state)) { - ErrorHandler("The state %d is not added to the state machine"); - return; - } - states[state].on_exit_actions.push_back(action); -} - -/** - * This function adds a transition between two states in a state machine. - * - * @param old_state The current state of the state machine before the transition occurs. It is - * represented as an unsigned 8-bit integer (uint8_t). - * @param new_state The new state to transition to. It is of type uint8_t. - * @param transition The parameter "transition" is a function pointer to a boolean function that takes - * no arguments. This function represents the condition that must be met in order for the state machine - * to transition from the old_state to the new_state. If the condition is true, the transition will - * occur. If the condition is false - */ -void StateMachine::add_transition(uint8_t old_state, uint8_t new_state, - function transition) { - if (not states.contains(old_state)) { - ErrorHandler("The state %d is not added to the state machine", old_state); - } - - if(not states.contains(new_state)) { - ErrorHandler("The state %d is not added to the state machine", new_state); - return; - } - - transitions[old_state][new_state] = transition; -} - -/** - * The function adds a nested state machine to the current state machine and clears any timed actions - * associated with the nested state machine's current state if it is not the current state. - * - * @param state_machine A reference to a StateMachine object that is being added as a nested state - * machine to the current StateMachine object. - * @param state The state to which the nested state machine is being added. - */ - - -/** - * The function checks for state transitions and changes the current state if necessary, and also - * checks for transitions in a nested state machine if applicable. - */ -void StateMachine::check_transitions() { - for (auto const& state_transition : transitions[current_state]) { - if (state_transition.second()) { - force_change_state(state_transition.first); - } - } - - if (nested_state_machine.contains(current_state)) { - nested_state_machine[current_state]->check_transitions(); - } -} - -void StateMachine::force_change_state(uint8_t new_state) { - if (not states.contains(new_state)) { - ErrorHandler("The state %d is not added to the state machine", new_state); - return; - } - - if(current_state == new_state) return; - - unregister_all_timed_actions(current_state); - exit_state(current_state); - - current_state = new_state; - - enter_state(current_state); - register_all_timed_actions(current_state); - - #ifdef SIM_ON - SharedMemory::update_current_state(state_machine_id_in_shm,current_state); - #endif -} - -void StateMachine::remove_cyclic_action(TimedAction* action) { - if (not states.contains(current_state)) { - ErrorHandler("The state %d is not added to the state machine", current_state); - return; - } - - states[current_state].erase_timed_action(action); -} - -void StateMachine::remove_cyclic_action(TimedAction* action, uint8_t state) { - if (not states.contains(state)) { - ErrorHandler("The state %d is not added to the state machine", state); - return; - } - - states[state].erase_timed_action(action); -} - - - -void StateMachine::enter_state(state_id state) { - states[state].enter(); - - if (nested_state_machine.contains(state)) { - StateMachine* nested_sm = nested_state_machine[state]; - nested_sm->is_on = true; - nested_sm->enter_state(nested_sm->current_state); - } -} - -void StateMachine::exit_state(state_id state) { - states[state].exit(); - - if (nested_state_machine.contains(state)) { - StateMachine* nested_sm = nested_state_machine[state]; - nested_sm->is_on = false; - nested_sm->exit_state(nested_sm->current_state); - nested_sm->current_state = nested_sm->initial_state; - } -} - -void StateMachine::register_all_timed_actions(state_id state) { - states[state].register_all_timed_actions(); - - if (nested_state_machine.contains(current_state)) { - StateMachine* nested_sm = nested_state_machine[current_state]; - nested_sm->states[nested_sm->current_state].register_all_timed_actions(); - } -} - -void StateMachine::unregister_all_timed_actions(state_id state) { - states[state].unregister_all_timed_actions(); - - if (nested_state_machine.contains(current_state)) { - StateMachine* nested_sm = nested_state_machine[current_state]; - nested_sm->states[nested_sm->current_state].unregister_all_timed_actions(); - } -} - -unordered_map& StateMachine::get_states(){ - return states; -} - - - -#ifdef SIM_ON -uint8_t StateMachine::get_id_in_shm(){ - return state_machine_id_in_shm; -} -#endif \ No newline at end of file From 11fa6eac9ac19dc8e7146ce8440a5d63b5db42e4 Mon Sep 17 00:00:00 2001 From: Cantonplas Date: Sun, 4 Jan 2026 19:03:07 +0100 Subject: [PATCH 04/18] Bruh --- Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp | 91 ++++++++++---------- 1 file changed, 47 insertions(+), 44 deletions(-) diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp index b44d79a83..f58c3ebc2 100644 --- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp +++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp @@ -26,6 +26,7 @@ using s = std::chrono::seconds; using Callback = void (*)(); using Guard = bool (*)(); + static constexpr size_t NUMBER_OF_ACTIONS = 20; enum AlarmType { LOW_PRECISION, MID_PRECISION, HIGH_PRECISION }; @@ -41,6 +42,30 @@ class TimedAction { TimedAction() = default; }; +template +class StaticVector { + std::array data; + size_t size_ = 0; + +public: + constexpr void push_back(const T& value) { + if (size_ >= Capacity) { + throw std::out_of_range("StaticVector capacity exceeded"); + } + data[size_] = value; + size_++; + } + + constexpr auto begin() { return data.begin(); } + constexpr auto end() { return data.begin() + size_; } + + constexpr const std::array& get_array() const { return data; } + constexpr const size_t size() const { return size_; } + constexpr const T& operator[](size_t i) { return data[i]; } +}; +template +using array = StaticVector; + template struct Transition { StateEnum target; @@ -53,17 +78,19 @@ concept are_transitions = (std::same_as> && ...); template class State { private: - std::array cyclic_actions; - std::array on_enter_actions = {}; - std::array on_exit_actions = {}; - [[no_unique_address]]std::array state_orders_ids = {}; + array cyclic_actions; + array on_enter_actions = {}; + array on_exit_actions = {}; + [[no_unique_address]]array state_orders_ids = {}; StateEnum state; - std::array, NTransitions> transitions; + array, NTransitions> transitions; public: template requires are_transitions consteval State(StateEnum state, T... transitions) - : state(state), transitions({transitions...}) {} + : state(state) { + (this->transitions.push_back(transitions), ...); + } consteval const StateEnum& get_state() const { return state; }; @@ -75,21 +102,11 @@ class State { consteval void add_cyclic_action(TimedAction *timed_action); consteval void add_enter_action(Callback action){ - for(auto& slot : on_enter_actions){ - if(slot == nullptr){ - slot = action; - return; - } - } + on_enter_actions.push_back(action); }; consteval void add_exit_action(Callback action){ - for(auto& slot : on_exit_actions){ - if(slot == nullptr){ - slot = action; - return; - } - } + on_exit_actions.push_back(action); }; void enter(){ @@ -152,9 +169,11 @@ class State { void remove_cyclic_action(TimedAction *timed_action){ if(timed_action->is_on) unregister_timed_action(timed_action); + // StaticVector no tiene erase, marcamos como vacía o implementamos erase en StaticVector si es crítico + // Por ahora, invalidamos la acción for(auto& slot : cyclic_actions){ if(&slot == timed_action){ - slot = TimedAction{}; + slot = TimedAction{}; return; } } @@ -193,13 +212,8 @@ class State { uint32_t miliseconds = chrono::duration_cast(period).count(); timed_action.period = miliseconds; - for(auto& slot : cyclic_actions){ - if(slot.action == nullptr){ - slot = timed_action; - return &slot; - } - } - return nullptr; + cyclic_actions.push_back(timed_action); + return &cyclic_actions[cyclic_actions.size() - 1]; }; template @@ -212,13 +226,8 @@ class State { uint32_t microseconds = chrono::duration_cast(period).count(); timed_action.period = microseconds; - for(auto& slot : cyclic_actions){ - if(slot.action == nullptr){ - slot = timed_action; - return &slot; - } - } - return nullptr; + cyclic_actions.push_back(timed_action); + return &cyclic_actions[cyclic_actions.size() - 1]; }; template @@ -231,13 +240,8 @@ class State { uint32_t microseconds = chrono::duration_cast(period).count(); timed_action.period = microseconds; - for(auto& slot : cyclic_actions){ - if(slot.action == nullptr){ - slot = timed_action; - return &slot; - } - } - return nullptr; + cyclic_actions.push_back(timed_action); + return &cyclic_actions[cyclic_actions.size() - 1]; }; }; @@ -255,13 +259,13 @@ concept are_states = (is_state::value && ...); template class StateMachine { private: - using Transitions = std::array, NTransitions>; - using TAssocs = std::array, NStates>; + using Transitions = array, NTransitions>; + using TAssocs = array, NStates>; StateEnum current_state; Transitions transitions; TAssocs transitions_assoc; - std::array, NStates> states; + array, NStates> states; unordered_map nested_state_machine; //ya veremos que hacemos con esto @@ -323,7 +327,6 @@ class StateMachine { enter(); }; - //Falta helper crear transiciones y crear estado consteval void add_cyclic_action(TimedAction *timed_action, StateEnum state); //helpers que llamarán en tiempo de compilacion a las funciones de state void remove_cyclic_action(TimedAction *timed_action, StateEnum state); From 4389c1a8b9214034b7a53abf0a166b05039ab11a Mon Sep 17 00:00:00 2001 From: Cantonplas Date: Sun, 4 Jan 2026 20:20:42 +0100 Subject: [PATCH 05/18] Casi compila, casi --- Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp | 93 +++++++++++--------- 1 file changed, 50 insertions(+), 43 deletions(-) diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp index f58c3ebc2..349be0f6a 100644 --- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp +++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp @@ -11,6 +11,7 @@ #include #include #include +#include #ifdef STLIB_ETH #include "StateMachine/StateOrder.hpp" @@ -61,10 +62,10 @@ class StaticVector { constexpr const std::array& get_array() const { return data; } constexpr const size_t size() const { return size_; } - constexpr const T& operator[](size_t i) { return data[i]; } + constexpr T& operator[](size_t i) { return data[i]; } }; template -using array = StaticVector; +using FixedVector = StaticVector; template struct Transition { @@ -78,12 +79,12 @@ concept are_transitions = (std::same_as> && ...); template class State { private: - array cyclic_actions; - array on_enter_actions = {}; - array on_exit_actions = {}; - [[no_unique_address]]array state_orders_ids = {}; + FixedVector cyclic_actions; + FixedVector on_enter_actions = {}; + FixedVector on_exit_actions = {}; + [[no_unique_address]]FixedVector state_orders_ids = {}; StateEnum state; - array, NTransitions> transitions; + FixedVector, NTransitions> transitions; public: template requires are_transitions @@ -123,7 +124,9 @@ class State { }; void unregister_all_timed_actions(){ - for(TimedAction& timed_action : cyclic_actions){ + for(size_t i = 0; i < cyclic_actions.size(); i++) + { + TimedAction& timed_action = cyclic_actions[i]; if(timed_action.action == nullptr) continue; if(!timed_action.is_on) continue; switch(timed_action.alarm_precision){ @@ -146,7 +149,9 @@ class State { }; void register_all_timed_actions(){ - for(TimedAction& timed_action : cyclic_actions){ + for(size_t i = 0; i < cyclic_actions.size(); i++) + { + TimedAction& timed_action = cyclic_actions[i]; if(timed_action.action == nullptr) continue; switch(timed_action.alarm_precision){ case LOW_PRECISION: @@ -171,7 +176,9 @@ class State { if(timed_action->is_on) unregister_timed_action(timed_action); // StaticVector no tiene erase, marcamos como vacía o implementamos erase en StaticVector si es crítico // Por ahora, invalidamos la acción - for(auto& slot : cyclic_actions){ + for(size_t i = 0; i < cyclic_actions.size(); i++) + { + TimedAction& slot = cyclic_actions[i]; if(&slot == timed_action){ slot = TimedAction{}; return; @@ -206,42 +213,42 @@ class State { consteval TimedAction * add_low_precision_cyclic_action(Callback action, chrono::duration period){ - TimedAction timed_action = {}; - timed_action.alarm_precision = LOW_PRECISION; - timed_action.action = action; - uint32_t miliseconds = chrono::duration_cast(period).count(); - timed_action.period = miliseconds; - - cyclic_actions.push_back(timed_action); - return &cyclic_actions[cyclic_actions.size() - 1]; + TimedAction timed_action = {}; + timed_action.alarm_precision = LOW_PRECISION; + timed_action.action = action; + uint32_t miliseconds = chrono::duration_cast(period).count(); + timed_action.period = miliseconds; + + cyclic_actions.push_back(timed_action); + return &cyclic_actions[cyclic_actions.size() - 1]; }; template consteval TimedAction * add_mid_precision_cyclic_action(Callback action, chrono::duration period){ - TimedAction timed_action = {}; - timed_action.alarm_precision = MID_PRECISION; - timed_action.action = action; - uint32_t microseconds = chrono::duration_cast(period).count(); - timed_action.period = microseconds; - - cyclic_actions.push_back(timed_action); - return &cyclic_actions[cyclic_actions.size() - 1]; + TimedAction timed_action = {}; + timed_action.alarm_precision = MID_PRECISION; + timed_action.action = action; + uint32_t microseconds = chrono::duration_cast(period).count(); + timed_action.period = microseconds; + + cyclic_actions.push_back(timed_action); + return &cyclic_actions[cyclic_actions.size() - 1]; }; template consteval TimedAction * add_high_precision_cyclic_action(Callback action, chrono::duration period){ - TimedAction timed_action = {}; - timed_action.alarm_precision = HIGH_PRECISION; - timed_action.action = action; - uint32_t microseconds = chrono::duration_cast(period).count(); - timed_action.period = microseconds; - - cyclic_actions.push_back(timed_action); - return &cyclic_actions[cyclic_actions.size() - 1]; + TimedAction timed_action = {}; + timed_action.alarm_precision = HIGH_PRECISION; + timed_action.action = action; + uint32_t microseconds = chrono::duration_cast(period).count(); + timed_action.period = microseconds; + + cyclic_actions.push_back(timed_action); + return &cyclic_actions[cyclic_actions.size() - 1]; }; }; @@ -259,13 +266,13 @@ concept are_states = (is_state::value && ...); template class StateMachine { private: - using Transitions = array, NTransitions>; - using TAssocs = array, NStates>; + using Transitions = FixedVector, NTransitions>; + using TAssocs = FixedVector, NStates>; StateEnum current_state; Transitions transitions; TAssocs transitions_assoc; - array, NStates> states; + std::array, NStates> states; unordered_map nested_state_machine; //ya veremos que hacemos con esto @@ -297,6 +304,7 @@ class StateMachine { requires are_states consteval StateMachine(StateEnum initial_state, S... states) { current_state = initial_state; + this->states = {states...}; size_t offset = 0; ((process_state(states, offset), offset += states.get_transitions().size()), @@ -372,16 +380,15 @@ class StateMachine { #endif } - consteval std::array, NStates> states& get_states(){ + consteval std::array, NStates>& get_states(){ return states; } - - template - requires are_transitions + template + requires are_transitions static consteval auto make_state(StateEnum state, Callback action, Transitions... transitions) { - constexpr size_t NTransitions = sizeof...(transitions); - return State(state, action, + constexpr size_t number_of_transitions = sizeof...(transitions); + return State(state, action, transitions...); } From abf1f96d9b6059b6a20cf2bd5cb1b5aa8c4ea5e3 Mon Sep 17 00:00:00 2001 From: Cantonplas Date: Sun, 4 Jan 2026 20:55:13 +0100 Subject: [PATCH 06/18] StateMachine compila, otras cosas no xd --- Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp | 110 ++++++++++++++----- 1 file changed, 83 insertions(+), 27 deletions(-) diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp index 349be0f6a..28db1a1f1 100644 --- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp +++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp @@ -63,6 +63,14 @@ class StaticVector { constexpr const std::array& get_array() const { return data; } constexpr const size_t size() const { return size_; } constexpr T& operator[](size_t i) { return data[i]; } + constexpr bool contains(const T& value) const { + for (size_t i = 0; i < size_; ++i) { + if (data[i] == value) { + return true; + } + } + return false; + } }; template using FixedVector = StaticVector; @@ -104,24 +112,24 @@ class State { consteval void add_enter_action(Callback action){ on_enter_actions.push_back(action); - }; + } consteval void add_exit_action(Callback action){ on_exit_actions.push_back(action); - }; + } void enter(){ for (const auto& action : on_enter_actions) { if(action) action(); } register_all_timed_actions(); - }; + } void exit(){ unregister_all_timed_actions(); for (const auto& action : on_exit_actions) { if(action) action(); } - }; + } void unregister_all_timed_actions(){ for(size_t i = 0; i < cyclic_actions.size(); i++) @@ -146,7 +154,7 @@ class State { } timed_action.is_on = false; } - }; + } void register_all_timed_actions(){ for(size_t i = 0; i < cyclic_actions.size(); i++) @@ -164,18 +172,15 @@ class State { timed_action.id = Time::register_high_precision_alarm(timed_action.period, timed_action.action); break; default: - ErrorHandler("Cannot register timed action with erroneus alarm precision, Alarm Precision Type: %d", timed_action.alarm_precision); return; break; } timed_action.is_on = true; } - }; + } void remove_cyclic_action(TimedAction *timed_action){ if(timed_action->is_on) unregister_timed_action(timed_action); - // StaticVector no tiene erase, marcamos como vacía o implementamos erase en StaticVector si es crítico - // Por ahora, invalidamos la acción for(size_t i = 0; i < cyclic_actions.size(); i++) { TimedAction& slot = cyclic_actions[i]; @@ -184,7 +189,7 @@ class State { return; } } - }; + } void unregister_timed_action(TimedAction* timed_action){ switch(timed_action->alarm_precision){ @@ -203,11 +208,11 @@ class State { break; } timed_action->is_on = false; - }; + } - void add_state_order(uint16_t id){ + consteval void add_state_order(uint16_t id){ state_orders_ids.push_back(id); - }; + } template consteval TimedAction * @@ -221,7 +226,7 @@ class State { cyclic_actions.push_back(timed_action); return &cyclic_actions[cyclic_actions.size() - 1]; - }; + } template consteval TimedAction * @@ -235,7 +240,7 @@ class State { cyclic_actions.push_back(timed_action); return &cyclic_actions[cyclic_actions.size() - 1]; - }; + } template consteval TimedAction * @@ -249,7 +254,7 @@ class State { cyclic_actions.push_back(timed_action); return &cyclic_actions[cyclic_actions.size() - 1]; - }; + } }; @@ -267,7 +272,7 @@ template class StateMachine { private: using Transitions = FixedVector, NTransitions>; - using TAssocs = FixedVector, NStates>; + using TAssocs = std::array, NStates>; StateEnum current_state; Transitions transitions; @@ -279,7 +284,8 @@ class StateMachine { consteval void process_state(auto state, size_t offset) { for (const auto& t : state.get_transitions()) { - transitions[offset++] = t; + transitions.push_back(t); + offset++; } transitions_assoc[static_cast(state.get_state())] = { @@ -304,7 +310,9 @@ class StateMachine { requires are_states consteval StateMachine(StateEnum initial_state, S... states) { current_state = initial_state; - this->states = {states...}; + for(const auto& state : {states...}) { + this->states[static_cast(state.get_state())] = state; + } size_t offset = 0; ((process_state(states, offset), offset += states.get_transitions().size()), @@ -321,23 +329,61 @@ class StateMachine { exit(); current_state = t.target; enter(); + refresh_state_orders(); break; } } if (nested_state_machine.contains(current_state)) { nested_state_machine[current_state]->check_transitions(); } - }; + } void force_change_state(StateEnum new_state) { exit(); current_state = new_state; enter(); - }; + } - consteval void add_cyclic_action(TimedAction *timed_action, StateEnum state); //helpers que llamarán en tiempo de compilacion a las funciones de state + template + consteval TimedAction * + add_low_precision_cyclic_action(Callback action, + chrono::duration period,StateEnum state){ + if (not states.contains(state)) { + throw "Error: The state is not added to the state machine"; + return nullptr; + } + return states[static_cast(state)].add_low_precision_cyclic_action(action, period); + } - void remove_cyclic_action(TimedAction *timed_action, StateEnum state); + template + consteval TimedAction * + add_mid_precision_cyclic_action(Callback action, + chrono::duration period,StateEnum state){ + if (not states.contains(state)) { + throw "Error: The state is not added to the state machine"; + return nullptr; + } + return states[static_cast(state)].add_mid_precision_cyclic_action(action, period); + } + + template + consteval TimedAction * + add_high_precision_cyclic_action(Callback action, + chrono::duration period,StateEnum state){ + if (not states.contains(state)) { + throw "Error: The state is not added to the state machine"; + return nullptr; + } + return states[static_cast(state)].add_high_precision_cyclic_action(action, period); + } + + constexpr void remove_cyclic_action(TimedAction *timed_action, StateEnum state){ + if (not states.contains(state)) { + throw "Error: The state is not added to the state machine"; + return; + } + states[static_cast(state)].remove_cyclic_action(timed_action); + } consteval void add_enter_action(Callback action, StateEnum state){ if (not states.contains(state)) { @@ -345,7 +391,7 @@ class StateMachine { return; } states[state].on_enter_actions.push_back(action); - }; + } consteval void add_exit_action(Callback action, StateEnum state){ if (not states.contains(state)) { @@ -353,7 +399,7 @@ class StateMachine { return; } states[state].on_exit_actions.push_back(action); - }; + } void add_state_machine(StateMachine& state_machine, uint8_t state) { if(nested_state_machine.contains(state)){ @@ -372,17 +418,18 @@ class StateMachine { } } - void add_state_machine(StateMachine &state_machine, StateEnum state); void refresh_state_orders(){ #ifdef STLIB_ETH - if(states[current_state].state_orders_ids.size() != 0) StateOrder::add_state_orders(states[current_state].state_orders_ids); //Por ver + if(states[current_state].state_orders_ids.size() != 0) StateOrder::add_state_orders(states[current_state].state_orders_ids); #endif } consteval std::array, NStates>& get_states(){ return states; } + + template requires are_transitions static consteval auto make_state(StateEnum state, Callback action, @@ -392,6 +439,15 @@ class StateMachine { transitions...); } + template + requires are_states + static consteval auto make_state_machine(StateEnum initial_state, States... states) { + constexpr size_t number_of_states = sizeof...(states); + constexpr size_t number_of_transitions = (states.get_transitions().size() + ... + 0); + + return StateMachine(initial_state, states...); + } + #ifdef SIM_ON From 2f3d1198f7cd0b174f7f44c00ccf72ef66029b26 Mon Sep 17 00:00:00 2001 From: Cantonplas Date: Mon, 5 Jan 2026 00:36:14 +0100 Subject: [PATCH 07/18] Some error fixing --- Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp | 47 +++++++++++--------- 1 file changed, 26 insertions(+), 21 deletions(-) diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp index 28db1a1f1..0ffcf8b07 100644 --- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp +++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp @@ -221,7 +221,7 @@ class State { TimedAction timed_action = {}; timed_action.alarm_precision = LOW_PRECISION; timed_action.action = action; - uint32_t miliseconds = chrono::duration_cast(period).count(); + constexpr uint32_t miliseconds = chrono::duration_cast(period).count(); timed_action.period = miliseconds; cyclic_actions.push_back(timed_action); @@ -235,7 +235,7 @@ class State { TimedAction timed_action = {}; timed_action.alarm_precision = MID_PRECISION; timed_action.action = action; - uint32_t microseconds = chrono::duration_cast(period).count(); + constexpr uint32_t microseconds = chrono::duration_cast(period).count(); timed_action.period = microseconds; cyclic_actions.push_back(timed_action); @@ -249,7 +249,7 @@ class State { TimedAction timed_action = {}; timed_action.alarm_precision = HIGH_PRECISION; timed_action.action = action; - uint32_t microseconds = chrono::duration_cast(period).count(); + constexpr uint32_t microseconds = chrono::duration_cast(period).count(); timed_action.period = microseconds; cyclic_actions.push_back(timed_action); @@ -278,9 +278,7 @@ class StateMachine { Transitions transitions; TAssocs transitions_assoc; std::array, NStates> states; - unordered_map nested_state_machine; //ya veremos que hacemos con esto - - + unordered_map nested_state_machine; consteval void process_state(auto state, size_t offset) { for (const auto& t : state.get_transitions()) { @@ -293,13 +291,13 @@ class StateMachine { state.get_transitions().size()}; } - void enter() { + inline void enter() { auto& state = states[static_cast(current_state)]; state.enter(); } - void exit() { + inline void exit() { auto& state = states[static_cast(current_state)]; state.exit(); } @@ -327,6 +325,7 @@ class StateMachine { const auto& t = transitions[index]; if (t.predicate()) { exit(); + remove_state_orders(); current_state = t.target; enter(); refresh_state_orders(); @@ -402,29 +401,35 @@ class StateMachine { } void add_state_machine(StateMachine& state_machine, uint8_t state) { - if(nested_state_machine.contains(state)){ - ErrorHandler("Only one Nested State Machine can be added per state, tried to add to state: %d", state); - return; - } + if(nested_state_machine.contains(state)){ + ErrorHandler("Only one Nested State Machine can be added per state, tried to add to state: %d", state); + return; + } - if(not state_machine.states[state_machine.current_state].cyclic_actions.empty()){ - ErrorHandler("Nested State Machine current state has actions registered, must be empty until nesting"); - } + if(not state_machine.states[state_machine.current_state].cyclic_actions.empty()){ + ErrorHandler("Nested State Machine current state has actions registered, must be empty until nesting"); + } - nested_state_machine[state] = &state_machine; + nested_state_machine[state] = &state_machine; - if (current_state != state) { - state_machine.is_on = false; - } -} + if (current_state != state) { + state_machine.is_on = false; + } + } - void refresh_state_orders(){ + inline void refresh_state_orders(){ #ifdef STLIB_ETH if(states[current_state].state_orders_ids.size() != 0) StateOrder::add_state_orders(states[current_state].state_orders_ids); #endif } + inline void remove_state_orders(){ + #ifdef STLIB_ETH + if(states[current_state].state_orders_ids.size() != 0) StateOrder::remove_state_orders(states[current_state].state_orders_ids); + #endif + } + consteval std::array, NStates>& get_states(){ return states; } From 0f2b3ee8a6b54266dfca0208e9a110e7db0cf1c7 Mon Sep 17 00:00:00 2001 From: Cantonplas Date: Mon, 5 Jan 2026 00:37:06 +0100 Subject: [PATCH 08/18] Se lleva el credito su prima en tanga --- Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp index 0ffcf8b07..acd21d85a 100644 --- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp +++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp @@ -1,7 +1,3 @@ -/* - * Created by Alejandro - */ - #pragma once #include "C++Utilities/CppUtils.hpp" #include "ErrorHandler/ErrorHandler.hpp" @@ -85,7 +81,7 @@ template concept are_transitions = (std::same_as> && ...); template -class State { +class State { private: FixedVector cyclic_actions; FixedVector on_enter_actions = {}; From 31639a825caa0b5c585b48be1f76d623fbe746e1 Mon Sep 17 00:00:00 2001 From: Cantonplas Date: Wed, 7 Jan 2026 01:09:40 +0100 Subject: [PATCH 09/18] Compila por ahora?? --- Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp | 138 ++++++++++++------- 1 file changed, 85 insertions(+), 53 deletions(-) diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp index acd21d85a..cb02766b7 100644 --- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp +++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp @@ -37,6 +37,7 @@ class TimedAction { bool is_on = false; TimedAction() = default; + constexpr bool operator==(const TimedAction&) const = default; }; template @@ -45,16 +46,19 @@ class StaticVector { size_t size_ = 0; public: + constexpr bool operator==(const StaticVector&) const = default; constexpr void push_back(const T& value) { if (size_ >= Capacity) { - throw std::out_of_range("StaticVector capacity exceeded"); + ErrorHandler("StaticVector capacity exceeded"); } data[size_] = value; size_++; } constexpr auto begin() { return data.begin(); } + constexpr auto begin() const { return data.begin(); } constexpr auto end() { return data.begin() + size_; } + constexpr auto end() const { return data.begin() + size_; } constexpr const std::array& get_array() const { return data; } constexpr const size_t size() const { return size_; } @@ -75,6 +79,7 @@ template struct Transition { StateEnum target; Guard predicate; + constexpr bool operator==(const Transition&) const = default; }; template @@ -90,19 +95,26 @@ class State { StateEnum state; FixedVector, NTransitions> transitions; public: + static constexpr size_t transition_count = NTransitions; + template requires are_transitions consteval State(StateEnum state, T... transitions) : state(state) { (this->transitions.push_back(transitions), ...); } + + consteval State() = default; + + constexpr bool operator==(const State&) const = default; + - consteval const StateEnum& get_state() const { return state; }; - consteval const auto& get_transitions() const { return transitions; }; - consteval const auto& get_cyclic_actions() const {return cyclic_actions;}; - consteval const auto& get_enter_actions() const {return on_enter_actions;}; - consteval const auto& get_exit_actions() const {return on_exit_actions;}; + constexpr const StateEnum& get_state() const { return state; }; + constexpr const auto& get_transitions() const { return transitions; }; + constexpr const auto& get_cyclic_actions() const {return cyclic_actions;}; + constexpr const auto& get_enter_actions() const {return on_enter_actions;}; + constexpr const auto& get_exit_actions() const {return on_exit_actions;}; consteval void add_cyclic_action(TimedAction *timed_action); @@ -264,17 +276,39 @@ struct is_state, StateEnum> : std::true_type {}; template concept are_states = (is_state::value && ...); +class IStateMachine { + public: + virtual void check_transitions() = 0; + virtual bool can_be_nested() const = 0; + virtual void set_on(bool is_on) = 0; + constexpr bool operator==(const IStateMachine&) const = default; +}; + template -class StateMachine { +class StateMachine : public IStateMachine { +public: + bool is_on = true; + void set_on(bool is_on) override { + this->is_on = is_on; + } private: using Transitions = FixedVector, NTransitions>; using TAssocs = std::array, NStates>; + + struct NestedPair { + StateEnum state; + IStateMachine* machine; + constexpr bool operator==(const NestedPair&) const = default; + }; + StateEnum current_state; + std::array, NStates> states; Transitions transitions; TAssocs transitions_assoc; - std::array, NStates> states; - unordered_map nested_state_machine; + FixedVector nested_state_machine; + + constexpr bool operator==(const StateMachine&) const = default; consteval void process_state(auto state, size_t offset) { for (const auto& t : state.get_transitions()) { @@ -302,11 +336,10 @@ class StateMachine { template requires are_states - consteval StateMachine(StateEnum initial_state, S... states) { - current_state = initial_state; - for(const auto& state : {states...}) { - this->states[static_cast(state.get_state())] = state; - } + consteval StateMachine(StateEnum initial_state, S... states) + : current_state(initial_state), + states{states...} + { size_t offset = 0; ((process_state(states, offset), offset += states.get_transitions().size()), @@ -315,7 +348,7 @@ class StateMachine { - void check_transitions() { + void check_transitions() override { auto& [i, n] = transitions_assoc[static_cast(current_state)]; for (auto index = i; index < i + n; ++index) { const auto& t = transitions[index]; @@ -328,8 +361,11 @@ class StateMachine { break; } } - if (nested_state_machine.contains(current_state)) { - nested_state_machine[current_state]->check_transitions(); + for(auto& nested : nested_state_machine){ + if(nested.state == current_state){ + nested.machine->check_transitions(); + break; + } } } @@ -344,7 +380,7 @@ class StateMachine { add_low_precision_cyclic_action(Callback action, chrono::duration period,StateEnum state){ if (not states.contains(state)) { - throw "Error: The state is not added to the state machine"; + ErrorHandler("Error: The state is not added to the state machine"); return nullptr; } return states[static_cast(state)].add_low_precision_cyclic_action(action, period); @@ -355,7 +391,7 @@ class StateMachine { add_mid_precision_cyclic_action(Callback action, chrono::duration period,StateEnum state){ if (not states.contains(state)) { - throw "Error: The state is not added to the state machine"; + ErrorHandler("Error: The state is not added to the state machine"); return nullptr; } return states[static_cast(state)].add_mid_precision_cyclic_action(action, period); @@ -366,7 +402,7 @@ class StateMachine { add_high_precision_cyclic_action(Callback action, chrono::duration period,StateEnum state){ if (not states.contains(state)) { - throw "Error: The state is not added to the state machine"; + ErrorHandler("Error: The state is not added to the state machine"); return nullptr; } return states[static_cast(state)].add_high_precision_cyclic_action(action, period); @@ -374,7 +410,7 @@ class StateMachine { constexpr void remove_cyclic_action(TimedAction *timed_action, StateEnum state){ if (not states.contains(state)) { - throw "Error: The state is not added to the state machine"; + ErrorHandler( "Error: The state is not added to the state machine"); return; } states[static_cast(state)].remove_cyclic_action(timed_action); @@ -382,7 +418,7 @@ class StateMachine { consteval void add_enter_action(Callback action, StateEnum state){ if (not states.contains(state)) { - throw "Error: The state is not added to the state machine"; + ErrorHandler( "Error: The state is not added to the state machine"); return; } states[state].on_enter_actions.push_back(action); @@ -390,27 +426,29 @@ class StateMachine { consteval void add_exit_action(Callback action, StateEnum state){ if (not states.contains(state)) { - throw "Error: The state is not added to the state machine"; + ErrorHandler( "Error: The state is not added to the state machine"); return; } states[state].on_exit_actions.push_back(action); } - void add_state_machine(StateMachine& state_machine, uint8_t state) { - if(nested_state_machine.contains(state)){ - ErrorHandler("Only one Nested State Machine can be added per state, tried to add to state: %d", state); - return; - } +constexpr void add_state_machine(IStateMachine& state_machine, StateEnum state) { + for(auto& nested : nested_state_machine){ + if(nested.state == state){ + ErrorHandler("Only one Nested State Machine can be added per state, tried to add to state: %d", state); + return; + } + } - if(not state_machine.states[state_machine.current_state].cyclic_actions.empty()){ - ErrorHandler("Nested State Machine current state has actions registered, must be empty until nesting"); - } + if(not state_machine.can_be_nested()){ + ErrorHandler("Nested State Machine current state has actions registered, must be empty until nesting"); + } - nested_state_machine[state] = &state_machine; + nested_state_machine.push_back({state, &state_machine}); + } - if (current_state != state) { - state_machine.is_on = false; - } + bool can_be_nested() const override { + return states[static_cast(current_state)].get_cyclic_actions().size() == 0; } @@ -430,30 +468,24 @@ class StateMachine { return states; } +#ifdef SIM_ON + uint8_t get_id_in_shm(); + uint8_t state_machine_id_in_shm; +#endif +}; - template +template requires are_transitions - static consteval auto make_state(StateEnum state, Callback action, - Transitions... transitions) { + consteval auto make_state(StateEnum state, Transitions... transitions) { constexpr size_t number_of_transitions = sizeof...(transitions); - return State(state, action, - transitions...); + return State(state,transitions...); } - template + template requires are_states - static consteval auto make_state_machine(StateEnum initial_state, States... states) { + consteval auto make_state_machine(StateEnum initial_state, States... states) { constexpr size_t number_of_states = sizeof...(states); - constexpr size_t number_of_transitions = (states.get_transitions().size() + ... + 0); + constexpr size_t number_of_transitions = (std::remove_reference_t::transition_count + ... + 0); return StateMachine(initial_state, states...); - } - - - -#ifdef SIM_ON - uint8_t get_id_in_shm(); - uint8_t state_machine_id_in_shm; -#endif -}; - + } \ No newline at end of file From ec5c6c76dd0c816b15e0169bef5d31bd78027b66 Mon Sep 17 00:00:00 2001 From: Cantonplas Date: Tue, 20 Jan 2026 15:26:29 +0100 Subject: [PATCH 10/18] Compiles without state orders --- Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp | 153 ++++++++++--------- 1 file changed, 84 insertions(+), 69 deletions(-) diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp index cb02766b7..6eceb5f79 100644 --- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp +++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp @@ -42,10 +42,15 @@ class TimedAction { template class StaticVector { - std::array data; + std::array data{}; size_t size_ = 0; public: + constexpr StaticVector() = default; + + template + constexpr StaticVector(Args&&... args) : data{std::forward(args)...}, size_(sizeof...(args)) {} + constexpr bool operator==(const StaticVector&) const = default; constexpr void push_back(const T& value) { if (size_ >= Capacity) { @@ -63,6 +68,7 @@ class StaticVector { constexpr const std::array& get_array() const { return data; } constexpr const size_t size() const { return size_; } constexpr T& operator[](size_t i) { return data[i]; } + constexpr const T& operator[](size_t i) const { return data[i]; } constexpr bool contains(const T& value) const { for (size_t i = 0; i < size_; ++i) { if (data[i] == value) { @@ -88,12 +94,13 @@ concept are_transitions = (std::same_as> && ...); template class State { private: - FixedVector cyclic_actions; + FixedVector cyclic_actions = {}; FixedVector on_enter_actions = {}; FixedVector on_exit_actions = {}; [[no_unique_address]]FixedVector state_orders_ids = {}; StateEnum state; FixedVector, NTransitions> transitions; + public: static constexpr size_t transition_count = NTransitions; @@ -116,13 +123,12 @@ class State { constexpr const auto& get_enter_actions() const {return on_enter_actions;}; constexpr const auto& get_exit_actions() const {return on_exit_actions;}; - consteval void add_cyclic_action(TimedAction *timed_action); - consteval void add_enter_action(Callback action){ + constexpr void add_enter_action(Callback action){ on_enter_actions.push_back(action); } - consteval void add_exit_action(Callback action){ + constexpr void add_exit_action(Callback action){ on_exit_actions.push_back(action); } @@ -218,18 +224,18 @@ class State { timed_action->is_on = false; } - consteval void add_state_order(uint16_t id){ + constexpr void add_state_order(uint16_t id){ state_orders_ids.push_back(id); } template - consteval TimedAction * + constexpr TimedAction * add_low_precision_cyclic_action(Callback action, chrono::duration period){ TimedAction timed_action = {}; timed_action.alarm_precision = LOW_PRECISION; timed_action.action = action; - constexpr uint32_t miliseconds = chrono::duration_cast(period).count(); + uint32_t miliseconds = chrono::duration_cast(period).count(); timed_action.period = miliseconds; cyclic_actions.push_back(timed_action); @@ -237,13 +243,13 @@ class State { } template - consteval TimedAction * + constexpr TimedAction * add_mid_precision_cyclic_action(Callback action, chrono::duration period){ TimedAction timed_action = {}; timed_action.alarm_precision = MID_PRECISION; timed_action.action = action; - constexpr uint32_t microseconds = chrono::duration_cast(period).count(); + uint32_t microseconds = chrono::duration_cast(period).count(); timed_action.period = microseconds; cyclic_actions.push_back(timed_action); @@ -251,13 +257,13 @@ class State { } template - consteval TimedAction * + constexpr TimedAction * add_high_precision_cyclic_action(Callback action, chrono::duration period){ TimedAction timed_action = {}; timed_action.alarm_precision = HIGH_PRECISION; timed_action.action = action; - constexpr uint32_t microseconds = chrono::duration_cast(period).count(); + uint32_t microseconds = chrono::duration_cast(period).count(); timed_action.period = microseconds; cyclic_actions.push_back(timed_action); @@ -279,7 +285,6 @@ concept are_states = (is_state::value && ...); class IStateMachine { public: virtual void check_transitions() = 0; - virtual bool can_be_nested() const = 0; virtual void set_on(bool is_on) = 0; constexpr bool operator==(const IStateMachine&) const = default; }; @@ -303,10 +308,10 @@ class StateMachine : public IStateMachine { StateEnum current_state; - std::array, NStates> states; - Transitions transitions; - TAssocs transitions_assoc; - FixedVector nested_state_machine; + FixedVector, NStates> states; + Transitions transitions = {}; + TAssocs transitions_assoc ={}; + FixedVector nested_state_machine = {}; constexpr bool operator==(const StateMachine&) const = default; @@ -368,87 +373,96 @@ class StateMachine : public IStateMachine { } } } - - void force_change_state(StateEnum new_state) { + template + void force_change_state(const State& state){ + StateEnum new_state = state.get_state(); + if(current_state == new_state) return; exit(); current_state = new_state; enter(); } - template - consteval TimedAction * +template + constexpr TimedAction * add_low_precision_cyclic_action(Callback action, - chrono::duration period,StateEnum state){ - if (not states.contains(state)) { - ErrorHandler("Error: The state is not added to the state machine"); - return nullptr; + chrono::duration period,const State& state){ + for(size_t i = 0; i < states.size(); ++i){ + if(states[i].get_state() == state.get_state()){ + return states[i].add_low_precision_cyclic_action(action, period); + } } - return states[static_cast(state)].add_low_precision_cyclic_action(action, period); + ErrorHandler("Error: The state is not added to the state machine"); + return nullptr; } - template - consteval TimedAction * + template + constexpr TimedAction * add_mid_precision_cyclic_action(Callback action, - chrono::duration period,StateEnum state){ - if (not states.contains(state)) { - ErrorHandler("Error: The state is not added to the state machine"); - return nullptr; + chrono::duration period,const State& state){ + for(size_t i = 0; i < states.size(); ++i){ + if(states[i].get_state() == state.get_state()){ + return states[i].add_mid_precision_cyclic_action(action, period); + } } - return states[static_cast(state)].add_mid_precision_cyclic_action(action, period); + ErrorHandler("Error: The state is not added to the state machine"); + return nullptr; } - template - consteval TimedAction * + template + constexpr TimedAction * add_high_precision_cyclic_action(Callback action, - chrono::duration period,StateEnum state){ - if (not states.contains(state)) { - ErrorHandler("Error: The state is not added to the state machine"); - return nullptr; + chrono::duration period,const State& state){ + for(size_t i = 0; i < states.size(); ++i){ + if(states[i].get_state() == state.get_state()){ + return states[i].add_high_precision_cyclic_action(action, period); + } } - return states[static_cast(state)].add_high_precision_cyclic_action(action, period); + ErrorHandler("Error: The state is not added to the state machine"); + return nullptr; } - constexpr void remove_cyclic_action(TimedAction *timed_action, StateEnum state){ - if (not states.contains(state)) { - ErrorHandler( "Error: The state is not added to the state machine"); - return; + template + constexpr void remove_cyclic_action(TimedAction *timed_action, const State& state){ + for(size_t i = 0; i < states.size(); ++i){ + if(states[i].get_state() == state.get_state()){ + states[i].remove_cyclic_action(timed_action); + return; + } } - states[static_cast(state)].remove_cyclic_action(timed_action); + ErrorHandler("Error: The state is not added to the state machine"); } - consteval void add_enter_action(Callback action, StateEnum state){ - if (not states.contains(state)) { - ErrorHandler( "Error: The state is not added to the state machine"); - return; + template + constexpr void add_enter_action(Callback action, const State& state){ + for(size_t i = 0; i < states.size(); ++i){ + if(states[i].get_state() == state.get_state()){ + states[i].add_enter_action(action); + return; + } } - states[state].on_enter_actions.push_back(action); + ErrorHandler("Error: The state is not added to the state machine"); } - consteval void add_exit_action(Callback action, StateEnum state){ - if (not states.contains(state)) { - ErrorHandler( "Error: The state is not added to the state machine"); - return; + template + constexpr void add_exit_action(Callback action, const State& state){ + for(size_t i = 0; i < states.size(); ++i){ + if(states[i].get_state() == state.get_state()){ + states[i].add_exit_action(action); + return; + } } - states[state].on_exit_actions.push_back(action); + ErrorHandler("Error: The state is not added to the state machine"); } -constexpr void add_state_machine(IStateMachine& state_machine, StateEnum state) { + template + constexpr void add_state_machine(IStateMachine& state_machine, const State& state) { for(auto& nested : nested_state_machine){ - if(nested.state == state){ + if(nested.state == state.get_state()){ ErrorHandler("Only one Nested State Machine can be added per state, tried to add to state: %d", state); return; } } - - if(not state_machine.can_be_nested()){ - ErrorHandler("Nested State Machine current state has actions registered, must be empty until nesting"); - } - - nested_state_machine.push_back({state, &state_machine}); - } - - bool can_be_nested() const override { - return states[static_cast(current_state)].get_cyclic_actions().size() == 0; + nested_state_machine.push_back({state.get_state(), &state_machine}); } @@ -464,7 +478,7 @@ constexpr void add_state_machine(IStateMachine& state_machine, StateEnum state) #endif } - consteval std::array, NStates>& get_states(){ + constexpr std::array, NStates>& get_states(){ return states; } @@ -488,4 +502,5 @@ template constexpr size_t number_of_transitions = (std::remove_reference_t::transition_count + ... + 0); return StateMachine(initial_state, states...); - } \ No newline at end of file + } + \ No newline at end of file From 8b8051bfc94d12e39fac3bf707861bf08e64b1c5 Mon Sep 17 00:00:00 2001 From: Cantonplas Date: Tue, 20 Jan 2026 22:50:52 +0100 Subject: [PATCH 11/18] Fixed StateOrder problems and some fixes on the state machine code --- .../Protections/ProtectionManager.hpp | 4 +- .../StateMachine/HeapStateOrder.hpp | 14 +- .../StateMachine/StackStateOrder.hpp | 16 +- Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp | 218 +++++++++++++----- Inc/ST-LIB_LOW/StateMachine/StateOrder.hpp | 9 +- .../Protections/ProtectionManager.cpp | 8 +- Src/ST-LIB_LOW/StateMachine/StateOrder.cpp | 5 +- 7 files changed, 186 insertions(+), 88 deletions(-) diff --git a/Inc/ST-LIB_HIGH/Protections/ProtectionManager.hpp b/Inc/ST-LIB_HIGH/Protections/ProtectionManager.hpp index d4a7e708e..4beb10945 100644 --- a/Inc/ST-LIB_HIGH/Protections/ProtectionManager.hpp +++ b/Inc/ST-LIB_HIGH/Protections/ProtectionManager.hpp @@ -42,7 +42,7 @@ class ProtectionManager { static void set_id(Boards::ID id); - static void link_state_machine(StateMachine& general_state_machine, + static void link_state_machine(IStateMachine& general_state_machine, state_id fault_id); template low_frequency_protections; static vector high_frequency_protections; - static StateMachine* general_state_machine; + static IStateMachine* general_state_machine; static state_id fault_state_id; static Notification fault_notification; diff --git a/Inc/ST-LIB_LOW/StateMachine/HeapStateOrder.hpp b/Inc/ST-LIB_LOW/StateMachine/HeapStateOrder.hpp index 5746f00e4..7341d204c 100644 --- a/Inc/ST-LIB_LOW/StateMachine/HeapStateOrder.hpp +++ b/Inc/ST-LIB_LOW/StateMachine/HeapStateOrder.hpp @@ -1,19 +1,20 @@ #pragma once #include "StateMachine/StateOrder.hpp" +template class HeapStateOrder : public HeapOrder{ public: - StateMachine& state_machine; - StateMachine::state_id state; + StateMachineType& state_machine; + typename StateMachineType::state_id state; template - HeapStateOrder(uint16_t id,void(*callback)(void), StateMachine& state_machine, StateMachine::state_id state,Types*... values) : HeapOrder(id,callback,values...), + HeapStateOrder(uint16_t id,void(*callback)(void), StateMachineType& state_machine, typename StateMachineType::state_id state,Types*... values) : HeapOrder(id,callback,values...), state_machine(state_machine), state(state) { if(not state_machine.get_states().contains(state)){ ErrorHandler("State Machine does not contain state, cannot add StateOrder"); return; } - else state_machine.get_states()[state].add_state_order(id); + else state_machine.get_states()[static_cast(state)].add_state_order(id); orders[id] = this; } @@ -24,4 +25,7 @@ class HeapStateOrder : public HeapOrder{ void parse(OrderProtocol* socket, uint8_t* data)override{ if(state_machine.is_on && state_machine.current_state == state) HeapOrder::parse(data); } -}; \ No newline at end of file +}; + +template +HeapStateOrder(uint16_t, void(*)(void), StateMachineType&, typename StateMachineType::state_id, Types*...) -> HeapStateOrder; diff --git a/Inc/ST-LIB_LOW/StateMachine/StackStateOrder.hpp b/Inc/ST-LIB_LOW/StateMachine/StackStateOrder.hpp index 40c845d7b..9955d6e7b 100644 --- a/Inc/ST-LIB_LOW/StateMachine/StackStateOrder.hpp +++ b/Inc/ST-LIB_LOW/StateMachine/StackStateOrder.hpp @@ -1,18 +1,18 @@ #pragma once #include "StateMachine/StateOrder.hpp" -template requires NotCallablePack +template requires NotCallablePack class StackStateOrder : public StackOrder{ public: - StateMachine& state_machine; - StateMachine::state_id state; - StackStateOrder(uint16_t id,void(*callback)(void), StateMachine& state_machine, StateMachine::state_id state,Types*... values) : StackOrder(id,callback,values...), + StateMachineType& state_machine; + typename StateMachineType::state_id state; + StackStateOrder(uint16_t id,void(*callback)(void), StateMachineType& state_machine, typename StateMachineType::state_id state,Types*... values) : StackOrder(id,callback,values...), state_machine(state_machine), state(state) { if(not state_machine.get_states().contains(state)){ ErrorHandler("State Machine does not contain state, cannot add StateOrder"); return; } - else state_machine.get_states()[state].add_state_order(id); + else state_machine.get_states()[static_cast(state)].add_state_order(id); Order::orders[id] = this; } @@ -26,6 +26,6 @@ class StackStateOrder : public StackOrder{ }; #if __cpp_deduction_guides >= 201606 -template requires NotCallablePack -StackStateOrder(uint16_t id,void(*callback)(void), StateMachine& state_machine, StateMachine::state_id state,Types*... values)->StackStateOrder<(!has_container::value)*total_sizeof::value, Types...>; -#endif \ No newline at end of file +template requires NotCallablePack +StackStateOrder(uint16_t id,void(*callback)(void), StateMachineType& state_machine, typename StateMachineType::state_id state,Types*... values)->StackStateOrder::value)*total_sizeof::value, Types...>; +#endif diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp index 6eceb5f79..17cf88ef4 100644 --- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp +++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp @@ -42,6 +42,7 @@ class TimedAction { template class StaticVector { + private: std::array data{}; size_t size_ = 0; @@ -52,8 +53,11 @@ class StaticVector { constexpr StaticVector(Args&&... args) : data{std::forward(args)...}, size_(sizeof...(args)) {} constexpr bool operator==(const StaticVector&) const = default; - constexpr void push_back(const T& value) { - if (size_ >= Capacity) { + + constexpr void push_back(const T& value) + { + if (size_ >= Capacity) + { ErrorHandler("StaticVector capacity exceeded"); } data[size_] = value; @@ -67,11 +71,16 @@ class StaticVector { constexpr const std::array& get_array() const { return data; } constexpr const size_t size() const { return size_; } + constexpr T* get_data() { return data.data(); } + constexpr const T* get_data() const { return data.data(); } constexpr T& operator[](size_t i) { return data[i]; } constexpr const T& operator[](size_t i) const { return data[i]; } - constexpr bool contains(const T& value) const { - for (size_t i = 0; i < size_; ++i) { - if (data[i] == value) { + constexpr bool contains(const T& value) const + { + for (size_t i = 0; i < size_; ++i) + { + if (data[i] == value) + { return true; } } @@ -97,17 +106,18 @@ class State { FixedVector cyclic_actions = {}; FixedVector on_enter_actions = {}; FixedVector on_exit_actions = {}; - [[no_unique_address]]FixedVector state_orders_ids = {}; StateEnum state; FixedVector, NTransitions> transitions; public: + [[no_unique_address]]FixedVector state_orders_ids = {}; static constexpr size_t transition_count = NTransitions; template requires are_transitions consteval State(StateEnum state, T... transitions) - : state(state) { + : state(state) + { (this->transitions.push_back(transitions), ...); } @@ -124,34 +134,44 @@ class State { constexpr const auto& get_exit_actions() const {return on_exit_actions;}; - constexpr void add_enter_action(Callback action){ + constexpr void add_enter_action(Callback action) + { on_enter_actions.push_back(action); } - constexpr void add_exit_action(Callback action){ + constexpr void add_exit_action(Callback action) + { on_exit_actions.push_back(action); } - void enter(){ - for (const auto& action : on_enter_actions) { + void enter() + { + for (const auto& action : on_enter_actions) + { if(action) action(); } register_all_timed_actions(); } - void exit(){ + + void exit() + { unregister_all_timed_actions(); - for (const auto& action : on_exit_actions) { + for (const auto& action : on_exit_actions) + { if(action) action(); - } + } } - void unregister_all_timed_actions(){ + void unregister_all_timed_actions() + { for(size_t i = 0; i < cyclic_actions.size(); i++) { TimedAction& timed_action = cyclic_actions[i]; - if(timed_action.action == nullptr) continue; - if(!timed_action.is_on) continue; - switch(timed_action.alarm_precision){ + if(timed_action.action == nullptr){continue;} + if(!timed_action.is_on){ continue;} + + switch(timed_action.alarm_precision) + { case LOW_PRECISION: Time::unregister_low_precision_alarm(timed_action.id); break; @@ -170,12 +190,15 @@ class State { } } - void register_all_timed_actions(){ + void register_all_timed_actions() + { for(size_t i = 0; i < cyclic_actions.size(); i++) { TimedAction& timed_action = cyclic_actions[i]; - if(timed_action.action == nullptr) continue; - switch(timed_action.alarm_precision){ + if(timed_action.action == nullptr){ continue;} + + switch(timed_action.alarm_precision) + { case LOW_PRECISION: timed_action.id = Time::register_low_precision_alarm(timed_action.period, timed_action.action); break; @@ -193,20 +216,25 @@ class State { } } - void remove_cyclic_action(TimedAction *timed_action){ - if(timed_action->is_on) unregister_timed_action(timed_action); + void remove_cyclic_action(TimedAction *timed_action) + { + if(timed_action->is_on){ unregister_timed_action(timed_action);} + for(size_t i = 0; i < cyclic_actions.size(); i++) { TimedAction& slot = cyclic_actions[i]; - if(&slot == timed_action){ + if(&slot == timed_action) + { slot = TimedAction{}; return; } } } - void unregister_timed_action(TimedAction* timed_action){ - switch(timed_action->alarm_precision){ + void unregister_timed_action(TimedAction* timed_action) + { + switch(timed_action->alarm_precision) + { case LOW_PRECISION: Time::unregister_low_precision_alarm(timed_action->id); break; @@ -224,7 +252,8 @@ class State { timed_action->is_on = false; } - constexpr void add_state_order(uint16_t id){ + constexpr void add_state_order(uint16_t id) + { state_orders_ids.push_back(id); } @@ -282,32 +311,57 @@ struct is_state, StateEnum> : std::true_type {}; template concept are_states = (is_state::value && ...); +/// Interface for State Machines to allow other classes to interact with the state machine without knowing its implementation class IStateMachine { public: virtual void check_transitions() = 0; virtual void set_on(bool is_on) = 0; + virtual void force_change_state(size_t state) = 0; + virtual size_t get_current_state_id() const = 0; constexpr bool operator==(const IStateMachine&) const = default; }; template class StateMachine : public IStateMachine { -public: - bool is_on = true; - void set_on(bool is_on) override { - this->is_on = is_on; - } private: using Transitions = FixedVector, NTransitions>; using TAssocs = std::array, NStates>; - struct NestedPair { + struct NestedPair + { StateEnum state; IStateMachine* machine; constexpr bool operator==(const NestedPair&) const = default; }; - +public: + using state_id = StateEnum; StateEnum current_state; + + void force_change_state(size_t state) override + { + StateEnum new_state = static_cast(state); + if(current_state == new_state) + { + return; + } + exit(); + current_state = new_state; + enter(); + } + + size_t get_current_state_id() const override + { + return static_cast(current_state); + } + + bool is_on = true; + void set_on(bool is_on) override + { + this->is_on = is_on; + } + +private: FixedVector, NStates> states; Transitions transitions = {}; TAssocs transitions_assoc ={}; @@ -315,8 +369,10 @@ class StateMachine : public IStateMachine { constexpr bool operator==(const StateMachine&) const = default; - consteval void process_state(auto state, size_t offset) { - for (const auto& t : state.get_transitions()) { + consteval void process_state(auto state, size_t offset) + { + for (const auto& t : state.get_transitions()) + { transitions.push_back(t); offset++; } @@ -324,15 +380,17 @@ class StateMachine : public IStateMachine { transitions_assoc[static_cast(state.get_state())] = { offset - state.get_transitions().size(), state.get_transitions().size()}; - } + } - inline void enter() { + inline void enter() + { auto& state = states[static_cast(current_state)]; state.enter(); } - inline void exit() { + inline void exit() + { auto& state = states[static_cast(current_state)]; state.exit(); } @@ -353,11 +411,14 @@ class StateMachine : public IStateMachine { - void check_transitions() override { + void check_transitions() override + { auto& [i, n] = transitions_assoc[static_cast(current_state)]; - for (auto index = i; index < i + n; ++index) { + for (auto index = i; index < i + n; ++index) + { const auto& t = transitions[index]; - if (t.predicate()) { + if (t.predicate()) + { exit(); remove_state_orders(); current_state = t.target; @@ -366,15 +427,19 @@ class StateMachine : public IStateMachine { break; } } - for(auto& nested : nested_state_machine){ + + for(auto& nested : nested_state_machine) + { if(nested.state == current_state){ nested.machine->check_transitions(); break; } } } + template - void force_change_state(const State& state){ + void force_change_state(const State& state) + { StateEnum new_state = state.get_state(); if(current_state == new_state) return; exit(); @@ -386,8 +451,10 @@ template constexpr TimedAction * add_low_precision_cyclic_action(Callback action, chrono::duration period,const State& state){ - for(size_t i = 0; i < states.size(); ++i){ - if(states[i].get_state() == state.get_state()){ + for(size_t i = 0; i < states.size(); ++i) + { + if(states[i].get_state() == state.get_state()) + { return states[i].add_low_precision_cyclic_action(action, period); } } @@ -399,8 +466,10 @@ template constexpr TimedAction * add_mid_precision_cyclic_action(Callback action, chrono::duration period,const State& state){ - for(size_t i = 0; i < states.size(); ++i){ - if(states[i].get_state() == state.get_state()){ + for(size_t i = 0; i < states.size(); ++i) + { + if(states[i].get_state() == state.get_state()) + { return states[i].add_mid_precision_cyclic_action(action, period); } } @@ -412,8 +481,10 @@ template constexpr TimedAction * add_high_precision_cyclic_action(Callback action, chrono::duration period,const State& state){ - for(size_t i = 0; i < states.size(); ++i){ - if(states[i].get_state() == state.get_state()){ + for(size_t i = 0; i < states.size(); ++i) + { + if(states[i].get_state() == state.get_state()) + { return states[i].add_high_precision_cyclic_action(action, period); } } @@ -423,8 +494,10 @@ template template constexpr void remove_cyclic_action(TimedAction *timed_action, const State& state){ - for(size_t i = 0; i < states.size(); ++i){ - if(states[i].get_state() == state.get_state()){ + for(size_t i = 0; i < states.size(); ++i) + { + if(states[i].get_state() == state.get_state()) + { states[i].remove_cyclic_action(timed_action); return; } @@ -434,8 +507,10 @@ template template constexpr void add_enter_action(Callback action, const State& state){ - for(size_t i = 0; i < states.size(); ++i){ - if(states[i].get_state() == state.get_state()){ + for(size_t i = 0; i < states.size(); ++i) + { + if(states[i].get_state() == state.get_state()) + { states[i].add_enter_action(action); return; } @@ -445,8 +520,10 @@ template template constexpr void add_exit_action(Callback action, const State& state){ - for(size_t i = 0; i < states.size(); ++i){ - if(states[i].get_state() == state.get_state()){ + for(size_t i = 0; i < states.size(); ++i) + { + if(states[i].get_state() == state.get_state()) + { states[i].add_exit_action(action); return; } @@ -456,8 +533,10 @@ template template constexpr void add_state_machine(IStateMachine& state_machine, const State& state) { - for(auto& nested : nested_state_machine){ - if(nested.state == state.get_state()){ + for(auto& nested : nested_state_machine) + { + if(nested.state == state.get_state()) + { ErrorHandler("Only one Nested State Machine can be added per state, tried to add to state: %d", state); return; } @@ -466,19 +545,32 @@ template } - inline void refresh_state_orders(){ + inline void refresh_state_orders() + { #ifdef STLIB_ETH - if(states[current_state].state_orders_ids.size() != 0) StateOrder::add_state_orders(states[current_state].state_orders_ids); + if(states[static_cast(current_state)].state_orders_ids.size() != 0) + { + std::span ids(states[static_cast(current_state)].state_orders_ids.get_data(), + states[static_cast(current_state)].state_orders_ids.size()); + StateOrder::add_state_orders(ids); + } #endif } - inline void remove_state_orders(){ + inline void remove_state_orders() + { #ifdef STLIB_ETH - if(states[current_state].state_orders_ids.size() != 0) StateOrder::remove_state_orders(states[current_state].state_orders_ids); + if(states[static_cast(current_state)].state_orders_ids.size() != 0) + { + std::span ids(states[static_cast(current_state)].state_orders_ids.get_data(), + states[static_cast(current_state)].state_orders_ids.size()); + StateOrder::remove_state_orders(ids); + } #endif } - constexpr std::array, NStates>& get_states(){ + constexpr auto& get_states() + { return states; } diff --git a/Inc/ST-LIB_LOW/StateMachine/StateOrder.hpp b/Inc/ST-LIB_LOW/StateMachine/StateOrder.hpp index d5bfa48fe..394e99fa9 100644 --- a/Inc/ST-LIB_LOW/StateMachine/StateOrder.hpp +++ b/Inc/ST-LIB_LOW/StateMachine/StateOrder.hpp @@ -1,6 +1,7 @@ #pragma once #include "HALAL/HALAL.hpp" #include "ErrorHandler/ErrorHandler.hpp" +#include class StateOrder : public Order{ public: @@ -11,14 +12,14 @@ class StateOrder : public Order{ static OrderProtocol* informer_socket; static uint16_t state_orders_ids_size; static vector* state_orders_ids; - static StackOrder<0,uint16_t, vector> add_state_orders_order; - static StackOrder<0,uint16_t, vector> remove_state_orders_order; + static StackOrder<0,uint16_t, std::span> add_state_orders_order; + static StackOrder<0,uint16_t, std::span> remove_state_orders_order; static void set_socket(OrderProtocol& socket){ informer_socket = &socket; } - static void add_state_orders(vector& new_ids){ + static void add_state_orders(std::span new_ids){ if(informer_socket == nullptr) { ErrorHandler("Informer Socket has not been set"); return; @@ -28,7 +29,7 @@ class StateOrder : public Order{ informer_socket->send_order(add_state_orders_order); } - static void remove_state_orders(vector& old_ids){ + static void remove_state_orders(std::span old_ids){ if(informer_socket == nullptr){ ErrorHandler("Informer Socket has not been set"); return; diff --git a/Src/ST-LIB_HIGH/Protections/ProtectionManager.cpp b/Src/ST-LIB_HIGH/Protections/ProtectionManager.cpp index fc2cf39b9..3b4799be4 100644 --- a/Src/ST-LIB_HIGH/Protections/ProtectionManager.cpp +++ b/Src/ST-LIB_HIGH/Protections/ProtectionManager.cpp @@ -2,7 +2,7 @@ #include "Protections/Notification.hpp" -StateMachine* ProtectionManager::general_state_machine = nullptr; +IStateMachine* ProtectionManager::general_state_machine = nullptr; Notification ProtectionManager::fault_notification = { ProtectionManager::fault_id, nullptr}; Notification ProtectionManager::warning_notification = { @@ -37,7 +37,7 @@ void ProtectionManager::set_id(Boards::ID board_id) { ProtectionManager::board_id = board_id; } -void ProtectionManager::link_state_machine(StateMachine& general_state_machine, +void ProtectionManager::link_state_machine(IStateMachine& general_state_machine, state_id fault_id) { ProtectionManager::general_state_machine = &general_state_machine; ProtectionManager::fault_state_id = fault_id; @@ -49,13 +49,13 @@ void ProtectionManager::tcp_to_fault() { } void ProtectionManager::to_fault() { - if (general_state_machine->current_state != fault_state_id) { + if (general_state_machine->get_current_state_id() != fault_state_id) { fault_and_propagate(); } } void ProtectionManager::external_to_fault() { - if (general_state_machine->current_state != fault_state_id) { + if (general_state_machine->get_current_state_id() != fault_state_id) { external_trigger = true; fault_and_propagate(); } diff --git a/Src/ST-LIB_LOW/StateMachine/StateOrder.cpp b/Src/ST-LIB_LOW/StateMachine/StateOrder.cpp index 38fb3d6bb..a317d3869 100644 --- a/Src/ST-LIB_LOW/StateMachine/StateOrder.cpp +++ b/Src/ST-LIB_LOW/StateMachine/StateOrder.cpp @@ -1,7 +1,8 @@ #include "StateMachine/StateOrder.hpp" +#include OrderProtocol* StateOrder::informer_socket = nullptr; uint16_t StateOrder::state_orders_ids_size = 0; vector* StateOrder::state_orders_ids = nullptr; -StackOrder<0,uint16_t, vector> StateOrder::add_state_orders_order(StateOrdersID::ADD_STATE_ORDERS, &StateOrder::state_orders_ids_size, StateOrder::state_orders_ids); -StackOrder<0,uint16_t, vector> StateOrder::remove_state_orders_order(StateOrdersID::REMOVE_STATE_ORDERS, &StateOrder::state_orders_ids_size, StateOrder::state_orders_ids); +StackOrder<0,uint16_t, std::span> StateOrder::add_state_orders_order(StateOrdersID::ADD_STATE_ORDERS, &StateOrder::state_orders_ids_size, nullptr); +StackOrder<0,uint16_t, std::span> StateOrder::remove_state_orders_order(StateOrdersID::REMOVE_STATE_ORDERS, &StateOrder::state_orders_ids_size, nullptr); From d4936a703bb9197e0c812109be5ba849f49947a1 Mon Sep 17 00:00:00 2001 From: Cantonplas Date: Tue, 20 Jan 2026 23:12:17 +0100 Subject: [PATCH 12/18] Now everything (nearly) is consteval --- Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp | 22 ++++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp index 17cf88ef4..2802c79b2 100644 --- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp +++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp @@ -134,12 +134,12 @@ class State { constexpr const auto& get_exit_actions() const {return on_exit_actions;}; - constexpr void add_enter_action(Callback action) + consteval void add_enter_action(Callback action) { on_enter_actions.push_back(action); } - constexpr void add_exit_action(Callback action) + consteval void add_exit_action(Callback action) { on_exit_actions.push_back(action); } @@ -258,7 +258,7 @@ class State { } template - constexpr TimedAction * + consteval TimedAction * add_low_precision_cyclic_action(Callback action, chrono::duration period){ TimedAction timed_action = {}; @@ -272,7 +272,7 @@ class State { } template - constexpr TimedAction * + consteval TimedAction * add_mid_precision_cyclic_action(Callback action, chrono::duration period){ TimedAction timed_action = {}; @@ -286,7 +286,7 @@ class State { } template - constexpr TimedAction * + consteval TimedAction * add_high_precision_cyclic_action(Callback action, chrono::duration period){ TimedAction timed_action = {}; @@ -448,7 +448,7 @@ class StateMachine : public IStateMachine { } template - constexpr TimedAction * + consteval TimedAction * add_low_precision_cyclic_action(Callback action, chrono::duration period,const State& state){ for(size_t i = 0; i < states.size(); ++i) @@ -463,7 +463,7 @@ template } template - constexpr TimedAction * + consteval TimedAction * add_mid_precision_cyclic_action(Callback action, chrono::duration period,const State& state){ for(size_t i = 0; i < states.size(); ++i) @@ -478,7 +478,7 @@ template } template - constexpr TimedAction * + consteval TimedAction * add_high_precision_cyclic_action(Callback action, chrono::duration period,const State& state){ for(size_t i = 0; i < states.size(); ++i) @@ -493,7 +493,7 @@ template } template - constexpr void remove_cyclic_action(TimedAction *timed_action, const State& state){ + consteval void remove_cyclic_action(TimedAction *timed_action, const State& state){ for(size_t i = 0; i < states.size(); ++i) { if(states[i].get_state() == state.get_state()) @@ -506,7 +506,7 @@ template } template - constexpr void add_enter_action(Callback action, const State& state){ + consteval void add_enter_action(Callback action, const State& state){ for(size_t i = 0; i < states.size(); ++i) { if(states[i].get_state() == state.get_state()) @@ -519,7 +519,7 @@ template } template - constexpr void add_exit_action(Callback action, const State& state){ + consteval void add_exit_action(Callback action, const State& state){ for(size_t i = 0; i < states.size(); ++i) { if(states[i].get_state() == state.get_state()) From 7f130669dfba274f33a77a0f0c6049d4b1ec44ba Mon Sep 17 00:00:00 2001 From: Cantonplas Date: Wed, 21 Jan 2026 15:01:10 +0100 Subject: [PATCH 13/18] New concepts added to ensure StateEnum is a enum --- Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp | 36 +++++++++++++------- Inc/ST-LIB_LOW/StateMachine/StateOrder.hpp | 8 ++--- Src/ST-LIB_LOW/StateMachine/StateOrder.cpp | 6 ++-- 3 files changed, 31 insertions(+), 19 deletions(-) diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp index 2802c79b2..ecb9e3dbd 100644 --- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp +++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp @@ -20,6 +20,9 @@ using ms = std::chrono::milliseconds; using us = std::chrono::microseconds; using s = std::chrono::seconds; +template +concept IsEnum = std::is_enum_v; + using Callback = void (*)(); using Guard = bool (*)(); @@ -59,6 +62,7 @@ class StaticVector { if (size_ >= Capacity) { ErrorHandler("StaticVector capacity exceeded"); + return; } data[size_] = value; size_++; @@ -70,7 +74,7 @@ class StaticVector { constexpr auto end() const { return data.begin() + size_; } constexpr const std::array& get_array() const { return data; } - constexpr const size_t size() const { return size_; } + constexpr size_t size() const { return size_; } constexpr T* get_data() { return data.data(); } constexpr const T* get_data() const { return data.data(); } constexpr T& operator[](size_t i) { return data[i]; } @@ -90,7 +94,7 @@ class StaticVector { template using FixedVector = StaticVector; -template +template struct Transition { StateEnum target; Guard predicate; @@ -100,7 +104,8 @@ struct Transition { template concept are_transitions = (std::same_as> && ...); -template + +template class State { private: FixedVector cyclic_actions = {}; @@ -245,7 +250,7 @@ class State { Time::unregister_high_precision_alarm(timed_action->id); break; default: - ErrorHandler("Cannot unregister timed action with erroneus alarm precision, Alarm Precision Type: %d", timed_action->alarm_precision); + ErrorHandler("Cannot unregister timed action with erroneous alarm precision, Alarm Precision Type: %d", timed_action->alarm_precision); return; break; } @@ -323,10 +328,7 @@ class IStateMachine { template class StateMachine : public IStateMachine { -private: - using Transitions = FixedVector, NTransitions>; - using TAssocs = std::array, NStates>; - +private: struct NestedPair { StateEnum state; @@ -345,9 +347,15 @@ class StateMachine : public IStateMachine { { return; } + #ifdef STLIB_ETH + remove_state_orders(); + #endif exit(); current_state = new_state; enter(); + #ifdef STLIB_ETH + refresh_state_orders(); + #endif } size_t get_current_state_id() const override @@ -363,13 +371,14 @@ class StateMachine : public IStateMachine { private: FixedVector, NStates> states; - Transitions transitions = {}; - TAssocs transitions_assoc ={}; + FixedVector, NTransitions> transitions = {}; + std::array, NStates> transitions_assoc ={}; FixedVector nested_state_machine = {}; constexpr bool operator==(const StateMachine&) const = default; - consteval void process_state(auto state, size_t offset) + template + consteval void process_state(const State& state, size_t offset) { for (const auto& t : state.get_transitions()) { @@ -420,10 +429,14 @@ class StateMachine : public IStateMachine { if (t.predicate()) { exit(); + #ifdef STLIB_ETH remove_state_orders(); + #endif current_state = t.target; enter(); + #ifdef STLIB_ETH refresh_state_orders(); + #endif break; } } @@ -595,4 +608,3 @@ template return StateMachine(initial_state, states...); } - \ No newline at end of file diff --git a/Inc/ST-LIB_LOW/StateMachine/StateOrder.hpp b/Inc/ST-LIB_LOW/StateMachine/StateOrder.hpp index 394e99fa9..99d0a3874 100644 --- a/Inc/ST-LIB_LOW/StateMachine/StateOrder.hpp +++ b/Inc/ST-LIB_LOW/StateMachine/StateOrder.hpp @@ -11,7 +11,7 @@ class StateOrder : public Order{ }; static OrderProtocol* informer_socket; static uint16_t state_orders_ids_size; - static vector* state_orders_ids; + static std::span state_orders_ids; static StackOrder<0,uint16_t, std::span> add_state_orders_order; static StackOrder<0,uint16_t, std::span> remove_state_orders_order; @@ -23,8 +23,8 @@ class StateOrder : public Order{ if(informer_socket == nullptr) { ErrorHandler("Informer Socket has not been set"); return; - } - add_state_orders_order.set_pointer(1, &new_ids); + }; + state_orders_ids = new_ids; state_orders_ids_size = new_ids.size(); informer_socket->send_order(add_state_orders_order); } @@ -34,7 +34,7 @@ class StateOrder : public Order{ ErrorHandler("Informer Socket has not been set"); return; } - remove_state_orders_order.set_pointer(1, &old_ids); + state_orders_ids = old_ids; state_orders_ids_size = old_ids.size(); informer_socket->send_order(remove_state_orders_order); } diff --git a/Src/ST-LIB_LOW/StateMachine/StateOrder.cpp b/Src/ST-LIB_LOW/StateMachine/StateOrder.cpp index a317d3869..1f8449d27 100644 --- a/Src/ST-LIB_LOW/StateMachine/StateOrder.cpp +++ b/Src/ST-LIB_LOW/StateMachine/StateOrder.cpp @@ -3,6 +3,6 @@ OrderProtocol* StateOrder::informer_socket = nullptr; uint16_t StateOrder::state_orders_ids_size = 0; -vector* StateOrder::state_orders_ids = nullptr; -StackOrder<0,uint16_t, std::span> StateOrder::add_state_orders_order(StateOrdersID::ADD_STATE_ORDERS, &StateOrder::state_orders_ids_size, nullptr); -StackOrder<0,uint16_t, std::span> StateOrder::remove_state_orders_order(StateOrdersID::REMOVE_STATE_ORDERS, &StateOrder::state_orders_ids_size, nullptr); +std::span StateOrder::state_orders_ids; +StackOrder<0,uint16_t, std::span> StateOrder::add_state_orders_order(StateOrdersID::ADD_STATE_ORDERS, &StateOrder::state_orders_ids_size, &StateOrder::state_orders_ids); +StackOrder<0,uint16_t, std::span> StateOrder::remove_state_orders_order(StateOrdersID::REMOVE_STATE_ORDERS, &StateOrder::state_orders_ids_size, &StateOrder::state_orders_ids); From d3388851a202875ac20ef44ac722bdc5ef487b29 Mon Sep 17 00:00:00 2001 From: Cantonplas Date: Wed, 21 Jan 2026 15:42:49 +0100 Subject: [PATCH 14/18] Much more checks with concepts, implemented scheduler logic --- Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp | 208 ++++++++----------- 1 file changed, 89 insertions(+), 119 deletions(-) diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp index ecb9e3dbd..6a3ccd167 100644 --- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp +++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp @@ -8,6 +8,7 @@ #include #include #include +#include #ifdef STLIB_ETH #include "StateMachine/StateOrder.hpp" @@ -23,19 +24,22 @@ using s = std::chrono::seconds; template concept IsEnum = std::is_enum_v; +template +concept ValidTime = std::same_as || std::same_as; + using Callback = void (*)(); using Guard = bool (*)(); static constexpr size_t NUMBER_OF_ACTIONS = 20; -enum AlarmType { LOW_PRECISION, MID_PRECISION, HIGH_PRECISION }; +enum AlarmType { Miliseconds = 0, Microseconds = 1 }; class TimedAction { public: Callback action = nullptr; uint32_t period = 0; - AlarmType alarm_precision = LOW_PRECISION; + AlarmType alarm_precision = Miliseconds; uint8_t id = 255; bool is_on = false; @@ -128,6 +132,18 @@ class State { consteval State() = default; + template + consteval State(const State& other){ + state = other.get_state(); + for(const auto& t : other.get_transitions()) transitions.push_back(t); + for(const auto& a : other.get_cyclic_actions()) cyclic_actions.push_back(a); + for(const auto& a : other.get_enter_actions()) on_enter_actions.push_back(a); + for(const auto& a : other.get_exit_actions()) on_exit_actions.push_back(a); + #ifdef STLIB_ETH + for(const auto& id : other.state_orders_ids) state_orders_ids.push_back(id); + #endif + } + constexpr bool operator==(const State&) const = default; @@ -175,22 +191,7 @@ class State { if(timed_action.action == nullptr){continue;} if(!timed_action.is_on){ continue;} - switch(timed_action.alarm_precision) - { - case LOW_PRECISION: - Time::unregister_low_precision_alarm(timed_action.id); - break; - case MID_PRECISION: - Time::unregister_mid_precision_alarm(timed_action.id); - break; - case HIGH_PRECISION: - Time::unregister_high_precision_alarm(timed_action.id); - break; - default: - ErrorHandler("Cannot unregister timed action with erroneus alarm precision, Alarm Precision Type: %d", timed_action.alarm_precision); - return; - break; - } + Scheduler::unregister_task(timed_action.id); timed_action.is_on = false; } } @@ -204,16 +205,15 @@ class State { switch(timed_action.alarm_precision) { - case LOW_PRECISION: - timed_action.id = Time::register_low_precision_alarm(timed_action.period, timed_action.action); - break; - case MID_PRECISION: - timed_action.id = Time::register_mid_precision_alarm(timed_action.period, timed_action.action); + case Miliseconds: + timed_action.id = Scheduler::register_task((timed_action.period * 1000), timed_action.action); break; - case HIGH_PRECISION: - timed_action.id = Time::register_high_precision_alarm(timed_action.period, timed_action.action); + case Microseconds: + timed_action.id = Scheduler::register_task(timed_action.period, timed_action.action); break; + default: + ErrorHandler("Invalid Alarm Precision"); return; break; } @@ -238,22 +238,7 @@ class State { void unregister_timed_action(TimedAction* timed_action) { - switch(timed_action->alarm_precision) - { - case LOW_PRECISION: - Time::unregister_low_precision_alarm(timed_action->id); - break; - case MID_PRECISION: - Time::unregister_mid_precision_alarm(timed_action->id); - break; - case HIGH_PRECISION: - Time::unregister_high_precision_alarm(timed_action->id); - break; - default: - ErrorHandler("Cannot unregister timed action with erroneous alarm precision, Alarm Precision Type: %d", timed_action->alarm_precision); - return; - break; - } + Scheduler::unregister_task(timed_action->id); timed_action->is_on = false; } @@ -262,48 +247,31 @@ class State { state_orders_ids.push_back(id); } - template + template consteval TimedAction * - add_low_precision_cyclic_action(Callback action, - chrono::duration period){ + add_cyclic_action(Callback action, + TimeUnit period){ TimedAction timed_action = {}; - timed_action.alarm_precision = LOW_PRECISION; - timed_action.action = action; - uint32_t miliseconds = chrono::duration_cast(period).count(); - timed_action.period = miliseconds; - - cyclic_actions.push_back(timed_action); - return &cyclic_actions[cyclic_actions.size() - 1]; - } + if constexpr (std::is_same_v){ + timed_action.alarm_precision = Miliseconds; + timed_action.period = period.count(); + } - template - consteval TimedAction * - add_mid_precision_cyclic_action(Callback action, - chrono::duration period){ - TimedAction timed_action = {}; - timed_action.alarm_precision = MID_PRECISION; - timed_action.action = action; - uint32_t microseconds = chrono::duration_cast(period).count(); - timed_action.period = microseconds; - - cyclic_actions.push_back(timed_action); - return &cyclic_actions[cyclic_actions.size() - 1]; - } + else if constexpr (std::is_same_v){ + timed_action.alarm_precision = Microseconds; + timed_action.period = period.count(); + } - template - consteval TimedAction * - add_high_precision_cyclic_action(Callback action, - chrono::duration period){ - TimedAction timed_action = {}; - timed_action.alarm_precision = HIGH_PRECISION; - timed_action.action = action; - uint32_t microseconds = chrono::duration_cast(period).count(); - timed_action.period = microseconds; + else { + ErrorHandler("Invalid Time Unit"); + } + timed_action.action = action; cyclic_actions.push_back(timed_action); return &cyclic_actions[cyclic_actions.size() - 1]; } + }; @@ -313,12 +281,16 @@ struct is_state : std::false_type {}; template struct is_state, StateEnum> : std::true_type {}; +template +concept IsState = is_state::value; + template -concept are_states = (is_state::value && ...); +concept are_states = (IsState && ...); /// Interface for State Machines to allow other classes to interact with the state machine without knowing its implementation class IStateMachine { public: + virtual constexpr ~IStateMachine() = default; virtual void check_transitions() = 0; virtual void set_on(bool is_on) = 0; virtual void force_change_state(size_t state) = 0; @@ -340,6 +312,8 @@ class StateMachine : public IStateMachine { using state_id = StateEnum; StateEnum current_state; + constexpr ~StateMachine() override = default; + void force_change_state(size_t state) override { StateEnum new_state = static_cast(state); @@ -406,16 +380,35 @@ class StateMachine : public IStateMachine { public: - template - requires are_states + template ... S> consteval StateMachine(StateEnum initial_state, S... states) : current_state(initial_state), - states{states...} + // Initialize states with copy of states passed to constructor by using the copy constructor + states{State(states)...} { + //Sort states by their enum value + using StateType = State; + std::array sorted_states = {StateType(states)...}; + std::sort(sorted_states.begin(), sorted_states.end(), [](const auto& a, const auto& b){ + return a.get_state() < b.get_state(); + }); + + //Check that states are contiguous and start from 0 + for(size_t i = 0; i < sorted_states.size(); i++) { + if(static_cast(sorted_states[i].get_state()) != i){ + ErrorHandler("States Enum must be contiguous and start from 0"); + } + } + + for(size_t i = 0; i < sorted_states.size(); i++) { + this->states[i] = sorted_states[i]; + } + size_t offset = 0; - ((process_state(states, offset), - offset += states.get_transitions().size()), - ...); + for(const auto& s : sorted_states) { + process_state(s, offset); + offset += s.get_transitions().size(); + } } @@ -460,45 +453,18 @@ class StateMachine : public IStateMachine { enter(); } -template - consteval TimedAction * - add_low_precision_cyclic_action(Callback action, - chrono::duration period,const State& state){ - for(size_t i = 0; i < states.size(); ++i) - { - if(states[i].get_state() == state.get_state()) - { - return states[i].add_low_precision_cyclic_action(action, period); - } - } - ErrorHandler("Error: The state is not added to the state machine"); - return nullptr; - } - template - consteval TimedAction * - add_mid_precision_cyclic_action(Callback action, - chrono::duration period,const State& state){ - for(size_t i = 0; i < states.size(); ++i) - { - if(states[i].get_state() == state.get_state()) - { - return states[i].add_mid_precision_cyclic_action(action, period); - } - } - ErrorHandler("Error: The state is not added to the state machine"); - return nullptr; - } - template - consteval TimedAction * - add_high_precision_cyclic_action(Callback action, - chrono::duration period,const State& state){ + template + consteval TimedAction * + add_cyclic_action(Callback action, + TimeUnit period,const State& state) + { for(size_t i = 0; i < states.size(); ++i) { if(states[i].get_state() == state.get_state()) { - return states[i].add_high_precision_cyclic_action(action, period); + return states[i].add_cyclic_action(action, period); } } ErrorHandler("Error: The state is not added to the state machine"); @@ -506,7 +472,8 @@ template } template - consteval void remove_cyclic_action(TimedAction *timed_action, const State& state){ + consteval void remove_cyclic_action(TimedAction *timed_action, const State& state) + { for(size_t i = 0; i < states.size(); ++i) { if(states[i].get_state() == state.get_state()) @@ -519,7 +486,8 @@ template } template - consteval void add_enter_action(Callback action, const State& state){ + consteval void add_enter_action(Callback action, const State& state) + { for(size_t i = 0; i < states.size(); ++i) { if(states[i].get_state() == state.get_state()) @@ -532,7 +500,8 @@ template } template - consteval void add_exit_action(Callback action, const State& state){ + consteval void add_exit_action(Callback action, const State& state) + { for(size_t i = 0; i < states.size(); ++i) { if(states[i].get_state() == state.get_state()) @@ -545,12 +514,13 @@ template } template - constexpr void add_state_machine(IStateMachine& state_machine, const State& state) { + constexpr void add_state_machine(IStateMachine& state_machine, const State& state) + { for(auto& nested : nested_state_machine) { if(nested.state == state.get_state()) { - ErrorHandler("Only one Nested State Machine can be added per state, tried to add to state: %d", state); + ErrorHandler("Only one Nested State Machine can be added per state, tried to add to state: %d", static_cast(state.get_state())); return; } } From 84650b71b1b455015b2a998acce70118e14c71ac Mon Sep 17 00:00:00 2001 From: Cantonplas Date: Wed, 21 Jan 2026 16:27:33 +0100 Subject: [PATCH 15/18] Testing needed, but might work --- Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp | 45 +++++++++++++++----- 1 file changed, 35 insertions(+), 10 deletions(-) diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp index 6a3ccd167..c93874119 100644 --- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp +++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp @@ -115,8 +115,8 @@ class State { FixedVector cyclic_actions = {}; FixedVector on_enter_actions = {}; FixedVector on_exit_actions = {}; - StateEnum state; - FixedVector, NTransitions> transitions; + StateEnum state = {}; + FixedVector, NTransitions> transitions = {}; public: [[no_unique_address]]FixedVector state_orders_ids = {}; @@ -382,16 +382,23 @@ class StateMachine : public IStateMachine { template ... S> consteval StateMachine(StateEnum initial_state, S... states) - : current_state(initial_state), - // Initialize states with copy of states passed to constructor by using the copy constructor - states{State(states)...} + : current_state(initial_state) { //Sort states by their enum value using StateType = State; - std::array sorted_states = {StateType(states)...}; - std::sort(sorted_states.begin(), sorted_states.end(), [](const auto& a, const auto& b){ - return a.get_state() < b.get_state(); - }); + std::array sorted_states; + size_t index = 0; + ((sorted_states[index++] = StateType(states)), ...); + + for(size_t i = 0; i < sorted_states.size(); i++){ + for(size_t j = 0; j < sorted_states.size() - 1; j++){ + if(sorted_states[j].get_state() > sorted_states[j+1].get_state()){ + auto temp = sorted_states[j]; + sorted_states[j] = sorted_states[j+1]; + sorted_states[j+1] = temp; + } + } + } //Check that states are contiguous and start from 0 for(size_t i = 0; i < sorted_states.size(); i++) { @@ -401,7 +408,7 @@ class StateMachine : public IStateMachine { } for(size_t i = 0; i < sorted_states.size(); i++) { - this->states[i] = sorted_states[i]; + this->states.push_back(sorted_states[i]); } size_t offset = 0; @@ -563,6 +570,16 @@ class StateMachine : public IStateMachine { #endif }; +/* @brief Helper function to create a State instance + * + * @tparam StateEnum Enum type representing the states + * @tparam Transitions Variadic template parameter pack representing the transitions + * @param state The state enum value + * @param transitions The transitions associated with the state + * @return A State instance initialized with the provided state and transitions +* +*/ + template requires are_transitions consteval auto make_state(StateEnum state, Transitions... transitions) { @@ -570,6 +587,14 @@ template return State(state,transitions...); } + /* @brief Helper function to create a StateMachine instance + * + * @tparam States Variadic template parameter pack representing the states + * @param initial_state The initial state enum value + * @param states The states to be included in the state machine + * @return A StateMachine instance initialized with the provided initial state and states + */ + template requires are_states consteval auto make_state_machine(StateEnum initial_state, States... states) { From 2cc193d6b380b47003738df7d0042d8e122a929b Mon Sep 17 00:00:00 2001 From: Cantonplas Date: Wed, 21 Jan 2026 17:58:43 +0100 Subject: [PATCH 16/18] Functional version? --- Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp index c93874119..73e26f82e 100644 --- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp +++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp @@ -450,6 +450,12 @@ class StateMachine : public IStateMachine { } } + void start() + { + enter(); + } + + template void force_change_state(const State& state) { From 291aaa314110d06cf6b644a9d86ea473310e12f2 Mon Sep 17 00:00:00 2001 From: Cantonplas Date: Wed, 21 Jan 2026 18:16:39 +0100 Subject: [PATCH 17/18] State Machine now works!! --- Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp | 20 +++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp index 73e26f82e..0ec0f0399 100644 --- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp +++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp @@ -33,13 +33,13 @@ using Guard = bool (*)(); static constexpr size_t NUMBER_OF_ACTIONS = 20; -enum AlarmType { Miliseconds = 0, Microseconds = 1 }; +enum AlarmType { Milliseconds = 0, Microseconds = 1 }; class TimedAction { public: Callback action = nullptr; uint32_t period = 0; - AlarmType alarm_precision = Miliseconds; + AlarmType alarm_precision = Milliseconds; uint8_t id = 255; bool is_on = false; @@ -205,7 +205,7 @@ class State { switch(timed_action.alarm_precision) { - case Miliseconds: + case Milliseconds: timed_action.id = Scheduler::register_task((timed_action.period * 1000), timed_action.action); break; case Microseconds: @@ -253,7 +253,7 @@ class State { TimeUnit period){ TimedAction timed_action = {}; if constexpr (std::is_same_v){ - timed_action.alarm_precision = Miliseconds; + timed_action.alarm_precision = Milliseconds; timed_action.period = period.count(); } @@ -460,10 +460,20 @@ class StateMachine : public IStateMachine { void force_change_state(const State& state) { StateEnum new_state = state.get_state(); - if(current_state == new_state) return; + if(current_state == new_state) + { + return; + } + exit(); + #ifdef STLIB_ETH + remove_state_orders(); + #endif current_state = new_state; enter(); + #ifdef STLIB_ETH + refresh_state_orders(); + #endif } From 7c9b1771101cc275ea2d1f88cdc8666f3b17ccce Mon Sep 17 00:00:00 2001 From: Cantonplas Date: Wed, 21 Jan 2026 18:27:26 +0100 Subject: [PATCH 18/18] Moved static_vector to a seperate file --- Inc/C++Utilities/StaticVector.hpp | 53 ++++++++++++++++++ Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp | 56 ++------------------ 2 files changed, 57 insertions(+), 52 deletions(-) create mode 100644 Inc/C++Utilities/StaticVector.hpp diff --git a/Inc/C++Utilities/StaticVector.hpp b/Inc/C++Utilities/StaticVector.hpp new file mode 100644 index 000000000..76efeaa11 --- /dev/null +++ b/Inc/C++Utilities/StaticVector.hpp @@ -0,0 +1,53 @@ +#pragma once +#include +#include "ErrorHandler/ErrorHandler.hpp" + +template +class StaticVector { + private: + std::array data{}; + size_t size_ = 0; + +public: + constexpr StaticVector() = default; + + template + constexpr StaticVector(Args&&... args) : data{std::forward(args)...}, size_(sizeof...(args)) {} + + constexpr bool operator==(const StaticVector&) const = default; + + constexpr void push_back(const T& value) + { + if (size_ >= Capacity) + { + ErrorHandler("StaticVector capacity exceeded"); + return; + } + data[size_] = value; + size_++; + } + + constexpr auto begin() { return data.begin(); } + constexpr auto begin() const { return data.begin(); } + constexpr auto end() { return data.begin() + size_; } + constexpr auto end() const { return data.begin() + size_; } + + constexpr const std::array& get_array() const { return data; } + constexpr size_t size() const { return size_; } + constexpr T* get_data() { return data.data(); } + constexpr const T* get_data() const { return data.data(); } + constexpr T& operator[](size_t i) { return data[i]; } + constexpr const T& operator[](size_t i) const { return data[i]; } + constexpr bool contains(const T& value) const + { + for (size_t i = 0; i < size_; ++i) + { + if (data[i] == value) + { + return true; + } + } + return false; + } +}; + diff --git a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp index 0ec0f0399..790d02b5b 100644 --- a/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp +++ b/Inc/ST-LIB_LOW/StateMachine/StateMachine.hpp @@ -1,5 +1,6 @@ #pragma once #include "C++Utilities/CppUtils.hpp" +#include "C++Utilities/StaticVector.hpp" #include "ErrorHandler/ErrorHandler.hpp" #include "HALAL/HALAL.hpp" #include @@ -21,6 +22,9 @@ using ms = std::chrono::milliseconds; using us = std::chrono::microseconds; using s = std::chrono::seconds; +template +using FixedVector = StaticVector; + template concept IsEnum = std::is_enum_v; @@ -47,57 +51,6 @@ class TimedAction { constexpr bool operator==(const TimedAction&) const = default; }; -template -class StaticVector { - private: - std::array data{}; - size_t size_ = 0; - -public: - constexpr StaticVector() = default; - - template - constexpr StaticVector(Args&&... args) : data{std::forward(args)...}, size_(sizeof...(args)) {} - - constexpr bool operator==(const StaticVector&) const = default; - - constexpr void push_back(const T& value) - { - if (size_ >= Capacity) - { - ErrorHandler("StaticVector capacity exceeded"); - return; - } - data[size_] = value; - size_++; - } - - constexpr auto begin() { return data.begin(); } - constexpr auto begin() const { return data.begin(); } - constexpr auto end() { return data.begin() + size_; } - constexpr auto end() const { return data.begin() + size_; } - - constexpr const std::array& get_array() const { return data; } - constexpr size_t size() const { return size_; } - constexpr T* get_data() { return data.data(); } - constexpr const T* get_data() const { return data.data(); } - constexpr T& operator[](size_t i) { return data[i]; } - constexpr const T& operator[](size_t i) const { return data[i]; } - constexpr bool contains(const T& value) const - { - for (size_t i = 0; i < size_; ++i) - { - if (data[i] == value) - { - return true; - } - } - return false; - } -}; -template -using FixedVector = StaticVector; - template struct Transition { StateEnum target; @@ -309,7 +262,6 @@ class StateMachine : public IStateMachine { }; public: - using state_id = StateEnum; StateEnum current_state; constexpr ~StateMachine() override = default;