Yann/feature/behaviorkit/add behaviors seal

app/os/CMakeLists.txt

@@ -25,7 +25,7 @@ target_link_libraries(LekaOS
 	FileManagerKit
 	SerialNumberKit
 	FirmwareKit
-	BehaviorKit
+	BehaviorKitDeprecated
 	CorePwm
 	CoreMotor
 	VideoKit

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 {

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 <span>
+
+#include "interface/Behavior.h"
+#include "internal/BehaviorID.h"
+
+namespace leka::interface {
+
+class BehaviorKit
+{
+  public:
+	virtual ~BehaviorKit() = default;
+
+	virtual void registerBehaviors(std::span<interface::Behavior *> behaviors) = 0;
+	virtual void start(interface::Behavior *behavior)						   = 0;
+	virtual void start(BehaviorID id)										   = 0;
+	virtual void stop()														   = 0;
+};
+
+}	// namespace leka::interface

libs/BehaviorKit/CMakeLists.txt

@@ -1,5 +1,5 @@
 # Leka - LekaOS
-# Copyright 2022 APF France handicap
+# Copyright 2023 APF France handicap
 # SPDX-License-Identifier: Apache-2.0
 
 add_library(BehaviorKit STATIC)
@@ -12,16 +12,17 @@ target_include_directories(BehaviorKit
 target_sources(BehaviorKit
 	PRIVATE
 	source/BehaviorKit.cpp
+	source/behaviors/AutochargeSeal.cpp
 )
 
 target_link_libraries(BehaviorKit
-	LedKit
-	VideoKit
-	CoreMotor
+	EventLoopKit
 )
 
 if(${CMAKE_PROJECT_NAME} STREQUAL "LekaOSUnitTests")
 	leka_unit_tests_sources(
+		tests/BehaviorID_test.cpp
 		tests/BehaviorKit_test.cpp
+		tests/BehaviorAutochargeSeal_test.cpp
 	)
 endif()

libs/BehaviorKit/include/BehaviorKit.h

@@ -1,55 +1,31 @@
 // Leka - LekaOS
-// Copyright 2022 APF France handicap
+// Copyright 2023 APF France handicap
 // SPDX-License-Identifier: Apache-2.0
 
 #pragma once
 
-#include "interface/drivers/Motor.h"
-#include "interface/libs/LedKit.h"
-#include "interface/libs/VideoKit.h"
+#include <span>
+
+#include "interface/Behavior.h"
+#include "interface/libs/BehaviorKit.h"
+#include "internal/BehaviorID.h"
 
 namespace leka {
 
-class BehaviorKit
+class BehaviorKit : public interface::BehaviorKit
 {
   public:
-	explicit BehaviorKit(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
-	}
-
-	void spinLeft(float speed);
-	void spinRight(float speed);
-
-	void launching();
-	void sleeping();
-	void waiting();
-
-	void blinkOnCharge();
-
-	void lowBattery();
-
-	void chargingEmpty();
-	void chargingLow();
-	void chargingMedium();
-	void chargingHigh();
-	void chargingFull();
-
-	void bleConnectionWithoutVideo();
-	void bleConnectionWithVideo();
-	void working();
+	explicit BehaviorKit() = default;
 
-	void fileExchange();
+	void registerBehaviors(std::span<interface::Behavior *> behaviors) final;
 
-	void stop();
+	void start(interface::Behavior *behavior) final;
+	void start(BehaviorID id) final;
+	void stop() final;
 
   private:
-	interface::VideoKit &_videokit;
-	interface::LedKit &_ledkit;
-	interface::Motor &_motor_left;
-	interface::Motor &_motor_right;
+	std::span<interface::Behavior *> _behaviors {};
+	interface::Behavior *_behavior = nullptr;
 };
 
 }	// namespace leka

libs/BehaviorKit/include/behaviors/AutochargeSeal.h

@@ -0,0 +1,57 @@
+// 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 loop();
+	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

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 <cstddef>
+#include <cstdint>
+
+#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

libs/BehaviorKit/include/internal/BehaviorID.h

@@ -0,0 +1,17 @@
+// Leka - LekaOS
+// Copyright 2023 APF France handicap
+// SPDX-License-Identifier: Apache-2.0
+
+#pragma once
+
+#include <cstdint>
+
+using BehaviorID = uint16_t;
+
+namespace leka::behavior_id {
+
+inline constexpr BehaviorID none = 0x0000;
+
+inline constexpr BehaviorID autocharge_seal = 0x0100;
+
+}	// namespace leka::behavior_id

libs/BehaviorKit/source/BehaviorKit.cpp

@@ -1,113 +1,52 @@
 // Leka - LekaOS
-// Copyright 2022 APF France handicap
+// Copyright 2023 APF France handicap
 // SPDX-License-Identifier: Apache-2.0
 
 #include "BehaviorKit.h"
-#include <fs_structure.hpp>
+#include <algorithm>
 
-#include "rtos/ThisThread.h"
+using namespace leka;
 
-#include "LedKitAnimations.h"
-
-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);
-}
-
-void BehaviorKit::sleeping()
-{
-	_ledkit.start(&led::animation::sleeping);
-	_videokit.playVideoOnce(fs::home::vid::system::robot_system_sleep_yawn_then_sleep_no_eyebrows);
-}
-
-void BehaviorKit::waiting()
-{
-	_ledkit.stop();
-	_videokit.playVideoOnRepeat(fs::home::vid::system::robot_system_idle_looking_top_right_left_no_eyebrows);
-}
-
-void BehaviorKit::blinkOnCharge()
-{
-	_ledkit.start(&led::animation::blink_on_charge);
-}
-
-void BehaviorKit::lowBattery()
+void BehaviorKit::registerBehaviors(std::span<interface::Behavior *> behaviors)
 {
-	_ledkit.stop();
-	_videokit.displayImage(fs::home::img::system::robot_battery_empty_must_be_charged);
-	_motor_left.stop();
-	_motor_right.stop();
+	_behaviors = behaviors;
 }
 
-void BehaviorKit::chargingEmpty()
+void BehaviorKit::start(interface::Behavior *behavior)
 {
-	_videokit.displayImage(fs::home::img::system::robot_battery_charging_empty_red);
-}
+	stop();
 
-void BehaviorKit::chargingLow()
-{
-	_videokit.displayImage(fs::home::img::system::robot_battery_charging_quarter_1_red);
-}
+	_behavior = nullptr;
+	for (auto *b: _behaviors) {
+		if (b == behavior) {
+			_behavior = b;
+		}
+	}
 
-void BehaviorKit::chargingMedium()
-{
-	_videokit.displayImage(fs::home::img::system::robot_battery_charging_quarter_2_orange);
-}
+	if (_behavior == nullptr) {
+		return;
+	}
 
-void BehaviorKit::chargingHigh()
-{
-	_videokit.displayImage(fs::home::img::system::robot_battery_charging_quarter_3_green);
+	_behavior->run();
 }
 
-void BehaviorKit::chargingFull()
+void BehaviorKit::start(BehaviorID id)
 {
-	_videokit.displayImage(fs::home::img::system::robot_battery_charging_quarter_4_green);
-}
+	interface::Behavior *found_behavior = nullptr;
 
-void BehaviorKit::bleConnectionWithoutVideo()
-{
-	_ledkit.start(&led::animation::ble_connection);
-}
+	for (auto *behavior: _behaviors) {
+		if (id == behavior->id()) {
+			found_behavior = behavior;
+			break;
+		}
+	}
 
-void BehaviorKit::bleConnectionWithVideo()
-{
-	_ledkit.start(&led::animation::ble_connection);
-	_videokit.playVideoOnce(fs::home::vid::system::robot_system_ble_connection_wink_no_eyebrows);
-}
-
-void BehaviorKit::working()
-{
-	_videokit.displayImage(fs::home::img::system::robot_face_smiling_slightly);
-}
-
-void BehaviorKit::fileExchange()
-{
-	// TODO(@ladislas): add file exchange image
-	_videokit.displayImage("/fs/home/img/system/robot-file_exchange.jpg");
+	start(found_behavior);
 }
 
 void BehaviorKit::stop()
 {
-	_ledkit.stop();
-	_videokit.stopVideo();
-	_motor_left.stop();
-	_motor_right.stop();
+	if (_behavior != nullptr) {
+		_behavior->stop();
+	}
 }
-
-}	// namespace leka