diff --git a/libs/CMakeLists.txt b/libs/CMakeLists.txt index f3cc0bc40d..b65576532d 100644 --- a/libs/CMakeLists.txt +++ b/libs/CMakeLists.txt @@ -16,6 +16,7 @@ add_subdirectory(${LIBS_DIR}/FirmwareKit) add_subdirectory(${LIBS_DIR}/IMUKit) add_subdirectory(${LIBS_DIR}/IOKit) add_subdirectory(${LIBS_DIR}/LedKit) +add_subdirectory(${LIBS_DIR}/MotionKit) add_subdirectory(${LIBS_DIR}/ReinforcerKit) add_subdirectory(${LIBS_DIR}/RobotKit) add_subdirectory(${LIBS_DIR}/RFIDKit) diff --git a/libs/MotionKit/CMakeLists.txt b/libs/MotionKit/CMakeLists.txt new file mode 100644 index 0000000000..c63fc9e0db --- /dev/null +++ b/libs/MotionKit/CMakeLists.txt @@ -0,0 +1,24 @@ +# Leka - LekaOS +# Copyright 2022 APF France handicap +# SPDX-License-Identifier: Apache-2.0 + +add_library(MotionKit STATIC) + +target_include_directories(MotionKit + PUBLIC + include +) + +target_sources(MotionKit + PRIVATE + source/PID.cpp +) + +target_link_libraries(MotionKit +) + +if(${CMAKE_PROJECT_NAME} STREQUAL "LekaOSUnitTests") + leka_unit_tests_sources( + tests/PID_test.cpp + ) +endif() diff --git a/libs/MotionKit/include/PID.h b/libs/MotionKit/include/PID.h new file mode 100644 index 0000000000..3a6319223e --- /dev/null +++ b/libs/MotionKit/include/PID.h @@ -0,0 +1,44 @@ +// Leka - LekaOS +// Copyright 2022 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +#include "interface/drivers/Motor.h" + +namespace leka { + +class PID +{ + public: + PID() = default; + + auto processPID([[maybe_unused]] float pitch, [[maybe_unused]] float roll, float yaw) + -> std::tuple; + + private: + // ? Kp, Ki, Kd were found empirically by increasing Kp until the rotation angle exceeds the target angle + // ? Then increase Kd to fix this excess angle + // ? Repeat this protocol until there is no Kd high enough to compensate Kp + // ? Then take the last set of Kp, Kd value with no excess angle + // ? Finally choose a low Ki that smooth out the movement + struct Parameters { + static constexpr auto Kp = float {0.3F}; + static constexpr auto Ki = float {0.0001F}; + static constexpr auto Kd = float {0.4F}; + }; + const float kStaticBound = 5.F; + const float kDeltaT = 70.F; + const float kTargetAngle = 180.F; + + float _error_position_total = 0.F; + float _error_position_current = 0.F; + float _error_position_last = 0.F; + float _proportional = 0.F; + float _integral = 0.F; + float _derivative = 0.F; +}; + +} // namespace leka diff --git a/libs/MotionKit/source/PID.cpp b/libs/MotionKit/source/PID.cpp new file mode 100644 index 0000000000..276d1dc904 --- /dev/null +++ b/libs/MotionKit/source/PID.cpp @@ -0,0 +1,42 @@ +// Leka - LekaOS +// Copyright 2022 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#include "PID.h" +#include + +using namespace leka; + +auto PID::processPID([[maybe_unused]] float pitch, [[maybe_unused]] float roll, float yaw) + -> std::tuple +{ + Rotation direction {}; + _error_position_current = kTargetAngle - yaw; + + if (std::abs(_error_position_current) < kStaticBound) { + _error_position_total += _error_position_current; + _error_position_total = std::min(_error_position_total, 50.F / Parameters::Ki); + } else { + _error_position_total = 0.F; + } + if (std::abs(_error_position_current) < kStaticBound) { + _derivative = 0.F; + } + + _proportional = _error_position_current * Parameters::Kp; + _integral = _error_position_total * Parameters::Ki; + _derivative = (_error_position_current - _error_position_last) * Parameters::Kd; + + _error_position_last = _error_position_current; + + auto speed = (_proportional + _integral + _derivative) / kDeltaT; + + if (speed < 0) { + speed = -speed; + direction = Rotation::counterClockwise; + } else { + direction = Rotation::clockwise; + } + + return {speed, direction}; +} diff --git a/libs/MotionKit/tests/PID_test.cpp b/libs/MotionKit/tests/PID_test.cpp new file mode 100644 index 0000000000..3ed4f4cf60 --- /dev/null +++ b/libs/MotionKit/tests/PID_test.cpp @@ -0,0 +1,87 @@ +// Leka - LekaOS +// Copyright 2022 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#include "PID.h" + +#include "gtest/gtest.h" + +using namespace leka; + +class PIDTest : public ::testing::Test +{ + protected: + PIDTest() = default; + + // void SetUp() override { } + // void TearDown() override {} + + PID pid {}; + + float max_speed_value = 1.8F; //? ((360-180)*Kp + (360-180)*Kd)/KDeltaT +}; + +TEST_F(PIDTest, initialization) +{ + ASSERT_NE(&pid, nullptr); +} + +TEST_F(PIDTest, processPIDDefaultPosition) +{ + auto pitch = 0.F; + auto roll = 0.F; + auto yaw = 180.F; + + auto [speed, direction] = pid.processPID(pitch, roll, yaw); + + EXPECT_EQ(speed, 0.F); + EXPECT_EQ(direction, Rotation::clockwise); +} + +TEST_F(PIDTest, processPIDRolledOverAHalfRight) +{ + auto pitch = 0.F; + auto roll = 0.F; + auto yaw = 0.F; + + auto [speed, direction] = pid.processPID(pitch, roll, yaw); + + EXPECT_EQ(speed, max_speed_value); + EXPECT_EQ(direction, Rotation::clockwise); +} + +TEST_F(PIDTest, processPIDRolledOverAQuarterRight) +{ + auto pitch = 0.F; + auto roll = 0.F; + auto yaw = 90.F; + + auto [speed, direction] = pid.processPID(pitch, roll, yaw); + + EXPECT_EQ(speed, 0.9F); + EXPECT_EQ(direction, Rotation::clockwise); +} + +TEST_F(PIDTest, processPIDRolledOverAQuarterLeft) +{ + auto pitch = 0.F; + auto roll = 0.F; + auto yaw = 270.F; + + auto [speed, direction] = pid.processPID(pitch, roll, yaw); + + EXPECT_EQ(speed, 0.9F); + EXPECT_EQ(direction, Rotation::counterClockwise); +} + +TEST_F(PIDTest, processPIDRolledOverAHalfLeft) +{ + auto pitch = 0.F; + auto roll = 0.F; + auto yaw = 360.F; + + auto [speed, direction] = pid.processPID(pitch, roll, yaw); + + EXPECT_EQ(speed, max_speed_value); + EXPECT_EQ(direction, Rotation::counterClockwise); +} diff --git a/tests/unit/CMakeLists.txt b/tests/unit/CMakeLists.txt index 246546752a..7edf9384bc 100644 --- a/tests/unit/CMakeLists.txt +++ b/tests/unit/CMakeLists.txt @@ -288,6 +288,7 @@ leka_register_unit_tests_for_library(WebKit) leka_register_unit_tests_for_library(IMUKit) leka_register_unit_tests_for_library(IOKit) leka_register_unit_tests_for_library(LedKit) +leka_register_unit_tests_for_library(MotionKit) leka_register_unit_tests_for_library(ReinforcerKit) leka_register_unit_tests_for_library(RobotKit) leka_register_unit_tests_for_library(RFIDKit)