✨ (Behavior): Add AutochargeSeal

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()

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

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

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);
+	}
+}

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();
+}