diff --git a/spikes/lk_auto_charge/CMakeLists.txt b/spikes/lk_auto_charge/CMakeLists.txt index 3b7d6a4be4..08b8e02ced 100644 --- a/spikes/lk_auto_charge/CMakeLists.txt +++ b/spikes/lk_auto_charge/CMakeLists.txt @@ -28,6 +28,7 @@ target_link_libraries(spike_lk_auto_charge CoreI2C IMUKit BLEKit + BehaviorKit ) target_link_custom_leka_targets(spike_lk_auto_charge) diff --git a/spikes/lk_auto_charge/main.cpp b/spikes/lk_auto_charge/main.cpp index b291afa727..812d75a537 100644 --- a/spikes/lk_auto_charge/main.cpp +++ b/spikes/lk_auto_charge/main.cpp @@ -10,6 +10,7 @@ #include "BLEServiceCommands.h" #include "BatteryKit.h" +#include "BehaviorKit.h" #include "CoreBattery.h" #include "CoreI2C.h" #include "CoreLSM6DSOX.hpp" @@ -24,6 +25,7 @@ #include "LogKit.h" #include "MathUtils.h" #include "SealStrategy.h" +#include "behaviors/AutochargeSeal.h" using namespace std::chrono; using namespace leka; @@ -100,30 +102,11 @@ auto service_commands = BLEServiceCommands {}; auto services = std::to_array({&service_commands}); auto blekit = BLEKit {}; -auto seal_strategy = - SealStrategy {event_loop, timeout, battery::cells, motors::left::motor, motors::right::motor, imukit}; -auto happy_toupie = HappyToupie {event_loop, timeout, battery::cells, motors::left::motor, motors::right::motor}; -auto happy_fishy = HappyFishy {event_loop, timeout, battery::cells, motors::left::motor, motors::right::motor}; +auto behavior_autocharge_seal = behavior::AutochargeSeal {motors::left::motor, motors::right::motor, imukit}; +auto behaviors = std::to_array({&behavior_autocharge_seal}); +auto behavior_kit = BehaviorKit {event_loop}; -auto last_strategy = uint8_t {0x00}; - -void runStrategy(uint8_t id) -{ - if (id == 0x01) { - seal_strategy.start(); - last_strategy = id; - } else if (id == 0x02) { - happy_toupie.start(); - last_strategy = id; - } else if (id == 0x03) { - happy_fishy.start(); - last_strategy = id; - } else { - seal_strategy.stop(); - happy_toupie.stop(); - happy_fishy.stop(); - } -} +auto last_strategy = BehaviorID {0x00}; auto main() -> int { @@ -140,10 +123,16 @@ auto main() -> int imukit.init(); imukit.start(); - battery::cells.onChargeDidStop([] { runStrategy(last_strategy); }); + behavior_kit.registerBehaviors(behaviors); + + battery::cells.onChargeDidStart([] { behavior_kit.stop(); }); + battery::cells.onChargeDidStop([] { behavior_kit.start(last_strategy); }); blekit.setServices(services); - service_commands.onCommandsReceived([](std::span _buffer) { runStrategy(_buffer[0]); }); + service_commands.onCommandsReceived([](std::span _buffer) { + behavior_kit.start(_buffer[0]); + last_strategy = _buffer[0]; + }); blekit.init(); while (true) {