✨ (RotationControl): Replace PID by ControlRotation

libs/MotionKit/CMakeLists.txt

@@ -12,7 +12,7 @@ target_include_directories(MotionKit
 target_sources(MotionKit
 	PRIVATE
 	source/MotionKit.cpp
-	source/PID.cpp
+	source/RotationControl.cpp
 )
 
 target_link_libraries(MotionKit
@@ -23,6 +23,6 @@ target_link_libraries(MotionKit
 if(${CMAKE_PROJECT_NAME} STREQUAL "LekaOSUnitTests")
 	leka_unit_tests_sources(
 		tests/MotionKit_test.cpp
-		tests/PID_test.cpp
+		tests/RotationControl_test.cpp
 	)
 endif()

libs/MotionKit/include/RotationControl.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"
+namespace leka {
+
+class RotationControl
+{
+  public:
+	RotationControl() = default;
+
+	void setTarget(EulerAngles starting_angles, float number_of_rotations);
+	auto processRotationAngle(EulerAngles current_angles) -> float;
+
+  private:
+	void calculateTotalYawRotation(EulerAngles angle);
+	[[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;
+
+	float _angle_rotation_target = 0.F;
+	float _angle_rotation_sum	 = 0.F;
+
+	EulerAngles _euler_angles_previous {};
+
+	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/RotationControl.cpp

@@ -0,0 +1,63 @@
+// Leka - LekaOS
+// Copyright 2023 APF France handicap
+// SPDX-License-Identifier: Apache-2.0
+
+#include "RotationControl.hpp"
+#include <algorithm>
+
+#include "MathUtils.h"
+
+using namespace leka;
+
+void RotationControl::setTarget(EulerAngles starting_angle, float number_of_rotations)
+{
+	_euler_angles_previous = starting_angle;
+	_angle_rotation_target = number_of_rotations * 360.F;
+	_angle_rotation_sum	   = 0;
+}
+
+auto RotationControl::processRotationAngle(EulerAngles current_angles) -> float
+{
+	calculateTotalYawRotation(current_angles);
+
+	auto error_position_current = _angle_rotation_target - _angle_rotation_sum;
+
+	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;
+
+	return mapSpeed(speed);
+}
+
+void RotationControl::calculateTotalYawRotation(EulerAngles angle)
+{
+	if (auto abs_yaw_delta = std::abs(_euler_angles_previous.yaw - angle.yaw); abs_yaw_delta >= 300.F) {
+		_angle_rotation_sum += 360.F - abs_yaw_delta;
+	} else {
+		_angle_rotation_sum += abs_yaw_delta;
+	}
+	_euler_angles_previous = angle;
+}
+
+auto RotationControl::mapSpeed(float speed) const -> float
+{
+	auto bounded_speed = utils::math::map(speed, 0.F, kInputSpeedLimit, kMinimalViableRobotPwm, kPwmMaxValue);
+	if (bounded_speed < kMinimalViableRobotPwm + kEpsilon) {
+		return 0.0;
+	}
+	return bounded_speed;
+}