✨ (PID): Add MotionKit folder and PID lib

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)

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

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 <tuple>
+
+#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<float, Rotation>;
+
+  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

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 <algorithm>
+
+using namespace leka;
+
+auto PID::processPID([[maybe_unused]] float pitch, [[maybe_unused]] float roll, float yaw)
+	-> std::tuple<float, Rotation>
+{
+	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};
+}

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

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)