diff --git a/app/os/main.cpp b/app/os/main.cpp index aad505a219..1b6f79f7fc 100644 --- a/app/os/main.cpp +++ b/app/os/main.cpp @@ -65,6 +65,7 @@ #include "SuperSimon.h" #include "VideoKit.h" #include "bootutil/bootutil_public.h" +#include "commands/BehaviorCommand.h" #include "commands/LedFullCommand.h" #include "commands/LedRangeCommand.h" #include "commands/LedSingleCommand.h" @@ -294,6 +295,7 @@ namespace command { auto motors = MotorsCommand {motors::left::motor, motors::right::motor}; auto reinforcer = ReinforcerCommand {reinforcerkit}; auto video = VideoCommand {videokit}; + auto behavior = BehaviorCommand {behaviorkit}; } // namespace internal @@ -304,6 +306,7 @@ namespace command { &internal::motors, &internal::reinforcer, &internal::video, + &internal::behavior, }); } // namespace command diff --git a/libs/BehaviorKit/include/BehaviorKit.h b/libs/BehaviorKit/include/BehaviorKit.h index cfb7b82fe7..8aa9caa6f7 100644 --- a/libs/BehaviorKit/include/BehaviorKit.h +++ b/libs/BehaviorKit/include/BehaviorKit.h @@ -20,9 +20,6 @@ class BehaviorKit // nothing do to } - void spinLeft(float speed); - void spinRight(float speed); - void launching(); void sleeping(); void waiting(); diff --git a/libs/BehaviorKit/source/BehaviorKit.cpp b/libs/BehaviorKit/source/BehaviorKit.cpp index 776fa5b97e..64f5770696 100644 --- a/libs/BehaviorKit/source/BehaviorKit.cpp +++ b/libs/BehaviorKit/source/BehaviorKit.cpp @@ -13,18 +13,6 @@ namespace leka { using namespace std::chrono; -void BehaviorKit::spinLeft(float speed) -{ - _motor_left.spin(Rotation::clockwise, speed); - _motor_right.spin(Rotation::clockwise, speed); -} - -void BehaviorKit::spinRight(float speed) -{ - _motor_left.spin(Rotation::counterClockwise, speed); - _motor_right.spin(Rotation::counterClockwise, speed); -} - void BehaviorKit::launching() { _videokit.displayImage(fs::home::img::system::robot_misc_splash_screen_large_400); diff --git a/libs/BehaviorKit/tests/BehaviorKit_test.cpp b/libs/BehaviorKit/tests/BehaviorKit_test.cpp index 76ac9973d2..892c3cb39c 100644 --- a/libs/BehaviorKit/tests/BehaviorKit_test.cpp +++ b/libs/BehaviorKit/tests/BehaviorKit_test.cpp @@ -44,26 +44,6 @@ TEST_F(BehaviorKitTest, initialization) ASSERT_NE(&behaviorkit, nullptr); } -TEST_F(BehaviorKitTest, spinLeftAnySpeed) -{ - auto expected_speed = 0.7; - - EXPECT_CALL(mock_motor_left, spin(Rotation::clockwise, expected_speed)); - EXPECT_CALL(mock_motor_right, spin(Rotation::clockwise, expected_speed)); - - behaviorkit.spinLeft(expected_speed); -} - -TEST_F(BehaviorKitTest, spinRightAnySpeed) -{ - auto expected_speed = 0.3; - - EXPECT_CALL(mock_motor_left, spin(Rotation::counterClockwise, expected_speed)); - EXPECT_CALL(mock_motor_right, spin(Rotation::counterClockwise, expected_speed)); - - behaviorkit.spinRight(expected_speed); -} - TEST_F(BehaviorKitTest, launching) { EXPECT_CALL(mock_videokit, displayImage); diff --git a/libs/CommandKit/CMakeLists.txt b/libs/CommandKit/CMakeLists.txt index cc36fcb094..81d213ecb8 100644 --- a/libs/CommandKit/CMakeLists.txt +++ b/libs/CommandKit/CMakeLists.txt @@ -19,6 +19,7 @@ target_link_libraries(CommandKit CoreLED CoreMotor ReinforcerKit + BehaviorKit EventLoopKit mbed-os ) diff --git a/libs/CommandKit/include/commands/BehaviorCommand.h b/libs/CommandKit/include/commands/BehaviorCommand.h new file mode 100644 index 0000000000..5b1bf1fe98 --- /dev/null +++ b/libs/CommandKit/include/commands/BehaviorCommand.h @@ -0,0 +1,120 @@ +// Leka - LekaOS +// Copyright 2022 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +#include "BehaviorKit.h" +#include "Utils.h" +#include "interface/Command.h" + +namespace leka { + +struct BehaviorCommand : interface::Command { + explicit BehaviorCommand(BehaviorKit &kit) : _behaviorkit(kit) {} + + auto id() -> uint8_t override { return cmd::id; } + + auto data() -> uint8_t * override + { + args = {}; + return args.data(); + } + + [[nodiscard]] auto size() const -> std::size_t override { return std::size(args); } + + auto execute() -> bool override + { + auto [id, chcksm] = std::tuple_cat(args); + + auto expected = [&] { return utils::math::checksum8(std::span {args.data(), args.size() - 1}); }; + + if (chcksm != expected()) { + return false; + } + + switch (id) { + case cmd::behavior::stop: + _behaviorkit.stop(); + break; + case cmd::behavior::launching: + _behaviorkit.launching(); + break; + case cmd::behavior::sleeping: + _behaviorkit.sleeping(); + break; + case cmd::behavior::waiting: + _behaviorkit.waiting(); + break; + case cmd::behavior::blink_on_charge: + _behaviorkit.blinkOnCharge(); + break; + case cmd::behavior::low_battery: + _behaviorkit.lowBattery(); + break; + case cmd::behavior::charging_empty: + _behaviorkit.chargingEmpty(); + break; + case cmd::behavior::charging_low: + _behaviorkit.chargingLow(); + break; + case cmd::behavior::charging_medium: + _behaviorkit.chargingMedium(); + break; + case cmd::behavior::charging_high: + _behaviorkit.chargingHigh(); + break; + case cmd::behavior::charging_full: + _behaviorkit.chargingFull(); + break; + case cmd::behavior::ble_connection_without_video: + _behaviorkit.bleConnectionWithoutVideo(); + break; + case cmd::behavior::ble_connection_with_video: + _behaviorkit.bleConnectionWithVideo(); + break; + case cmd::behavior::working: + _behaviorkit.working(); + break; + case cmd::behavior::file_exchange: + _behaviorkit.fileExchange(); + break; + default: + _behaviorkit.stop(); + break; + } + + return true; + } + + private: + struct cmd { + static constexpr auto id = uint8_t {0x60}; + static constexpr auto size = uint8_t {1 + 1}; // id + page + Checksum + + struct behavior { + static constexpr auto stop = uint8_t {0x00}; + static constexpr auto launching = uint8_t {0x01}; + static constexpr auto sleeping = uint8_t {0x02}; + static constexpr auto waiting = uint8_t {0x03}; + static constexpr auto blink_on_charge = uint8_t {0x04}; + static constexpr auto low_battery = uint8_t {0x05}; + static constexpr auto charging_empty = uint8_t {0x06}; + static constexpr auto charging_low = uint8_t {0x07}; + static constexpr auto charging_medium = uint8_t {0x08}; + static constexpr auto charging_high = uint8_t {0x09}; + static constexpr auto charging_full = uint8_t {0x0A}; + static constexpr auto ble_connection_without_video = uint8_t {0x0B}; + static constexpr auto ble_connection_with_video = uint8_t {0x0C}; + static constexpr auto working = uint8_t {0x0D}; + static constexpr auto file_exchange = uint8_t {0x0E}; + }; + }; + + std::array args {}; + BehaviorKit &_behaviorkit; +}; + +} // namespace leka diff --git a/spikes/lk_command_kit/main.cpp b/spikes/lk_command_kit/main.cpp index 9e5c964266..7e16b51f73 100644 --- a/spikes/lk_command_kit/main.cpp +++ b/spikes/lk_command_kit/main.cpp @@ -38,6 +38,7 @@ #include "ReinforcerKit.h" #include "SDBlockDevice.h" #include "VideoKit.h" +#include "commands/BehaviorCommand.h" #include "commands/LedFullCommand.h" #include "commands/LedRangeCommand.h" #include "commands/LedSingleCommand.h" @@ -148,6 +149,7 @@ auto videokit = VideoKit {internal::event_loop, internal::corevideo}; } // namespace display +auto behaviorkit = BehaviorKit {display::videokit, ledkit, motor::left, motor::right}; auto reinforcerkit = ReinforcerKit {display::videokit, ledkit, motionkit}; namespace command { @@ -163,6 +165,7 @@ namespace internal { auto motors = MotorsCommand {motor::left, motor::right}; auto reinforcer = ReinforcerCommand {reinforcerkit}; auto video = VideoCommand {display::videokit}; + auto behavior = BehaviorCommand {behaviorkit}; } // namespace internal @@ -214,6 +217,7 @@ auto list = std::to_array({ &internal::led_range, &internal::reinforcer, &internal::video, + &internal::behavior, }); } // namespace command