✨ (StabilizationControl): Add a stabilization control

libs/MotionKit/CMakeLists.txt

@@ -13,6 +13,7 @@ target_sources(MotionKit
 	PRIVATE
 	source/MotionKit.cpp
 	source/RotationControl.cpp
+	source/StabilizationControl.cpp
 )
 
 target_link_libraries(MotionKit
@@ -24,5 +25,6 @@ if(${CMAKE_PROJECT_NAME} STREQUAL "LekaOSUnitTests")
 	leka_unit_tests_sources(
 		tests/MotionKit_test.cpp
 		tests/RotationControl_test.cpp
+		tests/StabilizationControl_test.cpp
 	)
 endif()

libs/MotionKit/include/StabilizationControl.hpp

@@ -0,0 +1,57 @@
+// Leka - LekaOS
+// Copyright 2023 APF France handicap
+// SPDX-License-Identifier: Apache-2.0
+
+#pragma once
+
+#include <interface/libs/IMUKit.hpp>
+#include <tuple>
+
+#include "interface/drivers/Motor.h"
+
+namespace leka {
+
+class StabilizationControl
+{
+  public:
+	StabilizationControl() = default;
+
+	void setTarget(EulerAngles starting_angle);
+	auto processStabilizationAngle(EulerAngles current_angles) -> std::tuple<float, Rotation>;
+
+  private:
+	[[nodiscard]] auto mapSpeed(float speed) const -> float;
+
+	// ? 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};
+	};
+
+	static constexpr float kStaticBound = 5.F;
+	static constexpr float kDeltaT		= 20.F;
+
+	// ? When the motor is stopped, PWM values under kMinimalViableRobotPwm are too low to generate enough torque for
+	// ? the motor to start spinning ? At the same time, kMinimalViableRobotPwm needs to be the lowest possible to avoid
+	// ? overshooting when the target is reached
+	static constexpr float kMinimalViableRobotPwm = 0.15F;
+	static constexpr float kPwmMaxValue			  = 1.F;
+	static constexpr float kEpsilon				  = 0.005F;
+
+	static constexpr float kInputSpeedLimit = 90 * (Parameters::Kp + Parameters::Kd) / kDeltaT;
+
+	EulerAngles _euler_angles_target {};
+
+	float _error_position_total = 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/StabilizationControl.cpp

@@ -0,0 +1,58 @@
+// Leka - LekaOS
+// Copyright 2023 APF France handicap
+// SPDX-License-Identifier: Apache-2.0
+
+#include "StabilizationControl.hpp"
+#include <algorithm>
+
+#include "MathUtils.h"
+
+using namespace leka;
+
+void StabilizationControl::setTarget(EulerAngles starting_angle)
+{
+	_euler_angles_target = starting_angle;
+}
+
+auto StabilizationControl::processStabilizationAngle(EulerAngles current_angles) -> std::tuple<float, Rotation>
+{
+	auto direction = Rotation {};
+
+	auto error_position_current = _euler_angles_target.yaw - current_angles.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 {mapSpeed(speed), direction};
+}
+
+auto StabilizationControl::mapSpeed(float speed) const -> float
+{
+	auto bounded_speed = utils::math::map(speed, 0.F, kInputSpeedLimit, kMinimalViableRobotPwm, kPwmMaxValue);
+	if (bounded_speed < kMinimalViableRobotPwm + kEpsilon) {
+		return 0.F;
+	}
+	return bounded_speed;
+}

libs/MotionKit/tests/StabilizationControl_test.cpp

@@ -0,0 +1,91 @@
+// Leka - LekaOS
+// Copyright 2023 APF France handicap
+// SPDX-License-Identifier: Apache-2.0
+
+#include "StabilizationControl.hpp"
+
+#include "gtest/gtest.h"
+
+using namespace leka;
+
+class StabilizationControlTest : public ::testing::Test
+{
+  protected:
+	StabilizationControlTest() = default;
+
+	// void SetUp() override {}
+	//  void TearDown() override {}
+
+	StabilizationControl stabilization_control {};
+
+	EulerAngles target {.pitch = 0.F, .roll = 0.F, .yaw = 0.F};
+	EulerAngles current {.yaw = 0.F};
+
+	float quarterturn_error_speed = 1.F;	 //? mapSpeed(90 * (Kp + Kd)/ KDeltaT)
+	float halfturn_error_speed	  = 1.85F;	 //? mapSpeed(180 * (Kp + Kd)/ KDeltaT)
+};
+
+TEST_F(StabilizationControlTest, initialization)
+{
+	ASSERT_NE(&stabilization_control, nullptr);
+}
+
+TEST_F(StabilizationControlTest, processStabilizationAngleDefaultPosition)
+{
+	stabilization_control.setTarget(current);
+
+	current = {.pitch = 0.F, .roll = 0.F, .yaw = 0.F};
+
+	auto [speed, direction] = stabilization_control.processStabilizationAngle(current);
+
+	EXPECT_EQ(speed, 0.F);
+	EXPECT_EQ(direction, Rotation::clockwise);
+}
+
+TEST_F(StabilizationControlTest, processStabilizationAngleRolledOverAHalfRight)
+{
+	stabilization_control.setTarget(current);
+
+	current = {.pitch = 0.F, .roll = 0.F, .yaw = 180.F};
+
+	auto [speed, direction] = stabilization_control.processStabilizationAngle(current);
+
+	EXPECT_EQ(speed, halfturn_error_speed);
+	EXPECT_EQ(direction, Rotation::counterClockwise);
+}
+
+TEST_F(StabilizationControlTest, processStabilizationAngleRolledOverAQuarterRight)
+{
+	stabilization_control.setTarget(current);
+
+	current = {.pitch = 0.F, .roll = 0.F, .yaw = 90.F};
+
+	auto [speed, direction] = stabilization_control.processStabilizationAngle(current);
+
+	EXPECT_EQ(speed, quarterturn_error_speed);
+	EXPECT_EQ(direction, Rotation::counterClockwise);
+}
+
+TEST_F(StabilizationControlTest, processStabilizationAngleRolledOverAQuarterLeft)
+{
+	stabilization_control.setTarget(current);
+
+	current = {.pitch = 0.F, .roll = 0.F, .yaw = -90.F};
+
+	auto [speed, direction] = stabilization_control.processStabilizationAngle(current);
+
+	EXPECT_EQ(speed, quarterturn_error_speed);
+	EXPECT_EQ(direction, Rotation::clockwise);
+}
+
+TEST_F(StabilizationControlTest, processStabilizationAngleRolledOverAHalfLeft)
+{
+	stabilization_control.setTarget(current);
+
+	current = {.pitch = 0.F, .roll = 0.F, .yaw = -180.F};
+
+	auto [speed, direction] = stabilization_control.processStabilizationAngle(current);
+
+	EXPECT_EQ(speed, halfturn_error_speed);
+	EXPECT_EQ(direction, Rotation::clockwise);
+}