From 287253e527f3cdd145c0532fa608baee0d998bc8 Mon Sep 17 00:00:00 2001 From: Yann Locatelli Date: Wed, 13 Dec 2023 17:03:23 +0100 Subject: [PATCH 01/12] :truck: (BehaviorKit): Rename current BehaviorKit to BehaviorKitDeprecated --- app/os/CMakeLists.txt | 2 +- app/os/main.cpp | 2 +- .../CMakeLists.txt | 12 +++---- .../include/BehaviorKitDeprecated.h} | 6 ++-- .../source/BehaviorKitDeprecated.cpp} | 36 +++++++++---------- .../tests/BehaviorKitDeprecated_test.cpp} | 32 ++++++++--------- libs/CMakeLists.txt | 2 +- libs/RobotKit/CMakeLists.txt | 2 +- libs/RobotKit/include/RobotController.h | 6 ++-- libs/RobotKit/tests/RobotController_test.h | 4 +-- spikes/lk_activity_kit/CMakeLists.txt | 2 +- spikes/lk_activity_kit/main.cpp | 4 +-- spikes/lk_behavior_kit/CMakeLists.txt | 2 +- spikes/lk_behavior_kit/main.cpp | 4 +-- spikes/lk_command_kit/CMakeLists.txt | 2 +- tests/unit/CMakeLists.txt | 2 +- 16 files changed, 60 insertions(+), 60 deletions(-) rename libs/{BehaviorKit => BehaviorKitDeprecated}/CMakeLists.txt (50%) rename libs/{BehaviorKit/include/BehaviorKit.h => BehaviorKitDeprecated/include/BehaviorKitDeprecated.h} (82%) rename libs/{BehaviorKit/source/BehaviorKit.cpp => BehaviorKitDeprecated/source/BehaviorKitDeprecated.cpp} (71%) rename libs/{BehaviorKit/tests/BehaviorKit_test.cpp => BehaviorKitDeprecated/tests/BehaviorKitDeprecated_test.cpp} (75%) diff --git a/app/os/CMakeLists.txt b/app/os/CMakeLists.txt index 0e6ba00395..88ec154ff1 100644 --- a/app/os/CMakeLists.txt +++ b/app/os/CMakeLists.txt @@ -25,7 +25,7 @@ target_link_libraries(LekaOS FileManagerKit SerialNumberKit FirmwareKit - BehaviorKit + BehaviorKitDeprecated CorePwm CoreMotor VideoKit diff --git a/app/os/main.cpp b/app/os/main.cpp index 13f6636019..3ae0be1bb2 100644 --- a/app/os/main.cpp +++ b/app/os/main.cpp @@ -278,7 +278,7 @@ namespace motion::internal { auto motionkit = MotionKit {motors::left::motor, motors::right::motor, imukit, motion::internal::timeout}; -auto behaviorkit = BehaviorKit {videokit, ledkit, motors::left::motor, motors::right::motor}; +auto behaviorkit = BehaviorKitDeprecated {videokit, ledkit, motors::left::motor, motors::right::motor}; auto reinforcerkit = ReinforcerKit {videokit, ledkit, motionkit}; namespace command { diff --git a/libs/BehaviorKit/CMakeLists.txt b/libs/BehaviorKitDeprecated/CMakeLists.txt similarity index 50% rename from libs/BehaviorKit/CMakeLists.txt rename to libs/BehaviorKitDeprecated/CMakeLists.txt index 4366423873..907374d42e 100644 --- a/libs/BehaviorKit/CMakeLists.txt +++ b/libs/BehaviorKitDeprecated/CMakeLists.txt @@ -2,19 +2,19 @@ # Copyright 2022 APF France handicap # SPDX-License-Identifier: Apache-2.0 -add_library(BehaviorKit STATIC) +add_library(BehaviorKitDeprecated STATIC) -target_include_directories(BehaviorKit +target_include_directories(BehaviorKitDeprecated PUBLIC include ) -target_sources(BehaviorKit +target_sources(BehaviorKitDeprecated PRIVATE - source/BehaviorKit.cpp + source/BehaviorKitDeprecated.cpp ) -target_link_libraries(BehaviorKit +target_link_libraries(BehaviorKitDeprecated LedKit VideoKit CoreMotor @@ -22,6 +22,6 @@ target_link_libraries(BehaviorKit if(${CMAKE_PROJECT_NAME} STREQUAL "LekaOSUnitTests") leka_unit_tests_sources( - tests/BehaviorKit_test.cpp + tests/BehaviorKitDeprecated_test.cpp ) endif() diff --git a/libs/BehaviorKit/include/BehaviorKit.h b/libs/BehaviorKitDeprecated/include/BehaviorKitDeprecated.h similarity index 82% rename from libs/BehaviorKit/include/BehaviorKit.h rename to libs/BehaviorKitDeprecated/include/BehaviorKitDeprecated.h index cfb7b82fe7..1eccaf0295 100644 --- a/libs/BehaviorKit/include/BehaviorKit.h +++ b/libs/BehaviorKitDeprecated/include/BehaviorKitDeprecated.h @@ -10,11 +10,11 @@ namespace leka { -class BehaviorKit +class BehaviorKitDeprecated { public: - explicit BehaviorKit(interface::VideoKit &videokit, interface::LedKit &ledkit, interface::Motor &motor_left, - interface::Motor &motor_right) + explicit BehaviorKitDeprecated(interface::VideoKit &videokit, interface::LedKit &ledkit, + interface::Motor &motor_left, interface::Motor &motor_right) : _videokit(videokit), _ledkit(ledkit), _motor_left(motor_left), _motor_right(motor_right) { // nothing do to diff --git a/libs/BehaviorKit/source/BehaviorKit.cpp b/libs/BehaviorKitDeprecated/source/BehaviorKitDeprecated.cpp similarity index 71% rename from libs/BehaviorKit/source/BehaviorKit.cpp rename to libs/BehaviorKitDeprecated/source/BehaviorKitDeprecated.cpp index 776fa5b97e..51d35709dd 100644 --- a/libs/BehaviorKit/source/BehaviorKit.cpp +++ b/libs/BehaviorKitDeprecated/source/BehaviorKitDeprecated.cpp @@ -2,52 +2,52 @@ // Copyright 2022 APF France handicap // SPDX-License-Identifier: Apache-2.0 -#include "BehaviorKit.h" #include #include "rtos/ThisThread.h" +#include "BehaviorKitDeprecated.h" #include "LedKitAnimations.h" namespace leka { using namespace std::chrono; -void BehaviorKit::spinLeft(float speed) +void BehaviorKitDeprecated::spinLeft(float speed) { _motor_left.spin(Rotation::clockwise, speed); _motor_right.spin(Rotation::clockwise, speed); } -void BehaviorKit::spinRight(float speed) +void BehaviorKitDeprecated::spinRight(float speed) { _motor_left.spin(Rotation::counterClockwise, speed); _motor_right.spin(Rotation::counterClockwise, speed); } -void BehaviorKit::launching() +void BehaviorKitDeprecated::launching() { _videokit.displayImage(fs::home::img::system::robot_misc_splash_screen_large_400); } -void BehaviorKit::sleeping() +void BehaviorKitDeprecated::sleeping() { _ledkit.start(&led::animation::sleeping); _videokit.playVideoOnce(fs::home::vid::system::robot_system_sleep_yawn_then_sleep_no_eyebrows); } -void BehaviorKit::waiting() +void BehaviorKitDeprecated::waiting() { _ledkit.stop(); _videokit.playVideoOnRepeat(fs::home::vid::system::robot_system_idle_looking_top_right_left_no_eyebrows); } -void BehaviorKit::blinkOnCharge() +void BehaviorKitDeprecated::blinkOnCharge() { _ledkit.start(&led::animation::blink_on_charge); } -void BehaviorKit::lowBattery() +void BehaviorKitDeprecated::lowBattery() { _ledkit.stop(); _videokit.displayImage(fs::home::img::system::robot_battery_empty_must_be_charged); @@ -55,54 +55,54 @@ void BehaviorKit::lowBattery() _motor_right.stop(); } -void BehaviorKit::chargingEmpty() +void BehaviorKitDeprecated::chargingEmpty() { _videokit.displayImage(fs::home::img::system::robot_battery_charging_empty_red); } -void BehaviorKit::chargingLow() +void BehaviorKitDeprecated::chargingLow() { _videokit.displayImage(fs::home::img::system::robot_battery_charging_quarter_1_red); } -void BehaviorKit::chargingMedium() +void BehaviorKitDeprecated::chargingMedium() { _videokit.displayImage(fs::home::img::system::robot_battery_charging_quarter_2_orange); } -void BehaviorKit::chargingHigh() +void BehaviorKitDeprecated::chargingHigh() { _videokit.displayImage(fs::home::img::system::robot_battery_charging_quarter_3_green); } -void BehaviorKit::chargingFull() +void BehaviorKitDeprecated::chargingFull() { _videokit.displayImage(fs::home::img::system::robot_battery_charging_quarter_4_green); } -void BehaviorKit::bleConnectionWithoutVideo() +void BehaviorKitDeprecated::bleConnectionWithoutVideo() { _ledkit.start(&led::animation::ble_connection); } -void BehaviorKit::bleConnectionWithVideo() +void BehaviorKitDeprecated::bleConnectionWithVideo() { _ledkit.start(&led::animation::ble_connection); _videokit.playVideoOnce(fs::home::vid::system::robot_system_ble_connection_wink_no_eyebrows); } -void BehaviorKit::working() +void BehaviorKitDeprecated::working() { _videokit.displayImage(fs::home::img::system::robot_face_smiling_slightly); } -void BehaviorKit::fileExchange() +void BehaviorKitDeprecated::fileExchange() { // TODO(@ladislas): add file exchange image _videokit.displayImage("/fs/home/img/system/robot-file_exchange.jpg"); } -void BehaviorKit::stop() +void BehaviorKitDeprecated::stop() { _ledkit.stop(); _videokit.stopVideo(); diff --git a/libs/BehaviorKit/tests/BehaviorKit_test.cpp b/libs/BehaviorKitDeprecated/tests/BehaviorKitDeprecated_test.cpp similarity index 75% rename from libs/BehaviorKit/tests/BehaviorKit_test.cpp rename to libs/BehaviorKitDeprecated/tests/BehaviorKitDeprecated_test.cpp index 76ac9973d2..22e7978f02 100644 --- a/libs/BehaviorKit/tests/BehaviorKit_test.cpp +++ b/libs/BehaviorKitDeprecated/tests/BehaviorKitDeprecated_test.cpp @@ -2,7 +2,7 @@ // Copyright 2022 APF France handicap // SPDX-License-Identifier: Apache-2.0 -#include "BehaviorKit.h" +#include "BehaviorKitDeprecated.h" #include "LedKitAnimations.h" #include "gmock/gmock.h" @@ -21,10 +21,10 @@ MATCHER_P(isSameAnimation, expected_animation_type, "") return is_same; } -class BehaviorKitTest : public ::testing::Test +class BehaviorKitDeprecatedTest : public ::testing::Test { protected: - BehaviorKitTest() : behaviorkit(mock_videokit, mock_ledkit, mock_motor_left, mock_motor_right) {}; + BehaviorKitDeprecatedTest() : behaviorkit(mock_videokit, mock_ledkit, mock_motor_left, mock_motor_right) {}; // void SetUp() override {} // void TearDown() override {} @@ -36,15 +36,15 @@ class BehaviorKitTest : public ::testing::Test mock::CoreMotor mock_motor_left {}; mock::CoreMotor mock_motor_right {}; - BehaviorKit behaviorkit; + BehaviorKitDeprecated behaviorkit; }; -TEST_F(BehaviorKitTest, initialization) +TEST_F(BehaviorKitDeprecatedTest, initialization) { ASSERT_NE(&behaviorkit, nullptr); } -TEST_F(BehaviorKitTest, spinLeftAnySpeed) +TEST_F(BehaviorKitDeprecatedTest, spinLeftAnySpeed) { auto expected_speed = 0.7; @@ -54,7 +54,7 @@ TEST_F(BehaviorKitTest, spinLeftAnySpeed) behaviorkit.spinLeft(expected_speed); } -TEST_F(BehaviorKitTest, spinRightAnySpeed) +TEST_F(BehaviorKitDeprecatedTest, spinRightAnySpeed) { auto expected_speed = 0.3; @@ -64,13 +64,13 @@ TEST_F(BehaviorKitTest, spinRightAnySpeed) behaviorkit.spinRight(expected_speed); } -TEST_F(BehaviorKitTest, launching) +TEST_F(BehaviorKitDeprecatedTest, launching) { EXPECT_CALL(mock_videokit, displayImage); behaviorkit.launching(); } -TEST_F(BehaviorKitTest, sleeping) +TEST_F(BehaviorKitDeprecatedTest, sleeping) { EXPECT_CALL(mock_videokit, playVideoOnce); EXPECT_CALL(mock_ledkit, start(isSameAnimation(&led::animation::sleeping))).Times(1); @@ -78,14 +78,14 @@ TEST_F(BehaviorKitTest, sleeping) behaviorkit.sleeping(); } -TEST_F(BehaviorKitTest, waiting) +TEST_F(BehaviorKitDeprecatedTest, waiting) { EXPECT_CALL(mock_ledkit, stop); EXPECT_CALL(mock_videokit, playVideoOnRepeat); behaviorkit.waiting(); } -TEST_F(BehaviorKitTest, batteryBehaviors) +TEST_F(BehaviorKitDeprecatedTest, batteryBehaviors) { EXPECT_CALL(mock_videokit, displayImage).Times(6); EXPECT_CALL(mock_ledkit, stop); @@ -100,7 +100,7 @@ TEST_F(BehaviorKitTest, batteryBehaviors) behaviorkit.chargingFull(); } -TEST_F(BehaviorKitTest, bleConnectionWithoutVideo) +TEST_F(BehaviorKitDeprecatedTest, bleConnectionWithoutVideo) { EXPECT_CALL(mock_videokit, playVideoOnce).Times(0); EXPECT_CALL(mock_ledkit, start(isSameAnimation(&led::animation::ble_connection))).Times(1); @@ -108,7 +108,7 @@ TEST_F(BehaviorKitTest, bleConnectionWithoutVideo) behaviorkit.bleConnectionWithoutVideo(); } -TEST_F(BehaviorKitTest, bleConnectionWithVideo) +TEST_F(BehaviorKitDeprecatedTest, bleConnectionWithVideo) { EXPECT_CALL(mock_videokit, playVideoOnce); EXPECT_CALL(mock_ledkit, start(isSameAnimation(&led::animation::ble_connection))).Times(1); @@ -116,19 +116,19 @@ TEST_F(BehaviorKitTest, bleConnectionWithVideo) behaviorkit.bleConnectionWithVideo(); } -TEST_F(BehaviorKitTest, working) +TEST_F(BehaviorKitDeprecatedTest, working) { EXPECT_CALL(mock_videokit, displayImage); behaviorkit.working(); } -TEST_F(BehaviorKitTest, fileExchange) +TEST_F(BehaviorKitDeprecatedTest, fileExchange) { EXPECT_CALL(mock_videokit, displayImage); behaviorkit.fileExchange(); } -TEST_F(BehaviorKitTest, stop) +TEST_F(BehaviorKitDeprecatedTest, stop) { EXPECT_CALL(mock_ledkit, stop); EXPECT_CALL(mock_videokit, stopVideo); diff --git a/libs/CMakeLists.txt b/libs/CMakeLists.txt index f498e76cf5..c70dc14560 100644 --- a/libs/CMakeLists.txt +++ b/libs/CMakeLists.txt @@ -4,7 +4,7 @@ add_subdirectory(${LIBS_DIR}/ActivityKit) add_subdirectory(${LIBS_DIR}/BatteryKit) -add_subdirectory(${LIBS_DIR}/BehaviorKit) +add_subdirectory(${LIBS_DIR}/BehaviorKitDeprecated) add_subdirectory(${LIBS_DIR}/BLEKit) add_subdirectory(${LIBS_DIR}/ColorKit) add_subdirectory(${LIBS_DIR}/CommandKit) diff --git a/libs/RobotKit/CMakeLists.txt b/libs/RobotKit/CMakeLists.txt index 2d03e705f8..fa7378d7d0 100644 --- a/libs/RobotKit/CMakeLists.txt +++ b/libs/RobotKit/CMakeLists.txt @@ -26,7 +26,7 @@ target_link_libraries(RobotKit FileManagerKit CoreMutex VideoKit - BehaviorKit + BehaviorKitDeprecated CommandKit RFIDKit ActivityKit diff --git a/libs/RobotKit/include/RobotController.h b/libs/RobotKit/include/RobotController.h index 86f5c04d51..e93afeefcf 100644 --- a/libs/RobotKit/include/RobotController.h +++ b/libs/RobotKit/include/RobotController.h @@ -19,7 +19,7 @@ #include "ActivityKit.h" #include "BatteryKit.h" -#include "BehaviorKit.h" +#include "BehaviorKitDeprecated.h" #include "CommandKit.h" #include "ConfigKit.h" #include "CoreMutex.h" @@ -54,7 +54,7 @@ class RobotController : public interface::RobotController SerialNumberKit &serialnumberkit, interface::FirmwareUpdate &firmware_update, interface::Motor &motor_left, interface::Motor &motor_right, interface::LED &ears, interface::LED &belt, interface::LedKit &ledkit, interface::LCD &lcd, - interface::VideoKit &videokit, BehaviorKit &behaviorkit, CommandKit &cmdkit, + interface::VideoKit &videokit, BehaviorKitDeprecated &behaviorkit, CommandKit &cmdkit, RFIDKit &rfidkit, ActivityKit &activitykit) : _timeout_state_internal(timeout_state_internal), _timeout_state_transition(timeout_state_transition), @@ -580,7 +580,7 @@ class RobotController : public interface::RobotController RFIDKit &_rfidkit; ActivityKit &_activitykit; - BehaviorKit &_behaviorkit; + BehaviorKitDeprecated &_behaviorkit; CommandKit &_cmdkit; rtos::Thread _thread {}; diff --git a/libs/RobotKit/tests/RobotController_test.h b/libs/RobotKit/tests/RobotController_test.h index 840c2f6eaa..24e299a0cf 100644 --- a/libs/RobotKit/tests/RobotController_test.h +++ b/libs/RobotKit/tests/RobotController_test.h @@ -10,7 +10,7 @@ #include "ble_mocks.h" #include "ActivityKit.h" -#include "BehaviorKit.h" +#include "BehaviorKitDeprecated.h" #include "CommandKit.h" #include "CoreBufferedSerial.h" #include "CorePwm.h" @@ -99,7 +99,7 @@ class RobotControllerTest : public testing::Test mock::CoreLCD mock_lcd {}; mock::VideoKit mock_videokit {}; - BehaviorKit bhvkit {mock_videokit, mock_ledkit, mock_motor_left, mock_motor_right}; + BehaviorKitDeprecated bhvkit {mock_videokit, mock_ledkit, mock_motor_left, mock_motor_right}; CoreBufferedSerial serial {RFID_UART_TX, RFID_UART_RX, 57600}; CoreRFIDReaderCR95HF reader {serial}; diff --git a/spikes/lk_activity_kit/CMakeLists.txt b/spikes/lk_activity_kit/CMakeLists.txt index d551bd3a0f..476f9b47d0 100644 --- a/spikes/lk_activity_kit/CMakeLists.txt +++ b/spikes/lk_activity_kit/CMakeLists.txt @@ -17,7 +17,7 @@ target_sources(spike_lk_activity_kit target_link_libraries(spike_lk_activity_kit CoreTimeout FileManagerKit - BehaviorKit + BehaviorKitDeprecated CorePwm CoreMotor VideoKit diff --git a/spikes/lk_activity_kit/main.cpp b/spikes/lk_activity_kit/main.cpp index 453d817223..cd1a844b59 100644 --- a/spikes/lk_activity_kit/main.cpp +++ b/spikes/lk_activity_kit/main.cpp @@ -10,7 +10,7 @@ #include "rtos/Thread.h" #include "ActivityKit.h" -#include "BehaviorKit.h" +#include "BehaviorKitDeprecated.h" #include "ChooseReinforcer.h" #include "CoreBufferedSerial.h" #include "CoreDMA2D.hpp" @@ -209,7 +209,7 @@ namespace motion::internal { auto motionkit = MotionKit {motors::left::motor, motors::right::motor, imukit, motion::internal::timeout}; -auto behaviorkit = BehaviorKit {videokit, ledkit, motors::left::motor, motors::right::motor}; +auto behaviorkit = BehaviorKitDeprecated {videokit, ledkit, motors::left::motor, motors::right::motor}; auto reinforcerkit = ReinforcerKit {videokit, ledkit, motionkit}; namespace rfid { diff --git a/spikes/lk_behavior_kit/CMakeLists.txt b/spikes/lk_behavior_kit/CMakeLists.txt index 346086848f..b5e0ae0976 100644 --- a/spikes/lk_behavior_kit/CMakeLists.txt +++ b/spikes/lk_behavior_kit/CMakeLists.txt @@ -15,7 +15,7 @@ target_sources(spike_lk_behavior_kit ) target_link_libraries(spike_lk_behavior_kit - BehaviorKit + BehaviorKitDeprecated CorePwm EventLoopKit ) diff --git a/spikes/lk_behavior_kit/main.cpp b/spikes/lk_behavior_kit/main.cpp index 5eb6f7c0c8..0ac9f0a5dc 100644 --- a/spikes/lk_behavior_kit/main.cpp +++ b/spikes/lk_behavior_kit/main.cpp @@ -7,7 +7,7 @@ #include "drivers/HighResClock.h" #include "rtos/ThisThread.h" -#include "BehaviorKit.h" +#include "BehaviorKitDeprecated.h" #include "CoreDMA2D.hpp" #include "CoreDSI.hpp" #include "CoreFont.hpp" @@ -172,7 +172,7 @@ namespace display { } // namespace display -auto behaviorkit = BehaviorKit {display::videokit, leds::kit, motors::left::motor, motors::right::motor}; +auto behaviorkit = BehaviorKitDeprecated {display::videokit, leds::kit, motors::left::motor, motors::right::motor}; auto hello = HelloWorld {}; } // namespace diff --git a/spikes/lk_command_kit/CMakeLists.txt b/spikes/lk_command_kit/CMakeLists.txt index 1e9bf3303a..1b04d02f8c 100644 --- a/spikes/lk_command_kit/CMakeLists.txt +++ b/spikes/lk_command_kit/CMakeLists.txt @@ -19,7 +19,7 @@ target_link_libraries(spike_lk_command_kit CoreMotor CorePwm CoreSPI - BehaviorKit + BehaviorKitDeprecated CommandKit LedKit VideoKit diff --git a/tests/unit/CMakeLists.txt b/tests/unit/CMakeLists.txt index ac790e5550..d5af3a081d 100644 --- a/tests/unit/CMakeLists.txt +++ b/tests/unit/CMakeLists.txt @@ -281,7 +281,7 @@ leka_register_unit_tests_for_driver(CoreVideo) # Register libraries leka_register_unit_tests_for_library(ActivityKit) leka_register_unit_tests_for_library(BatteryKit) -leka_register_unit_tests_for_library(BehaviorKit) +leka_register_unit_tests_for_library(BehaviorKitDeprecated) leka_register_unit_tests_for_library(BLEKit) leka_register_unit_tests_for_library(ColorKit) leka_register_unit_tests_for_library(CommandKit) From bc8e069cd942bfb5d9ce34729f2d5d03e08ba02a Mon Sep 17 00:00:00 2001 From: Yann Locatelli Date: Wed, 13 Dec 2023 17:10:12 +0100 Subject: [PATCH 02/12] :sparkles: (BehaviorKit): Create new BehaviorKit --- libs/BehaviorKit/CMakeLists.txt | 24 +++++++++++++++++++ libs/BehaviorKit/include/BehaviorKit.h | 17 ++++++++++++++ libs/BehaviorKit/source/BehaviorKit.cpp | 7 ++++++ libs/BehaviorKit/tests/BehaviorKit_test.cpp | 26 +++++++++++++++++++++ libs/CMakeLists.txt | 1 + spikes/lk_behavior_kit/CMakeLists.txt | 1 + tests/unit/CMakeLists.txt | 1 + 7 files changed, 77 insertions(+) create mode 100644 libs/BehaviorKit/CMakeLists.txt create mode 100644 libs/BehaviorKit/include/BehaviorKit.h create mode 100644 libs/BehaviorKit/source/BehaviorKit.cpp create mode 100644 libs/BehaviorKit/tests/BehaviorKit_test.cpp diff --git a/libs/BehaviorKit/CMakeLists.txt b/libs/BehaviorKit/CMakeLists.txt new file mode 100644 index 0000000000..73c3901a1b --- /dev/null +++ b/libs/BehaviorKit/CMakeLists.txt @@ -0,0 +1,24 @@ +# Leka - LekaOS +# Copyright 2023 APF France handicap +# SPDX-License-Identifier: Apache-2.0 + +add_library(BehaviorKit STATIC) + +target_include_directories(BehaviorKit + PUBLIC + include +) + +target_sources(BehaviorKit + PRIVATE + source/BehaviorKit.cpp +) + +target_link_libraries(BehaviorKit +) + +if(${CMAKE_PROJECT_NAME} STREQUAL "LekaOSUnitTests") + leka_unit_tests_sources( + tests/BehaviorKit_test.cpp + ) +endif() diff --git a/libs/BehaviorKit/include/BehaviorKit.h b/libs/BehaviorKit/include/BehaviorKit.h new file mode 100644 index 0000000000..76f563e4f1 --- /dev/null +++ b/libs/BehaviorKit/include/BehaviorKit.h @@ -0,0 +1,17 @@ +// Leka - LekaOS +// Copyright 2023 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +namespace leka { + +class BehaviorKit +{ + public: + explicit BehaviorKit() = default; + + private: +}; + +} // namespace leka diff --git a/libs/BehaviorKit/source/BehaviorKit.cpp b/libs/BehaviorKit/source/BehaviorKit.cpp new file mode 100644 index 0000000000..80ba6d3d1f --- /dev/null +++ b/libs/BehaviorKit/source/BehaviorKit.cpp @@ -0,0 +1,7 @@ +// Leka - LekaOS +// Copyright 2023 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#include "BehaviorKit.h" + +using namespace leka; diff --git a/libs/BehaviorKit/tests/BehaviorKit_test.cpp b/libs/BehaviorKit/tests/BehaviorKit_test.cpp new file mode 100644 index 0000000000..84e497d153 --- /dev/null +++ b/libs/BehaviorKit/tests/BehaviorKit_test.cpp @@ -0,0 +1,26 @@ +// Leka - LekaOS +// Copyright 2023 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#include "BehaviorKit.h" + +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +using namespace leka; + +class BehaviorKitTest : public ::testing::Test +{ + protected: + BehaviorKitTest() : behaviorkit() {}; + + // void SetUp() override {} + // void TearDown() override {} + + BehaviorKit behaviorkit; +}; + +TEST_F(BehaviorKitTest, initialization) +{ + ASSERT_NE(&behaviorkit, nullptr); +} diff --git a/libs/CMakeLists.txt b/libs/CMakeLists.txt index c70dc14560..734c63fa87 100644 --- a/libs/CMakeLists.txt +++ b/libs/CMakeLists.txt @@ -4,6 +4,7 @@ add_subdirectory(${LIBS_DIR}/ActivityKit) add_subdirectory(${LIBS_DIR}/BatteryKit) +add_subdirectory(${LIBS_DIR}/BehaviorKit) add_subdirectory(${LIBS_DIR}/BehaviorKitDeprecated) add_subdirectory(${LIBS_DIR}/BLEKit) add_subdirectory(${LIBS_DIR}/ColorKit) diff --git a/spikes/lk_behavior_kit/CMakeLists.txt b/spikes/lk_behavior_kit/CMakeLists.txt index b5e0ae0976..ab3c1e8b55 100644 --- a/spikes/lk_behavior_kit/CMakeLists.txt +++ b/spikes/lk_behavior_kit/CMakeLists.txt @@ -15,6 +15,7 @@ target_sources(spike_lk_behavior_kit ) target_link_libraries(spike_lk_behavior_kit + BehaviorKit BehaviorKitDeprecated CorePwm EventLoopKit diff --git a/tests/unit/CMakeLists.txt b/tests/unit/CMakeLists.txt index d5af3a081d..674dce1eeb 100644 --- a/tests/unit/CMakeLists.txt +++ b/tests/unit/CMakeLists.txt @@ -281,6 +281,7 @@ leka_register_unit_tests_for_driver(CoreVideo) # Register libraries leka_register_unit_tests_for_library(ActivityKit) leka_register_unit_tests_for_library(BatteryKit) +leka_register_unit_tests_for_library(BehaviorKit) leka_register_unit_tests_for_library(BehaviorKitDeprecated) leka_register_unit_tests_for_library(BLEKit) leka_register_unit_tests_for_library(ColorKit) From 4d443b28b28473a01aed8c822733c268cfbde12b Mon Sep 17 00:00:00 2001 From: Yann Locatelli Date: Wed, 13 Dec 2023 17:28:59 +0100 Subject: [PATCH 03/12] :sparkles: (BehaviorKit): Add structure to play behaviors interface::Behavior and interface::BehaviorKit BehaviorID Register behaviors Use eventloop to start and stop behavior Unit tests + mock --- include/interface/libs/BehaviorKit.h | 25 +++++ libs/BehaviorKit/CMakeLists.txt | 2 + libs/BehaviorKit/include/BehaviorKit.h | 23 ++++- libs/BehaviorKit/include/interface/Behavior.h | 23 +++++ .../BehaviorKit/include/internal/BehaviorID.h | 15 +++ libs/BehaviorKit/source/BehaviorKit.cpp | 55 +++++++++++ libs/BehaviorKit/tests/BehaviorID_test.cpp | 29 ++++++ libs/BehaviorKit/tests/BehaviorKit_test.cpp | 94 ++++++++++++++++++- tests/unit/mocks/mocks/leka/Behavior.h | 21 +++++ 9 files changed, 283 insertions(+), 4 deletions(-) create mode 100644 include/interface/libs/BehaviorKit.h create mode 100644 libs/BehaviorKit/include/interface/Behavior.h create mode 100644 libs/BehaviorKit/include/internal/BehaviorID.h create mode 100644 libs/BehaviorKit/tests/BehaviorID_test.cpp create mode 100644 tests/unit/mocks/mocks/leka/Behavior.h diff --git a/include/interface/libs/BehaviorKit.h b/include/interface/libs/BehaviorKit.h new file mode 100644 index 0000000000..50755131b4 --- /dev/null +++ b/include/interface/libs/BehaviorKit.h @@ -0,0 +1,25 @@ +// Leka - LekaOS +// Copyright 2023 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +#include "interface/Behavior.h" +#include "internal/BehaviorID.h" + +namespace leka::interface { + +class BehaviorKit +{ + public: + virtual ~BehaviorKit() = default; + + virtual void registerBehaviors(std::span behaviors) = 0; + virtual void start(interface::Behavior *behavior) = 0; + virtual void start(BehaviorID id) = 0; + virtual void stop() = 0; +}; + +} // namespace leka::interface diff --git a/libs/BehaviorKit/CMakeLists.txt b/libs/BehaviorKit/CMakeLists.txt index 73c3901a1b..95d922c947 100644 --- a/libs/BehaviorKit/CMakeLists.txt +++ b/libs/BehaviorKit/CMakeLists.txt @@ -15,10 +15,12 @@ target_sources(BehaviorKit ) target_link_libraries(BehaviorKit + EventLoopKit ) if(${CMAKE_PROJECT_NAME} STREQUAL "LekaOSUnitTests") leka_unit_tests_sources( + tests/BehaviorID_test.cpp tests/BehaviorKit_test.cpp ) endif() diff --git a/libs/BehaviorKit/include/BehaviorKit.h b/libs/BehaviorKit/include/BehaviorKit.h index 76f563e4f1..30be72fe96 100644 --- a/libs/BehaviorKit/include/BehaviorKit.h +++ b/libs/BehaviorKit/include/BehaviorKit.h @@ -4,14 +4,33 @@ #pragma once +#include + +#include "interface/Behavior.h" +#include "interface/libs/BehaviorKit.h" +#include "interface/libs/EventLoop.h" +#include "internal/BehaviorID.h" + namespace leka { -class BehaviorKit +class BehaviorKit : public interface::BehaviorKit { public: - explicit BehaviorKit() = default; + explicit BehaviorKit(interface::EventLoop &event_loop); + + void registerBehaviors(std::span behaviors) final; + + void start(interface::Behavior *behavior) final; + void start(BehaviorID id) final; + void stop() final; private: + void run(); + + interface::EventLoop &_event_loop; + + std::span _behaviors {}; + interface::Behavior *_behavior = nullptr; }; } // namespace leka diff --git a/libs/BehaviorKit/include/interface/Behavior.h b/libs/BehaviorKit/include/interface/Behavior.h new file mode 100644 index 0000000000..5d4e668a36 --- /dev/null +++ b/libs/BehaviorKit/include/interface/Behavior.h @@ -0,0 +1,23 @@ +// Leka - LekaOS +// Copyright 2023 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include + +#include "../internal/BehaviorID.h" + +namespace leka::interface { + +struct Behavior { + virtual ~Behavior() = default; + + virtual auto id() -> BehaviorID = 0; + + virtual void run() = 0; + virtual void stop() = 0; +}; + +} // namespace leka::interface diff --git a/libs/BehaviorKit/include/internal/BehaviorID.h b/libs/BehaviorKit/include/internal/BehaviorID.h new file mode 100644 index 0000000000..b54b9a4d01 --- /dev/null +++ b/libs/BehaviorKit/include/internal/BehaviorID.h @@ -0,0 +1,15 @@ +// Leka - LekaOS +// Copyright 2023 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +using BehaviorID = uint16_t; + +namespace leka::behavior_id { + +inline constexpr BehaviorID none = 0x0000; + +} // namespace leka::behavior_id diff --git a/libs/BehaviorKit/source/BehaviorKit.cpp b/libs/BehaviorKit/source/BehaviorKit.cpp index 80ba6d3d1f..926159142f 100644 --- a/libs/BehaviorKit/source/BehaviorKit.cpp +++ b/libs/BehaviorKit/source/BehaviorKit.cpp @@ -3,5 +3,60 @@ // SPDX-License-Identifier: Apache-2.0 #include "BehaviorKit.h" +#include using namespace leka; + +BehaviorKit::BehaviorKit(interface::EventLoop &event_loop) : _event_loop(event_loop) +{ + _event_loop.registerCallback([this] { run(); }); +} + +void BehaviorKit::registerBehaviors(std::span behaviors) +{ + _behaviors = behaviors; +} + +void BehaviorKit::start(interface::Behavior *behavior) +{ + stop(); + + _behavior = nullptr; + for (auto *b: _behaviors) { + if (b == behavior) { + _behavior = b; + } + } + + if (_behavior == nullptr) { + return; + } + + _event_loop.start(); +} + +void BehaviorKit::start(BehaviorID id) +{ + interface::Behavior *found_behavior = nullptr; + + for (auto *behavior: _behaviors) { + if (id == behavior->id()) { + found_behavior = behavior; + break; + } + } + + start(found_behavior); +} + +void BehaviorKit::run() +{ + _behavior->run(); +} + +void BehaviorKit::stop() +{ + if (_behavior != nullptr) { + _behavior->stop(); + } +} diff --git a/libs/BehaviorKit/tests/BehaviorID_test.cpp b/libs/BehaviorKit/tests/BehaviorID_test.cpp new file mode 100644 index 0000000000..e3d8392bc5 --- /dev/null +++ b/libs/BehaviorKit/tests/BehaviorID_test.cpp @@ -0,0 +1,29 @@ +// Leka - LekaOS +// Copyright 2023 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#include + +#include "gtest/gtest.h" +#include "interface/Behavior.h" + +using namespace leka; + +TEST(BehaviorIDTest, behaviorsHaveUniqueID) +{ + // auto all_behaviors = std::to_array({}); + std::array all_behaviors = + {}; // TODO: to remove, only for empty list, use above instead + + auto comparator = [](interface::Behavior *a, interface::Behavior *b) { return a->id() < b->id(); }; + std::sort(all_behaviors.begin(), all_behaviors.end(), comparator); + + auto array_size = std::size(all_behaviors); + if (array_size <= 1) { + return; + } + + for (auto i = 1; i < array_size; i++) { + ASSERT_NE(all_behaviors.at(i - 1)->id(), all_behaviors.at(i)->id()); + } +} diff --git a/libs/BehaviorKit/tests/BehaviorKit_test.cpp b/libs/BehaviorKit/tests/BehaviorKit_test.cpp index 84e497d153..70b0d402be 100644 --- a/libs/BehaviorKit/tests/BehaviorKit_test.cpp +++ b/libs/BehaviorKit/tests/BehaviorKit_test.cpp @@ -6,21 +6,111 @@ #include "gmock/gmock.h" #include "gtest/gtest.h" +#include "mocks/leka/Behavior.h" +#include "stubs/leka/EventLoopKit.h" using namespace leka; +using ::testing::Return; + class BehaviorKitTest : public ::testing::Test { protected: - BehaviorKitTest() : behaviorkit() {}; + BehaviorKitTest() = default; // void SetUp() override {} // void TearDown() override {} - BehaviorKit behaviorkit; + stub::EventLoopKit stub_event_loop {}; + BehaviorKit behaviorkit {stub_event_loop}; + + mock::Behavior mock_behavior_a {}; + mock::Behavior mock_behavior_b {}; }; TEST_F(BehaviorKitTest, initialization) { ASSERT_NE(&behaviorkit, nullptr); } + +TEST_F(BehaviorKitTest, startFirstBehavior) +{ + auto behaviors = std::to_array({&mock_behavior_a}); + behaviorkit.registerBehaviors(behaviors); + + EXPECT_CALL(mock_behavior_a, run); + + behaviorkit.start(&mock_behavior_a); +} + +TEST_F(BehaviorKitTest, startBehaviorNullPtr) +{ + // nothing expected + + behaviorkit.start(nullptr); +} + +TEST_F(BehaviorKitTest, startFirstBehaviorID) +{ + auto behaviors = std::to_array({ + &mock_behavior_a, + &mock_behavior_b, + }); + behaviorkit.registerBehaviors(behaviors); + + auto behavior_a_id = BehaviorID {1}; + auto behavior_b_id = BehaviorID {2}; + + EXPECT_CALL(mock_behavior_a, id).WillRepeatedly(Return(behavior_a_id)); + EXPECT_CALL(mock_behavior_b, id).WillRepeatedly(Return(behavior_b_id)); + EXPECT_CALL(mock_behavior_b, run); + + behaviorkit.start(behavior_b_id); +} + +TEST_F(BehaviorKitTest, startBehaviorIDNotRegistered) +{ + auto behaviors = std::to_array({ + &mock_behavior_a, + &mock_behavior_b, + }); + behaviorkit.registerBehaviors(behaviors); + + auto behavior_a_id = BehaviorID {1}; + auto behavior_b_id = BehaviorID {2}; + + EXPECT_CALL(mock_behavior_a, id).WillRepeatedly(Return(behavior_a_id)); + EXPECT_CALL(mock_behavior_b, id).WillRepeatedly(Return(behavior_b_id)); + + behaviorkit.start(BehaviorID {3}); +} + +TEST_F(BehaviorKitTest, startBehaviorNotRegistered) +{ + auto behaviors = std::to_array({ + &mock_behavior_a, + }); + behaviorkit.registerBehaviors(behaviors); + + EXPECT_CALL(mock_behavior_b, run).Times(0); + + behaviorkit.start(&mock_behavior_b); +} + +TEST_F(BehaviorKitTest, startAnyBehavior) +{ + auto behaviors = std::to_array({ + &mock_behavior_a, + &mock_behavior_b, + }); + behaviorkit.registerBehaviors(behaviors); + + EXPECT_CALL(mock_behavior_a, run); + + behaviorkit.start(&mock_behavior_a); + + EXPECT_CALL(mock_behavior_a, stop); + EXPECT_CALL(mock_behavior_b, run); + + behaviorkit.start(&mock_behavior_b); +} diff --git a/tests/unit/mocks/mocks/leka/Behavior.h b/tests/unit/mocks/mocks/leka/Behavior.h new file mode 100644 index 0000000000..377b57c7d7 --- /dev/null +++ b/tests/unit/mocks/mocks/leka/Behavior.h @@ -0,0 +1,21 @@ +// Leka - LekaOS +// Copyright 2023 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "gmock/gmock.h" +#include "interface/Behavior.h" + +namespace leka::mock { + +class Behavior : public interface::Behavior +{ + public: + MOCK_METHOD(BehaviorID, id, (), (override)); + + MOCK_METHOD(void, run, (), (override)); + MOCK_METHOD(void, stop, (), (override)); +}; + +} // namespace leka::mock From 283355b4653b2a146cddd00748472cb6d3a32a3b Mon Sep 17 00:00:00 2001 From: Yann Locatelli Date: Wed, 20 Dec 2023 13:25:53 +0100 Subject: [PATCH 04/12] Apply suggestion from oral review Remove eventloop --- libs/BehaviorKit/include/BehaviorKit.h | 7 +------ libs/BehaviorKit/source/BehaviorKit.cpp | 12 +----------- libs/BehaviorKit/tests/BehaviorKit_test.cpp | 4 +--- 3 files changed, 3 insertions(+), 20 deletions(-) diff --git a/libs/BehaviorKit/include/BehaviorKit.h b/libs/BehaviorKit/include/BehaviorKit.h index 30be72fe96..4de8db6677 100644 --- a/libs/BehaviorKit/include/BehaviorKit.h +++ b/libs/BehaviorKit/include/BehaviorKit.h @@ -8,7 +8,6 @@ #include "interface/Behavior.h" #include "interface/libs/BehaviorKit.h" -#include "interface/libs/EventLoop.h" #include "internal/BehaviorID.h" namespace leka { @@ -16,7 +15,7 @@ namespace leka { class BehaviorKit : public interface::BehaviorKit { public: - explicit BehaviorKit(interface::EventLoop &event_loop); + explicit BehaviorKit() = default; void registerBehaviors(std::span behaviors) final; @@ -25,10 +24,6 @@ class BehaviorKit : public interface::BehaviorKit void stop() final; private: - void run(); - - interface::EventLoop &_event_loop; - std::span _behaviors {}; interface::Behavior *_behavior = nullptr; }; diff --git a/libs/BehaviorKit/source/BehaviorKit.cpp b/libs/BehaviorKit/source/BehaviorKit.cpp index 926159142f..7d3d263072 100644 --- a/libs/BehaviorKit/source/BehaviorKit.cpp +++ b/libs/BehaviorKit/source/BehaviorKit.cpp @@ -7,11 +7,6 @@ using namespace leka; -BehaviorKit::BehaviorKit(interface::EventLoop &event_loop) : _event_loop(event_loop) -{ - _event_loop.registerCallback([this] { run(); }); -} - void BehaviorKit::registerBehaviors(std::span behaviors) { _behaviors = behaviors; @@ -32,7 +27,7 @@ void BehaviorKit::start(interface::Behavior *behavior) return; } - _event_loop.start(); + _behavior->run(); } void BehaviorKit::start(BehaviorID id) @@ -49,11 +44,6 @@ void BehaviorKit::start(BehaviorID id) start(found_behavior); } -void BehaviorKit::run() -{ - _behavior->run(); -} - void BehaviorKit::stop() { if (_behavior != nullptr) { diff --git a/libs/BehaviorKit/tests/BehaviorKit_test.cpp b/libs/BehaviorKit/tests/BehaviorKit_test.cpp index 70b0d402be..2c1d7bfe29 100644 --- a/libs/BehaviorKit/tests/BehaviorKit_test.cpp +++ b/libs/BehaviorKit/tests/BehaviorKit_test.cpp @@ -7,7 +7,6 @@ #include "gmock/gmock.h" #include "gtest/gtest.h" #include "mocks/leka/Behavior.h" -#include "stubs/leka/EventLoopKit.h" using namespace leka; @@ -21,8 +20,7 @@ class BehaviorKitTest : public ::testing::Test // void SetUp() override {} // void TearDown() override {} - stub::EventLoopKit stub_event_loop {}; - BehaviorKit behaviorkit {stub_event_loop}; + BehaviorKit behaviorkit {}; mock::Behavior mock_behavior_a {}; mock::Behavior mock_behavior_b {}; From b5ea6f46e7a8aa224b83e228a3f329ab9b0a5835 Mon Sep 17 00:00:00 2001 From: Yann Locatelli Date: Wed, 22 Feb 2023 19:54:53 +0100 Subject: [PATCH 05/12] :alembic: (spike): Add autocharge of robot seal strategy --- spikes/CMakeLists.txt | 2 + spikes/lk_auto_charge/CMakeLists.txt | 31 ++++++ spikes/lk_auto_charge/SealStrategy.cpp | 112 ++++++++++++++++++++ spikes/lk_auto_charge/SealStrategy.h | 64 +++++++++++ spikes/lk_auto_charge/main.cpp | 140 +++++++++++++++++++++++++ 5 files changed, 349 insertions(+) create mode 100644 spikes/lk_auto_charge/CMakeLists.txt create mode 100644 spikes/lk_auto_charge/SealStrategy.cpp create mode 100644 spikes/lk_auto_charge/SealStrategy.h create mode 100644 spikes/lk_auto_charge/main.cpp diff --git a/spikes/CMakeLists.txt b/spikes/CMakeLists.txt index cc3bd94479..d9dabd86ad 100644 --- a/spikes/CMakeLists.txt +++ b/spikes/CMakeLists.txt @@ -4,6 +4,7 @@ add_subdirectory(${SPIKES_DIR}/lk_activity_kit) add_subdirectory(${SPIKES_DIR}/lk_audio) +add_subdirectory(${SPIKES_DIR}/lk_auto_charge) add_subdirectory(${SPIKES_DIR}/lk_behavior_kit) add_subdirectory(${SPIKES_DIR}/lk_ble) add_subdirectory(${SPIKES_DIR}/lk_bluetooth) @@ -50,6 +51,7 @@ add_subdirectory(${SPIKES_DIR}/stl_cxxsupport) add_custom_target(spikes_leka) add_dependencies(spikes_leka spike_lk_activity_kit + spike_lk_auto_charge spike_lk_ble spike_lk_bluetooth spike_lk_cg_animations diff --git a/spikes/lk_auto_charge/CMakeLists.txt b/spikes/lk_auto_charge/CMakeLists.txt new file mode 100644 index 0000000000..af7d115332 --- /dev/null +++ b/spikes/lk_auto_charge/CMakeLists.txt @@ -0,0 +1,31 @@ +# Leka - LekaOS +# Copyright 2023 APF France handicap +# SPDX-License-Identifier: Apache-2.0 + +add_mbed_executable(spike_lk_auto_charge) + +target_include_directories(spike_lk_auto_charge + PRIVATE + . +) + +target_sources(spike_lk_auto_charge + PRIVATE + main.cpp + SealStrategy.cpp +) + +target_link_libraries(spike_lk_auto_charge + EventLoopKit + CoreTimeout + CoreBattery + BatteryKit + CorePwm + CoreMotor + CoreIMU + CoreI2C + IMUKit + BLEKit +) + +target_link_custom_leka_targets(spike_lk_auto_charge) diff --git a/spikes/lk_auto_charge/SealStrategy.cpp b/spikes/lk_auto_charge/SealStrategy.cpp new file mode 100644 index 0000000000..c766371440 --- /dev/null +++ b/spikes/lk_auto_charge/SealStrategy.cpp @@ -0,0 +1,112 @@ +// Leka - LekaOS +// Copyright 2023 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#include "SealStrategy.h" + +#include "rtos/ThisThread.h" + +#include "MathUtils.h" + +using namespace leka; +using namespace std::chrono; + +auto SealStrategy::convertToPwmFrom(float angle) const -> float +{ + auto res = utils::math::map(angle, kMinAngleInput, kMaxAngleInput, kMinPwmOutput, kMaxPwmOutput); + return res; +} + +void SealStrategy::start() +{ + auto on_timeout = [this] { stop(); }; + _timeout.onTimeout(on_timeout); + _timeout.start(20s); + + should_stop = false; + + _event_loop.registerCallback([this] { run(); }); + _event_loop.start(); +} + +void SealStrategy::stop() +{ + should_stop = true; + _event_loop.stop(); + rtos::ThisThread::sleep_for(100ms); + stopMotors(); + _timeout.stop(); +} + +void SealStrategy::run() +{ + if (should_stop || _battery.isCharging()) { + stop(); + return; + } + + auto angles = _imukit.getEulerAngles(); + auto is_right_tilted = angles.roll > 0; + auto should_move_forward = angles.pitch > 0; + + auto abs_float = [](float value) { return value > 0 ? value : -value; }; + + if (abs_float(angles.roll) > kRollTolerance) { + auto speed_offset = convertToPwmFrom(angles.roll > 0 ? angles.roll : -angles.roll); + if (is_right_tilted) { + if (should_move_forward) { + spinRight(kMinPwmOutput, kMinPwmOutput + speed_offset); + } else { + spinLeft(kMinPwmOutput, kMinPwmOutput + speed_offset); + } + } else { + if (should_move_forward) { + spinLeft(kMinPwmOutput + speed_offset, kMinPwmOutput); + } else { + spinRight(kMinPwmOutput + speed_offset, kMinPwmOutput); + } + } + } else if (abs_float(angles.pitch) > kPitchTolerance) { + auto speed = convertToPwmFrom(angles.pitch > 0 ? angles.pitch : -angles.pitch); + if (should_move_forward) { + moveForward(speed); + } else { + moveBackward(speed); + } + } else { + stopMotors(); + } + + rtos::ThisThread::sleep_for(10ms); + _event_loop.start(); +} + +void SealStrategy::stopMotors() +{ + _motor_right.stop(); + _motor_left.stop(); +} + +void SealStrategy::spinLeft(float left_speed, float right_speed) +{ + _motor_left.spin(Rotation::clockwise, left_speed); + _motor_right.spin(Rotation::clockwise, right_speed); +} + +void SealStrategy::spinRight(float left_speed, float right_speed) +{ + _motor_left.spin(Rotation::counterClockwise, left_speed); + _motor_right.spin(Rotation::counterClockwise, right_speed); +} + +void SealStrategy::moveForward(float speed) +{ + _motor_left.spin(Rotation::counterClockwise, speed); + _motor_right.spin(Rotation::clockwise, speed); +} + +void SealStrategy::moveBackward(float speed) +{ + _motor_left.spin(Rotation::clockwise, speed); + _motor_right.spin(Rotation::counterClockwise, speed); +} diff --git a/spikes/lk_auto_charge/SealStrategy.h b/spikes/lk_auto_charge/SealStrategy.h new file mode 100644 index 0000000000..469facdaaf --- /dev/null +++ b/spikes/lk_auto_charge/SealStrategy.h @@ -0,0 +1,64 @@ +// Leka - LekaOS +// Copyright 2023 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "interface/drivers/Battery.h" +#include "interface/drivers/Motor.h" +#include "interface/drivers/Timeout.h" +#include "interface/libs/EventLoop.h" +#include "interface/libs/IMUKit.hpp" + +namespace leka { + +class SealStrategy +{ + public: + SealStrategy(interface::EventLoop &event_loop, interface::Timeout &timeout, interface::Battery &battery, + interface::Motor &motor_left, interface::Motor &motor_right, interface::IMUKit &imu_kit) + : _event_loop(event_loop), + _timeout(timeout), + _battery(battery), + _motor_left(motor_left), + _motor_right(motor_right), + _imukit(imu_kit) + { + } + + ~SealStrategy() = default; + + void start(); + void stop(); + + private: + [[nodiscard]] auto convertToPwmFrom(float angle) const -> float; + + void run(); + + void stopMotors(); + void spinRight(float left_speed, float right_speed); + void spinLeft(float left_speed, float right_speed); + void moveForward(float speed); + void moveBackward(float speed); + + interface::EventLoop &_event_loop; + interface::Timeout &_timeout; + + interface::Battery &_battery; + interface::Motor &_motor_left; + interface::Motor &_motor_right; + interface::IMUKit &_imukit; + + const float kMinAngleInput = 0.F; + const float kMaxAngleInput = 30.F; + const float kMinPwmOutput = 0.15F; // Min to move robot + const float kMaxPwmOutput = 0.70F; + + const float kRollTolerance = 3.F; // in degrees + const float kPitchTolerance = 3.F; // in degrees + + bool should_stop = true; +}; + +} // namespace leka diff --git a/spikes/lk_auto_charge/main.cpp b/spikes/lk_auto_charge/main.cpp new file mode 100644 index 0000000000..49403e1efa --- /dev/null +++ b/spikes/lk_auto_charge/main.cpp @@ -0,0 +1,140 @@ +// Leka - LekaOS +// Copyright 2023 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#include "drivers/DigitalOut.h" +#include "drivers/InterruptIn.h" +#include "rtos/ThisThread.h" + +#include "BLEKit.h" +#include "BLEServiceCommands.h" + +#include "BatteryKit.h" +#include "CoreBattery.h" +#include "CoreI2C.h" +#include "CoreLSM6DSOX.hpp" +#include "CoreMotor.h" +#include "CorePwm.h" +#include "CoreTimeout.h" +#include "EventLoopKit.h" +#include "HelloWorld.h" +#include "IMUKit.hpp" +#include "LogKit.h" +#include "MathUtils.h" +#include "SealStrategy.h" + +using namespace std::chrono; +using namespace leka; + +namespace { + +auto event_loop = EventLoopKit {}; +auto timeout = CoreTimeout {}; + +namespace battery { + + namespace charge { + + auto status_input = mbed::InterruptIn {PinName::BATTERY_CHARGE_STATUS}; + + } + + auto cells = CoreBattery {PinName::BATTERY_VOLTAGE, battery::charge::status_input}; + +} // namespace battery + +namespace motors { + + namespace left { + + namespace internal { + + auto dir_1 = mbed::DigitalOut {MOTOR_LEFT_DIRECTION_1}; + auto dir_2 = mbed::DigitalOut {MOTOR_LEFT_DIRECTION_2}; + auto speed = CorePwm {MOTOR_LEFT_PWM}; + + } // namespace internal + + auto motor = CoreMotor {internal::dir_1, internal::dir_2, internal::speed}; + + } // namespace left + + namespace right { + + namespace internal { + + auto dir_1 = mbed::DigitalOut {MOTOR_RIGHT_DIRECTION_1}; + auto dir_2 = mbed::DigitalOut {MOTOR_RIGHT_DIRECTION_2}; + auto speed = CorePwm {MOTOR_RIGHT_PWM}; + + } // namespace internal + + auto motor = CoreMotor {internal::dir_1, internal::dir_2, internal::speed}; + + } // namespace right + +} // namespace motors + +namespace imu { + + namespace internal { + + auto drdy_irq = CoreInterruptIn {PinName::SENSOR_IMU_IRQ}; + auto i2c = CoreI2C(PinName::SENSOR_IMU_TH_I2C_SDA, PinName::SENSOR_IMU_TH_I2C_SCL); + + } // namespace internal + + CoreLSM6DSOX lsm6dsox(internal::i2c, internal::drdy_irq); + +} // namespace imu + +IMUKit imukit(imu::lsm6dsox); + +} // namespace + +auto mainboard_led = mbed::DigitalOut {LED1}; + +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 last_strategy = uint8_t {0x00}; + +void runStrategy(uint8_t id) +{ + if (id == 0x01) { + seal_strategy.start(); + last_strategy = id; + } else { + seal_strategy.stop(); + } +} + +auto main() -> int +{ + logger::init(); + + HelloWorld hello; + hello.start(); + + log_info("Hello world!"); + + imu::lsm6dsox.init(); + + imukit.stop(); + imukit.init(); + imukit.start(); + + battery::cells.onChargeDidStop([] { runStrategy(last_strategy); }); + + blekit.setServices(services); + service_commands.onCommandsReceived([](std::span _buffer) { runStrategy(_buffer[0]); }); + blekit.init(); + + while (true) { + rtos::ThisThread::sleep_for(10min); + } +} From 2f8226a4e63d457d99f05317e0a70072e2dd07ca Mon Sep 17 00:00:00 2001 From: Yann Locatelli Date: Tue, 12 Dec 2023 17:21:06 +0100 Subject: [PATCH 06/12] :alembic: (spike): Autocharge - HappyToupie An original suggestion from @HPezz Co-Authored-By: Hugo Pezziardi <84374761+HPezz@users.noreply.github.com> --- spikes/lk_auto_charge/CMakeLists.txt | 1 + spikes/lk_auto_charge/HappyToupie.cpp | 82 +++++++++++++++++++++++++++ spikes/lk_auto_charge/HappyToupie.h | 58 +++++++++++++++++++ spikes/lk_auto_charge/main.cpp | 6 ++ 4 files changed, 147 insertions(+) create mode 100644 spikes/lk_auto_charge/HappyToupie.cpp create mode 100644 spikes/lk_auto_charge/HappyToupie.h diff --git a/spikes/lk_auto_charge/CMakeLists.txt b/spikes/lk_auto_charge/CMakeLists.txt index af7d115332..f942443be6 100644 --- a/spikes/lk_auto_charge/CMakeLists.txt +++ b/spikes/lk_auto_charge/CMakeLists.txt @@ -13,6 +13,7 @@ target_sources(spike_lk_auto_charge PRIVATE main.cpp SealStrategy.cpp + HappyToupie.cpp ) target_link_libraries(spike_lk_auto_charge diff --git a/spikes/lk_auto_charge/HappyToupie.cpp b/spikes/lk_auto_charge/HappyToupie.cpp new file mode 100644 index 0000000000..877b535008 --- /dev/null +++ b/spikes/lk_auto_charge/HappyToupie.cpp @@ -0,0 +1,82 @@ +// Leka - LekaOS +// Copyright 2023 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#include "HappyToupie.h" + +#include "rtos/ThisThread.h" + +#include "MathUtils.h" + +using namespace leka; +using namespace std::chrono; + +auto HappyToupie::convertToPwmFrom(float angle) const -> float +{ + auto res = utils::math::map(angle, kMinAngleInput, kMaxAngleInput, kMinPwmOutput, kMaxPwmOutput); + return res; +} + +void HappyToupie::start() +{ + auto on_timeout = [this] { stop(); }; + _timeout.onTimeout(on_timeout); + _timeout.start(20s); + + should_stop = false; + + _event_loop.registerCallback([this] { run(); }); + _event_loop.start(); +} + +void HappyToupie::stop() +{ + should_stop = true; + _event_loop.stop(); + rtos::ThisThread::sleep_for(100ms); + stopMotors(); + _timeout.stop(); +} + +void HappyToupie::run() +{ + if (should_stop || _battery.isCharging()) { + stop(); + return; + } + + spinLeft(1.F, 1.F); + + rtos::ThisThread::sleep_for(10ms); + _event_loop.start(); +} + +void HappyToupie::stopMotors() +{ + _motor_right.stop(); + _motor_left.stop(); +} + +void HappyToupie::spinLeft(float left_speed, float right_speed) +{ + _motor_left.spin(Rotation::clockwise, left_speed); + _motor_right.spin(Rotation::clockwise, right_speed); +} + +void HappyToupie::spinRight(float left_speed, float right_speed) +{ + _motor_left.spin(Rotation::counterClockwise, left_speed); + _motor_right.spin(Rotation::counterClockwise, right_speed); +} + +void HappyToupie::moveForward(float speed) +{ + _motor_left.spin(Rotation::counterClockwise, speed); + _motor_right.spin(Rotation::clockwise, speed); +} + +void HappyToupie::moveBackward(float speed) +{ + _motor_left.spin(Rotation::clockwise, speed); + _motor_right.spin(Rotation::counterClockwise, speed); +} diff --git a/spikes/lk_auto_charge/HappyToupie.h b/spikes/lk_auto_charge/HappyToupie.h new file mode 100644 index 0000000000..8777a6ee33 --- /dev/null +++ b/spikes/lk_auto_charge/HappyToupie.h @@ -0,0 +1,58 @@ +// Leka - LekaOS +// Copyright 2023 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "interface/drivers/Battery.h" +#include "interface/drivers/Motor.h" +#include "interface/drivers/Timeout.h" +#include "interface/libs/EventLoop.h" + +namespace leka { + +class HappyToupie +{ + public: + HappyToupie(interface::EventLoop &event_loop, interface::Timeout &timeout, interface::Battery &battery, + interface::Motor &motor_left, interface::Motor &motor_right) + : _event_loop(event_loop), + _timeout(timeout), + _battery(battery), + _motor_left(motor_left), + _motor_right(motor_right) + { + } + + ~HappyToupie() = default; + + void start(); + void stop(); + + private: + [[nodiscard]] auto convertToPwmFrom(float angle) const -> float; + + void run(); + + void stopMotors(); + void spinRight(float left_speed, float right_speed); + void spinLeft(float left_speed, float right_speed); + void moveForward(float speed); + void moveBackward(float speed); + + interface::EventLoop &_event_loop; + interface::Timeout &_timeout; + + interface::Battery &_battery; + interface::Motor &_motor_left; + interface::Motor &_motor_right; + + const float kMinAngleInput = 0.F; + const float kMaxAngleInput = 30.F; + const float kMinPwmOutput = 0.15F; // Min to move robot + const float kMaxPwmOutput = 0.70F; + + bool should_stop = true; +}; + +} // namespace leka diff --git a/spikes/lk_auto_charge/main.cpp b/spikes/lk_auto_charge/main.cpp index 49403e1efa..0ebad93e77 100644 --- a/spikes/lk_auto_charge/main.cpp +++ b/spikes/lk_auto_charge/main.cpp @@ -17,6 +17,7 @@ #include "CorePwm.h" #include "CoreTimeout.h" #include "EventLoopKit.h" +#include "HappyToupie.h" #include "HelloWorld.h" #include "IMUKit.hpp" #include "LogKit.h" @@ -100,6 +101,7 @@ 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 last_strategy = uint8_t {0x00}; @@ -108,8 +110,12 @@ 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 { seal_strategy.stop(); + happy_toupie.stop(); } } From 1be69acb6112518d87391c866f7fe5a1a2d9a843 Mon Sep 17 00:00:00 2001 From: Yann Locatelli Date: Tue, 12 Dec 2023 17:21:38 +0100 Subject: [PATCH 07/12] :alembic: (spike): Autocharge - HappyFishy An original suggestion from @HPezz Co-Authored-By: Hugo Pezziardi <84374761+HPezz@users.noreply.github.com> --- spikes/lk_auto_charge/CMakeLists.txt | 1 + spikes/lk_auto_charge/HappyFishy.cpp | 88 ++++++++++++++++++++++++++++ spikes/lk_auto_charge/HappyFishy.h | 60 +++++++++++++++++++ spikes/lk_auto_charge/main.cpp | 6 ++ 4 files changed, 155 insertions(+) create mode 100644 spikes/lk_auto_charge/HappyFishy.cpp create mode 100644 spikes/lk_auto_charge/HappyFishy.h diff --git a/spikes/lk_auto_charge/CMakeLists.txt b/spikes/lk_auto_charge/CMakeLists.txt index f942443be6..3b7d6a4be4 100644 --- a/spikes/lk_auto_charge/CMakeLists.txt +++ b/spikes/lk_auto_charge/CMakeLists.txt @@ -14,6 +14,7 @@ target_sources(spike_lk_auto_charge main.cpp SealStrategy.cpp HappyToupie.cpp + HappyFishy.cpp ) target_link_libraries(spike_lk_auto_charge diff --git a/spikes/lk_auto_charge/HappyFishy.cpp b/spikes/lk_auto_charge/HappyFishy.cpp new file mode 100644 index 0000000000..933648b5c0 --- /dev/null +++ b/spikes/lk_auto_charge/HappyFishy.cpp @@ -0,0 +1,88 @@ +// Leka - LekaOS +// Copyright 2023 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#include "HappyFishy.h" + +#include "rtos/ThisThread.h" + +#include "MathUtils.h" + +using namespace leka; +using namespace std::chrono; + +auto HappyFishy::convertToPwmFrom(float angle) const -> float +{ + auto res = utils::math::map(angle, kMinAngleInput, kMaxAngleInput, kMinPwmOutput, kMaxPwmOutput); + return res; +} + +void HappyFishy::start() +{ + auto on_timeout = [this] { stop(); }; + _timeout.onTimeout(on_timeout); + _timeout.start(20s); + + should_stop = false; + + _event_loop.registerCallback([this] { run(); }); + _event_loop.start(); +} + +void HappyFishy::stop() +{ + should_stop = true; + _event_loop.stop(); + rtos::ThisThread::sleep_for(100ms); + stopMotors(); + _timeout.stop(); +} + +void HappyFishy::run() +{ + if (should_stop || _battery.isCharging()) { + stop(); + return; + } + + if (move_left) { + spinLeft(0.25F, 0.25F); + rtos::ThisThread::sleep_for(300ms); + } else { + spinRight(0.25F, 0.25F); + rtos::ThisThread::sleep_for(320ms); + } + move_left = !move_left; + + _event_loop.start(); +} + +void HappyFishy::stopMotors() +{ + _motor_right.stop(); + _motor_left.stop(); +} + +void HappyFishy::spinLeft(float left_speed, float right_speed) +{ + _motor_left.spin(Rotation::clockwise, left_speed); + _motor_right.spin(Rotation::clockwise, right_speed); +} + +void HappyFishy::spinRight(float left_speed, float right_speed) +{ + _motor_left.spin(Rotation::counterClockwise, left_speed); + _motor_right.spin(Rotation::counterClockwise, right_speed); +} + +void HappyFishy::moveForward(float speed) +{ + _motor_left.spin(Rotation::counterClockwise, speed); + _motor_right.spin(Rotation::clockwise, speed); +} + +void HappyFishy::moveBackward(float speed) +{ + _motor_left.spin(Rotation::clockwise, speed); + _motor_right.spin(Rotation::counterClockwise, speed); +} diff --git a/spikes/lk_auto_charge/HappyFishy.h b/spikes/lk_auto_charge/HappyFishy.h new file mode 100644 index 0000000000..7ab1329799 --- /dev/null +++ b/spikes/lk_auto_charge/HappyFishy.h @@ -0,0 +1,60 @@ +// Leka - LekaOS +// Copyright 2023 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "interface/drivers/Battery.h" +#include "interface/drivers/Motor.h" +#include "interface/drivers/Timeout.h" +#include "interface/libs/EventLoop.h" + +namespace leka { + +class HappyFishy +{ + public: + HappyFishy(interface::EventLoop &event_loop, interface::Timeout &timeout, interface::Battery &battery, + interface::Motor &motor_left, interface::Motor &motor_right) + : _event_loop(event_loop), + _timeout(timeout), + _battery(battery), + _motor_left(motor_left), + _motor_right(motor_right) + { + } + + ~HappyFishy() = default; + + void start(); + void stop(); + + private: + [[nodiscard]] auto convertToPwmFrom(float angle) const -> float; + + void run(); + + void stopMotors(); + void spinRight(float left_speed, float right_speed); + void spinLeft(float left_speed, float right_speed); + void moveForward(float speed); + void moveBackward(float speed); + + interface::EventLoop &_event_loop; + interface::Timeout &_timeout; + + interface::Battery &_battery; + interface::Motor &_motor_left; + interface::Motor &_motor_right; + + const float kMinAngleInput = 0.F; + const float kMaxAngleInput = 30.F; + const float kMinPwmOutput = 0.15F; // Min to move robot + const float kMaxPwmOutput = 0.70F; + + bool move_left = true; + + bool should_stop = true; +}; + +} // namespace leka diff --git a/spikes/lk_auto_charge/main.cpp b/spikes/lk_auto_charge/main.cpp index 0ebad93e77..b291afa727 100644 --- a/spikes/lk_auto_charge/main.cpp +++ b/spikes/lk_auto_charge/main.cpp @@ -17,6 +17,7 @@ #include "CorePwm.h" #include "CoreTimeout.h" #include "EventLoopKit.h" +#include "HappyFishy.h" #include "HappyToupie.h" #include "HelloWorld.h" #include "IMUKit.hpp" @@ -102,6 +103,7 @@ 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 last_strategy = uint8_t {0x00}; @@ -113,9 +115,13 @@ void runStrategy(uint8_t 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(); } } From 48d22dc43e16a3b889fcba5e9a00ec4420760292 Mon Sep 17 00:00:00 2001 From: Yann Locatelli Date: Thu, 14 Dec 2023 17:54:32 +0100 Subject: [PATCH 08/12] Apply suggestion from review --- spikes/lk_auto_charge/SealStrategy.cpp | 46 ++++++++++++++++---------- spikes/lk_auto_charge/SealStrategy.h | 3 ++ 2 files changed, 31 insertions(+), 18 deletions(-) diff --git a/spikes/lk_auto_charge/SealStrategy.cpp b/spikes/lk_auto_charge/SealStrategy.cpp index c766371440..908add48a3 100644 --- a/spikes/lk_auto_charge/SealStrategy.cpp +++ b/spikes/lk_auto_charge/SealStrategy.cpp @@ -53,26 +53,10 @@ void SealStrategy::run() if (abs_float(angles.roll) > kRollTolerance) { auto speed_offset = convertToPwmFrom(angles.roll > 0 ? angles.roll : -angles.roll); - if (is_right_tilted) { - if (should_move_forward) { - spinRight(kMinPwmOutput, kMinPwmOutput + speed_offset); - } else { - spinLeft(kMinPwmOutput, kMinPwmOutput + speed_offset); - } - } else { - if (should_move_forward) { - spinLeft(kMinPwmOutput + speed_offset, kMinPwmOutput); - } else { - spinRight(kMinPwmOutput + speed_offset, kMinPwmOutput); - } - } + spinToFixRoll(is_right_tilted, should_move_forward, speed_offset); } else if (abs_float(angles.pitch) > kPitchTolerance) { auto speed = convertToPwmFrom(angles.pitch > 0 ? angles.pitch : -angles.pitch); - if (should_move_forward) { - moveForward(speed); - } else { - moveBackward(speed); - } + moveToFixPitch(should_move_forward, speed); } else { stopMotors(); } @@ -110,3 +94,29 @@ void SealStrategy::moveBackward(float speed) _motor_left.spin(Rotation::clockwise, speed); _motor_right.spin(Rotation::counterClockwise, speed); } + +void SealStrategy::spinToFixRoll(bool is_right_tilted, bool should_move_forward, float speed_offset) +{ + if (is_right_tilted) { + if (should_move_forward) { + spinRight(kMinPwmOutput, kMinPwmOutput + speed_offset); + } else { + spinLeft(kMinPwmOutput, kMinPwmOutput + speed_offset); + } + } else { + if (should_move_forward) { + spinLeft(kMinPwmOutput + speed_offset, kMinPwmOutput); + } else { + spinRight(kMinPwmOutput + speed_offset, kMinPwmOutput); + } + } +} + +void SealStrategy::moveToFixPitch(bool should_move_forward, float speed) +{ + if (should_move_forward) { + moveForward(speed); + } else { + moveBackward(speed); + } +} diff --git a/spikes/lk_auto_charge/SealStrategy.h b/spikes/lk_auto_charge/SealStrategy.h index 469facdaaf..72b16d07b9 100644 --- a/spikes/lk_auto_charge/SealStrategy.h +++ b/spikes/lk_auto_charge/SealStrategy.h @@ -42,6 +42,9 @@ class SealStrategy void moveForward(float speed); void moveBackward(float speed); + void spinToFixRoll(bool is_right_tilted, bool should_move_forward, float speed_offset); + void moveToFixPitch(bool should_move_forward, float speed); + interface::EventLoop &_event_loop; interface::Timeout &_timeout; From b632b7c6870b5b5986d68347172acf6c3aba689a Mon Sep 17 00:00:00 2001 From: Yann Locatelli Date: Fri, 15 Dec 2023 14:43:39 +0100 Subject: [PATCH 09/12] :sparkles: (Behavior): Add AutochargeSeal --- libs/BehaviorKit/CMakeLists.txt | 2 + .../include/behaviors/AutochargeSeal.h | 56 +++++++ .../BehaviorKit/include/internal/BehaviorID.h | 2 + .../source/behaviors/AutochargeSeal.cpp | 111 ++++++++++++++ .../tests/BehaviorAutochargeSeal_test.cpp | 142 ++++++++++++++++++ 5 files changed, 313 insertions(+) create mode 100644 libs/BehaviorKit/include/behaviors/AutochargeSeal.h create mode 100644 libs/BehaviorKit/source/behaviors/AutochargeSeal.cpp create mode 100644 libs/BehaviorKit/tests/BehaviorAutochargeSeal_test.cpp diff --git a/libs/BehaviorKit/CMakeLists.txt b/libs/BehaviorKit/CMakeLists.txt index 95d922c947..16966c9f68 100644 --- a/libs/BehaviorKit/CMakeLists.txt +++ b/libs/BehaviorKit/CMakeLists.txt @@ -12,6 +12,7 @@ target_include_directories(BehaviorKit target_sources(BehaviorKit PRIVATE source/BehaviorKit.cpp + source/behaviors/AutochargeSeal.cpp ) target_link_libraries(BehaviorKit @@ -22,5 +23,6 @@ if(${CMAKE_PROJECT_NAME} STREQUAL "LekaOSUnitTests") leka_unit_tests_sources( tests/BehaviorID_test.cpp tests/BehaviorKit_test.cpp + tests/BehaviorAutochargeSeal_test.cpp ) endif() diff --git a/libs/BehaviorKit/include/behaviors/AutochargeSeal.h b/libs/BehaviorKit/include/behaviors/AutochargeSeal.h new file mode 100644 index 0000000000..e74740637c --- /dev/null +++ b/libs/BehaviorKit/include/behaviors/AutochargeSeal.h @@ -0,0 +1,56 @@ +// Leka - LekaOS +// Copyright 2023 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "interface/Behavior.h" +#include "interface/drivers/Motor.h" +#include "interface/libs/IMUKit.hpp" + +namespace leka::behavior { + +class AutochargeSeal : public interface::Behavior +{ + public: + explicit AutochargeSeal(interface::Motor &motor_left, interface::Motor &motor_right, interface::IMUKit &imu_kit) + : _motor_left(motor_left), _motor_right(motor_right), _imukit(imu_kit) + { + // nothing to do + } + + auto id() -> BehaviorID final; + + void run() final; + void stop() final; + + private: + [[nodiscard]] auto convertToPwmFrom(float angle) const -> float; + + void stopMotors(); + void spinRight(float left_speed, float right_speed); + void spinLeft(float left_speed, float right_speed); + void moveForward(float speed); + void moveBackward(float speed); + + void spinToFixRoll(bool is_right_tilted, bool should_move_forward, float speed_offset); + void moveToFixPitch(bool should_move_forward, float speed); + + const float kMinAngleInput = 0.F; + const float kMaxAngleInput = 30.F; + const float kMinPwmOutput = 0.15F; // Min to move robot + const float kMaxPwmOutput = 0.70F; + + const float kRollTolerance = 3.F; // in degrees + const float kPitchTolerance = 3.F; // in degrees + + interface::Motor &_motor_left; + interface::Motor &_motor_right; + interface::IMUKit &_imukit; + + BehaviorID _id {behavior_id::autocharge_seal}; + + bool must_stop {true}; +}; + +} // namespace leka::behavior diff --git a/libs/BehaviorKit/include/internal/BehaviorID.h b/libs/BehaviorKit/include/internal/BehaviorID.h index b54b9a4d01..fea1f42837 100644 --- a/libs/BehaviorKit/include/internal/BehaviorID.h +++ b/libs/BehaviorKit/include/internal/BehaviorID.h @@ -12,4 +12,6 @@ namespace leka::behavior_id { inline constexpr BehaviorID none = 0x0000; +inline constexpr BehaviorID autocharge_seal = 0x0100; + } // namespace leka::behavior_id diff --git a/libs/BehaviorKit/source/behaviors/AutochargeSeal.cpp b/libs/BehaviorKit/source/behaviors/AutochargeSeal.cpp new file mode 100644 index 0000000000..a3a398f39a --- /dev/null +++ b/libs/BehaviorKit/source/behaviors/AutochargeSeal.cpp @@ -0,0 +1,111 @@ +// Leka - LekaOS +// Copyright 2023 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#include "behaviors/AutochargeSeal.h" + +#include "rtos/ThisThread.h" + +#include "MathUtils.h" + +using namespace leka::behavior; +using namespace std::chrono; + +auto AutochargeSeal::id() -> BehaviorID +{ + return _id; +} + +auto AutochargeSeal::convertToPwmFrom(float angle) const -> float +{ + auto res = utils::math::map(angle, kMinAngleInput, kMaxAngleInput, kMinPwmOutput, kMaxPwmOutput); + return res; +} + +void AutochargeSeal::run() +{ + must_stop = true; // TODO: Update for UT + + do { + auto angles = _imukit.getEulerAngles(); + auto is_right_tilted = angles.roll > 0; + auto should_move_forward = angles.pitch > 0; + + auto abs_float = [](float value) { return value > 0 ? value : -value; }; + + if (abs_float(angles.roll) > kRollTolerance) { + auto speed_offset = convertToPwmFrom(angles.roll > 0 ? angles.roll : -angles.roll); + spinToFixRoll(is_right_tilted, should_move_forward, speed_offset); + } else if (abs_float(angles.pitch) > kPitchTolerance) { + auto speed = convertToPwmFrom(angles.pitch > 0 ? angles.pitch : -angles.pitch); + moveToFixPitch(should_move_forward, speed); + } else { + stopMotors(); + } + + rtos::ThisThread::sleep_for(10ms); + } while (!must_stop); +} + +void AutochargeSeal::stop() +{ + must_stop = true; + rtos::ThisThread::sleep_for(100ms); + stopMotors(); +} + +void AutochargeSeal::stopMotors() +{ + _motor_right.stop(); + _motor_left.stop(); +} + +void AutochargeSeal::spinLeft(float left_speed, float right_speed) +{ + _motor_left.spin(Rotation::clockwise, left_speed); + _motor_right.spin(Rotation::clockwise, right_speed); +} + +void AutochargeSeal::spinRight(float left_speed, float right_speed) +{ + _motor_left.spin(Rotation::counterClockwise, left_speed); + _motor_right.spin(Rotation::counterClockwise, right_speed); +} + +void AutochargeSeal::moveForward(float speed) +{ + _motor_left.spin(Rotation::counterClockwise, speed); + _motor_right.spin(Rotation::clockwise, speed); +} + +void AutochargeSeal::moveBackward(float speed) +{ + _motor_left.spin(Rotation::clockwise, speed); + _motor_right.spin(Rotation::counterClockwise, speed); +} + +void AutochargeSeal::spinToFixRoll(bool is_right_tilted, bool should_move_forward, float speed_offset) +{ + if (is_right_tilted) { + if (should_move_forward) { + spinRight(kMinPwmOutput, kMinPwmOutput + speed_offset); + } else { + spinLeft(kMinPwmOutput, kMinPwmOutput + speed_offset); + } + } else { + if (should_move_forward) { + spinLeft(kMinPwmOutput + speed_offset, kMinPwmOutput); + } else { + spinRight(kMinPwmOutput + speed_offset, kMinPwmOutput); + } + } +} + +void AutochargeSeal::moveToFixPitch(bool should_move_forward, float speed) +{ + if (should_move_forward) { + moveForward(speed); + } else { + moveBackward(speed); + } +} diff --git a/libs/BehaviorKit/tests/BehaviorAutochargeSeal_test.cpp b/libs/BehaviorKit/tests/BehaviorAutochargeSeal_test.cpp new file mode 100644 index 0000000000..d8393dd2a8 --- /dev/null +++ b/libs/BehaviorKit/tests/BehaviorAutochargeSeal_test.cpp @@ -0,0 +1,142 @@ +// Leka - LekaOS +// Copyright 2023 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#include "behaviors/AutochargeSeal.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" +#include "mocks/leka/CoreMotor.h" +#include "mocks/leka/IMUKit.hpp" + +using namespace leka; + +using ::testing::_; +using ::testing::Return; + +class AutochargeSealTest : public ::testing::Test +{ + protected: + AutochargeSealTest() = default; + + // void SetUp() override {} + // void TearDown() override {} + + mock::CoreMotor mock_motor_left {}; + mock::CoreMotor mock_motor_right {}; + mock::IMUKit mock_imukit {}; + + behavior::AutochargeSeal autocharge_seal {mock_motor_left, mock_motor_right, mock_imukit}; + + void expectedCallsSpinLeft() + { + EXPECT_CALL(mock_motor_left, spin(Rotation::clockwise, _)); + EXPECT_CALL(mock_motor_right, spin(Rotation::clockwise, _)); + } + + void expectedCallsSpinRight() + { + EXPECT_CALL(mock_motor_left, spin(Rotation::counterClockwise, _)); + EXPECT_CALL(mock_motor_right, spin(Rotation::counterClockwise, _)); + } + + void expectedCallsMoveForward() + { + EXPECT_CALL(mock_motor_left, spin(Rotation::counterClockwise, _)); + EXPECT_CALL(mock_motor_right, spin(Rotation::clockwise, _)); + } + + void expectedCallsMoveBackward() + { + EXPECT_CALL(mock_motor_left, spin(Rotation::clockwise, _)); + EXPECT_CALL(mock_motor_right, spin(Rotation::counterClockwise, _)); + } +}; + +TEST_F(AutochargeSealTest, initialization) +{ + ASSERT_NE(&autocharge_seal, nullptr); +} + +TEST_F(AutochargeSealTest, id) +{ + ASSERT_EQ(autocharge_seal.id(), behavior_id::autocharge_seal); +} + +TEST_F(AutochargeSealTest, runNothingToFix) +{ + auto angles = EulerAngles {0, 0, 0}; + + EXPECT_CALL(mock_imukit, getEulerAngles).WillOnce(Return(angles)); + EXPECT_CALL(mock_motor_left, stop); + EXPECT_CALL(mock_motor_right, stop); + + autocharge_seal.run(); +} + +TEST_F(AutochargeSealTest, runFixPositivePitch) +{ + auto angles = EulerAngles {10, 0, 0}; + + EXPECT_CALL(mock_imukit, getEulerAngles).WillOnce(Return(angles)); + expectedCallsMoveForward(); + + autocharge_seal.run(); +} + +TEST_F(AutochargeSealTest, runFixNegativePitch) +{ + auto angles = EulerAngles {-10, 0, 0}; + + EXPECT_CALL(mock_imukit, getEulerAngles).WillOnce(Return(angles)); + expectedCallsMoveBackward(); + + autocharge_seal.run(); +} + +TEST_F(AutochargeSealTest, runFixPositiveRollPositivePitch) +{ + auto angles = EulerAngles {10, 10, 0}; + + EXPECT_CALL(mock_imukit, getEulerAngles).WillOnce(Return(angles)); + expectedCallsSpinRight(); + + autocharge_seal.run(); +} + +TEST_F(AutochargeSealTest, runFixPositiveRollNegativePitch) +{ + auto angles = EulerAngles {-10, 10, 0}; + + EXPECT_CALL(mock_imukit, getEulerAngles).WillOnce(Return(angles)); + expectedCallsSpinLeft(); + + autocharge_seal.run(); +} + +TEST_F(AutochargeSealTest, runFixNegativeRollPositivePitch) +{ + auto angles = EulerAngles {10, -10, 0}; + + EXPECT_CALL(mock_imukit, getEulerAngles).WillOnce(Return(angles)); + expectedCallsSpinLeft(); + + autocharge_seal.run(); +} + +TEST_F(AutochargeSealTest, runFixNegativeRollNegativePitch) +{ + auto angles = EulerAngles {-10, -10, 0}; + + EXPECT_CALL(mock_imukit, getEulerAngles).WillOnce(Return(angles)); + expectedCallsSpinRight(); + + autocharge_seal.run(); +} + +TEST_F(AutochargeSealTest, stop) +{ + EXPECT_CALL(mock_motor_left, stop); + EXPECT_CALL(mock_motor_right, stop); + + autocharge_seal.stop(); +} From 558f6da123a6d10446092e3ebd5a93343b7146ac Mon Sep 17 00:00:00 2001 From: Yann Locatelli Date: Tue, 19 Dec 2023 11:02:48 +0100 Subject: [PATCH 10/12] :recycle: (Autocharge): Update spike using BehaviorKit --- spikes/lk_auto_charge/CMakeLists.txt | 1 + spikes/lk_auto_charge/main.cpp | 39 ++++++++++------------------ 2 files changed, 15 insertions(+), 25 deletions(-) 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) { From 2146a834754045b818e1adf6901357c643d20445 Mon Sep 17 00:00:00 2001 From: Yann Locatelli Date: Wed, 20 Dec 2023 13:30:40 +0100 Subject: [PATCH 11/12] Apply suggestion from oral review Move algorithm into another function --- .../include/behaviors/AutochargeSeal.h | 1 + .../source/behaviors/AutochargeSeal.cpp | 39 +++++++++++-------- .../tests/BehaviorAutochargeSeal_test.cpp | 28 ++++++------- 3 files changed, 37 insertions(+), 31 deletions(-) diff --git a/libs/BehaviorKit/include/behaviors/AutochargeSeal.h b/libs/BehaviorKit/include/behaviors/AutochargeSeal.h index e74740637c..aa07b99a87 100644 --- a/libs/BehaviorKit/include/behaviors/AutochargeSeal.h +++ b/libs/BehaviorKit/include/behaviors/AutochargeSeal.h @@ -22,6 +22,7 @@ class AutochargeSeal : public interface::Behavior auto id() -> BehaviorID final; void run() final; + void loop(); void stop() final; private: diff --git a/libs/BehaviorKit/source/behaviors/AutochargeSeal.cpp b/libs/BehaviorKit/source/behaviors/AutochargeSeal.cpp index a3a398f39a..91f658074a 100644 --- a/libs/BehaviorKit/source/behaviors/AutochargeSeal.cpp +++ b/libs/BehaviorKit/source/behaviors/AutochargeSeal.cpp @@ -27,26 +27,31 @@ void AutochargeSeal::run() must_stop = true; // TODO: Update for UT do { - auto angles = _imukit.getEulerAngles(); - auto is_right_tilted = angles.roll > 0; - auto should_move_forward = angles.pitch > 0; - - auto abs_float = [](float value) { return value > 0 ? value : -value; }; - - if (abs_float(angles.roll) > kRollTolerance) { - auto speed_offset = convertToPwmFrom(angles.roll > 0 ? angles.roll : -angles.roll); - spinToFixRoll(is_right_tilted, should_move_forward, speed_offset); - } else if (abs_float(angles.pitch) > kPitchTolerance) { - auto speed = convertToPwmFrom(angles.pitch > 0 ? angles.pitch : -angles.pitch); - moveToFixPitch(should_move_forward, speed); - } else { - stopMotors(); - } - - rtos::ThisThread::sleep_for(10ms); + loop(); } while (!must_stop); } +void AutochargeSeal::loop() +{ + auto angles = _imukit.getEulerAngles(); + auto is_right_tilted = angles.roll > 0; + auto should_move_forward = angles.pitch > 0; + + auto abs_float = [](float value) { return value > 0 ? value : -value; }; + + if (abs_float(angles.roll) > kRollTolerance) { + auto speed_offset = convertToPwmFrom(angles.roll > 0 ? angles.roll : -angles.roll); + spinToFixRoll(is_right_tilted, should_move_forward, speed_offset); + } else if (abs_float(angles.pitch) > kPitchTolerance) { + auto speed = convertToPwmFrom(angles.pitch > 0 ? angles.pitch : -angles.pitch); + moveToFixPitch(should_move_forward, speed); + } else { + stopMotors(); + } + + rtos::ThisThread::sleep_for(10ms); +} + void AutochargeSeal::stop() { must_stop = true; diff --git a/libs/BehaviorKit/tests/BehaviorAutochargeSeal_test.cpp b/libs/BehaviorKit/tests/BehaviorAutochargeSeal_test.cpp index d8393dd2a8..7191057550 100644 --- a/libs/BehaviorKit/tests/BehaviorAutochargeSeal_test.cpp +++ b/libs/BehaviorKit/tests/BehaviorAutochargeSeal_test.cpp @@ -62,7 +62,7 @@ TEST_F(AutochargeSealTest, id) ASSERT_EQ(autocharge_seal.id(), behavior_id::autocharge_seal); } -TEST_F(AutochargeSealTest, runNothingToFix) +TEST_F(AutochargeSealTest, loopNothingToFix) { auto angles = EulerAngles {0, 0, 0}; @@ -70,67 +70,67 @@ TEST_F(AutochargeSealTest, runNothingToFix) EXPECT_CALL(mock_motor_left, stop); EXPECT_CALL(mock_motor_right, stop); - autocharge_seal.run(); + autocharge_seal.loop(); } -TEST_F(AutochargeSealTest, runFixPositivePitch) +TEST_F(AutochargeSealTest, loopFixPositivePitch) { auto angles = EulerAngles {10, 0, 0}; EXPECT_CALL(mock_imukit, getEulerAngles).WillOnce(Return(angles)); expectedCallsMoveForward(); - autocharge_seal.run(); + autocharge_seal.loop(); } -TEST_F(AutochargeSealTest, runFixNegativePitch) +TEST_F(AutochargeSealTest, loopFixNegativePitch) { auto angles = EulerAngles {-10, 0, 0}; EXPECT_CALL(mock_imukit, getEulerAngles).WillOnce(Return(angles)); expectedCallsMoveBackward(); - autocharge_seal.run(); + autocharge_seal.loop(); } -TEST_F(AutochargeSealTest, runFixPositiveRollPositivePitch) +TEST_F(AutochargeSealTest, loopFixPositiveRollPositivePitch) { auto angles = EulerAngles {10, 10, 0}; EXPECT_CALL(mock_imukit, getEulerAngles).WillOnce(Return(angles)); expectedCallsSpinRight(); - autocharge_seal.run(); + autocharge_seal.loop(); } -TEST_F(AutochargeSealTest, runFixPositiveRollNegativePitch) +TEST_F(AutochargeSealTest, loopFixPositiveRollNegativePitch) { auto angles = EulerAngles {-10, 10, 0}; EXPECT_CALL(mock_imukit, getEulerAngles).WillOnce(Return(angles)); expectedCallsSpinLeft(); - autocharge_seal.run(); + autocharge_seal.loop(); } -TEST_F(AutochargeSealTest, runFixNegativeRollPositivePitch) +TEST_F(AutochargeSealTest, loopFixNegativeRollPositivePitch) { auto angles = EulerAngles {10, -10, 0}; EXPECT_CALL(mock_imukit, getEulerAngles).WillOnce(Return(angles)); expectedCallsSpinLeft(); - autocharge_seal.run(); + autocharge_seal.loop(); } -TEST_F(AutochargeSealTest, runFixNegativeRollNegativePitch) +TEST_F(AutochargeSealTest, loopFixNegativeRollNegativePitch) { auto angles = EulerAngles {-10, -10, 0}; EXPECT_CALL(mock_imukit, getEulerAngles).WillOnce(Return(angles)); expectedCallsSpinRight(); - autocharge_seal.run(); + autocharge_seal.loop(); } TEST_F(AutochargeSealTest, stop) From a38139262f969a883e5ead8b0e4c03333099e423 Mon Sep 17 00:00:00 2001 From: Yann Locatelli Date: Wed, 20 Dec 2023 13:32:01 +0100 Subject: [PATCH 12/12] Finalize last TODO with must_stop --- libs/BehaviorKit/source/behaviors/AutochargeSeal.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/libs/BehaviorKit/source/behaviors/AutochargeSeal.cpp b/libs/BehaviorKit/source/behaviors/AutochargeSeal.cpp index 91f658074a..2d79ab0eaa 100644 --- a/libs/BehaviorKit/source/behaviors/AutochargeSeal.cpp +++ b/libs/BehaviorKit/source/behaviors/AutochargeSeal.cpp @@ -22,14 +22,15 @@ auto AutochargeSeal::convertToPwmFrom(float angle) const -> float return res; } +// LCOV_EXCL_START void AutochargeSeal::run() { - must_stop = true; // TODO: Update for UT - - do { + must_stop = false; + while (!must_stop) { loop(); - } while (!must_stop); + } } +// LCOV_EXCL_STOP void AutochargeSeal::loop() {