hugo/feature/Add Stabilization to MotionKit

app/os/main.cpp

@@ -47,6 +47,7 @@
 #include "FlashNumberCounting.h"
 #include "FoodRecognition.h"
 #include "HelloWorld.h"
+#include "IMUKit.hpp"
 #include "LedColorRecognition.h"
 #include "LedKit.h"
 #include "LedNumberCounting.h"
@@ -270,13 +271,11 @@ auto imukit = IMUKit {imu::lsm6dsox};
 
 namespace motion::internal {
 
-	EventLoopKit event_loop {};
 	CoreTimeout timeout {};
 
 }	// namespace motion::internal
 
-auto motionkit = MotionKit {motors::left::motor, motors::right::motor, imukit, motion::internal::event_loop,
-							motion::internal::timeout};
+auto motionkit = MotionKit {motors::left::motor, motors::right::motor, imukit, motion::internal::timeout};
 
 auto behaviorkit   = BehaviorKit {videokit, ledkit, motors::left::motor, motors::right::motor};
 auto reinforcerkit = ReinforcerKit {videokit, ledkit, motionkit};
@@ -562,7 +561,6 @@ auto main() -> int
 
 	imu::lsm6dsox.init();
 	imukit.init();
-	motionkit.init();
 
 	robot::controller.initializeComponents();
 	robot::controller.registerOnUpdateLoadedCallback(firmware::setPendingUpdate);

include/interface/libs/IMUKit.hpp

@@ -0,0 +1,36 @@
+// Leka - LekaOS
+// Copyright 2023 APF France handicap
+// SPDX-License-Identifier: Apache-2.0
+
+#pragma once
+
+#include <functional>
+
+namespace leka {
+
+struct EulerAngles {
+	float pitch;
+	float roll;
+	float yaw;
+};
+
+namespace interface {
+
+	class IMUKit
+	{
+	  public:
+		using angles_ready_callback_t = std::function<void(const EulerAngles &)>;
+
+		virtual ~IMUKit() = default;
+
+		virtual void start() = 0;
+		virtual void stop()	 = 0;
+
+		virtual void setOrigin()												 = 0;
+		virtual void onEulerAnglesReady(angles_ready_callback_t const &callback) = 0;
+		[[nodiscard]] virtual auto getEulerAngles() const -> EulerAngles		 = 0;
+	};
+
+}	// namespace interface
+
+}	// namespace leka

libs/IMUKit/include/IMUKit.hpp

@@ -5,32 +5,29 @@
 #pragma once
 
 #include "interface/LSM6DSOX.hpp"
+#include "interface/libs/IMUKit.hpp"
 
 namespace leka {
 
-struct EulerAngles {
-	float pitch;
-	float roll;
-	float yaw;
-};
-
-class IMUKit
+class IMUKit : public interface::IMUKit
 {
   public:
 	explicit IMUKit(interface::LSM6DSOX &lsm6dsox) : _lsm6dsox(lsm6dsox) {}
 
 	void init();
-	void start();
-	void stop();
+	void start() final;
+	void stop() final;
 
-	void setOrigin();
-	[[nodiscard]] auto getEulerAngles() const -> EulerAngles;
+	void setOrigin() final;
+	void onEulerAnglesReady(angles_ready_callback_t const &callback) final;
+	[[nodiscard]] auto getEulerAngles() const -> EulerAngles final;
 
   private:
 	void drdy_callback(interface::LSM6DSOX::SensorData data);
 
 	interface::LSM6DSOX &_lsm6dsox;
 	EulerAngles _euler_angles {};
+	angles_ready_callback_t _on_euler_angles_rdy_callback {};
 };
 
 }	// namespace leka

libs/IMUKit/source/IMUKit.cpp

@@ -83,6 +83,11 @@ auto IMUKit::getEulerAngles() const -> EulerAngles
 	return _euler_angles;
 }
 
+void IMUKit::onEulerAnglesReady(angles_ready_callback_t const &callback)
+{
+	_on_euler_angles_rdy_callback = callback;
+}
+
 void IMUKit::drdy_callback(const interface::LSM6DSOX::SensorData data)
 {
 	// ? Note: For a detailed explanation on the code below, checkout
@@ -113,4 +118,8 @@ void IMUKit::drdy_callback(const interface::LSM6DSOX::SensorData data)
 		   .roll  = euler.angle.roll,
 		   .yaw	  = euler.angle.yaw,
 	   };
+
+	if (_on_euler_angles_rdy_callback) {
+		_on_euler_angles_rdy_callback(_euler_angles);
+	}
 };

libs/IMUKit/tests/IMUKit_test.cpp

@@ -66,6 +66,41 @@ TEST_F(IMUKitTest, setOrigin)
 
 TEST_F(IMUKitTest, onDataReady)
 {
+	testing::MockFunction<void(EulerAngles angles)> mock_callback {};
+
+	imukit.onEulerAnglesReady(mock_callback.AsStdFunction());
+
+	const auto data_initial = interface::LSM6DSOX::SensorData {
+		.xl = {.x = 0.F, .y = 0.F, .z = 0.F}, .gy = {.x = 0.F, .y = 0.F, .z = 0.F }
+	};
+
+	EXPECT_CALL(mock_callback, Call);
+
+	mock_lsm6dox.call_drdy_callback(data_initial);
+
+	const auto angles_initial = imukit.getEulerAngles();
+
+	spy_kernel_addElapsedTimeToTickCount(80ms);
+
+	const auto data_updated = interface::LSM6DSOX::SensorData {
+		.xl = {.x = 1.F, .y = 2.F, .z = 3.F}, .gy = {.x = 1.F, .y = 2.F, .z = 3.F }
+	};
+
+	EXPECT_CALL(mock_callback, Call);
+
+	mock_lsm6dox.call_drdy_callback(data_updated);
+
+	auto angles_updated = imukit.getEulerAngles();
+
+	EXPECT_NE(angles_initial.pitch, angles_updated.pitch);
+	EXPECT_NE(angles_initial.roll, angles_updated.roll);
+	EXPECT_NE(angles_initial.yaw, angles_updated.yaw);
+}
+
+TEST_F(IMUKitTest, onDataReadyEmptyEulerAngleCallback)
+{
+	imukit.onEulerAnglesReady({});
+
 	const auto data_initial = interface::LSM6DSOX::SensorData {
 		.xl = {.x = 0.F, .y = 0.F, .z = 0.F}, .gy = {.x = 0.F, .y = 0.F, .z = 0.F }
 	};

libs/MotionKit/CMakeLists.txt

@@ -12,7 +12,8 @@ target_include_directories(MotionKit
 target_sources(MotionKit
 	PRIVATE
 	source/MotionKit.cpp
-	source/PID.cpp
+	source/RotationControl.cpp
+	source/StabilizationControl.cpp
 )
 
 target_link_libraries(MotionKit
@@ -23,6 +24,7 @@ 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
+		tests/StabilizationControl_test.cpp
 	)
 endif()

libs/MotionKit/include/MotionKit.hpp

@@ -5,65 +5,46 @@
 #pragma once
 
 #include <interface/drivers/Motor.h>
-#include <interface/libs/EventLoop.h>
 
-#include "IMUKit.hpp"
-#include "PID.hpp"
+#include "RotationControl.hpp"
+#include "StabilizationControl.hpp"
 #include "interface/drivers/Timeout.h"
+#include "interface/libs/IMUKit.hpp"
 
 namespace leka {
 
 class MotionKit
 {
   public:
-	MotionKit(interface::Motor &motor_left, interface::Motor &motor_right, IMUKit &imu_kit,
-			  interface::EventLoop &event_loop, interface::Timeout &timeout)
-		: _motor_left(motor_left),
-		  _motor_right(motor_right),
-		  _imukit(imu_kit),
-		  _event_loop(event_loop),
-		  _timeout(timeout)
+	MotionKit(interface::Motor &motor_left, interface::Motor &motor_right, interface::IMUKit &imu_kit,
+			  interface::Timeout &timeout)
+		: _motor_left(motor_left), _motor_right(motor_right), _imukit(imu_kit), _timeout(timeout)
 	{
 	}
 
-	void init();
-
-	void rotate(uint8_t number_of_rotations, Rotation direction,
-				const std::function<void()> &on_rotation_ended_callback = {});
-	void startStabilisation();
+	void startYawRotation(float degrees, Rotation direction,
+						  const std::function<void()> &on_rotation_ended_callback = {});
+	void startStabilization();
 
 	void stop();
 
   private:
-	void run();
+	void processAngleForRotation(const EulerAngles &angles, Rotation direction);
+	void processAngleForStabilization(const EulerAngles &angles);
 
-	[[nodiscard]] auto mapSpeed(float speed) const -> float;
-	void executeSpeed(float speed, Rotation direction);
+	void setMotorsSpeedAndDirection(float speed, Rotation direction);
 
 	interface::Motor &_motor_left;
 	interface::Motor &_motor_right;
-	IMUKit &_imukit;
-	interface::EventLoop &_event_loop;
+	interface::IMUKit &_imukit;
 	interface::Timeout &_timeout;
-	PID _pid;
 
-	uint8_t _rotations_to_execute = 0;
+	RotationControl _rotation_control;
+	StabilizationControl _stabilization_control;
 	std::function<void()> _on_rotation_ended_callback {};
 
 	bool _target_not_reached	   = false;
-	bool _stabilisation_requested  = false;
 	bool _rotate_x_turns_requested = false;
-
-	const float kReferenceAngle = 180.F;
-	const float kPIDMaxValue	= 1.8F;
-
-	// ? 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
-
-	const float kMinimalViableRobotPwm = 0.15F;
-	const float kPwmMaxValue		   = 1.F;
-	const float kEpsilon			   = 0.005F;
 };
 
 }	// namespace leka

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