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