diff --git a/app/os/main.cpp b/app/os/main.cpp
index 3ac5081c44..4c64b55ce7 100644
--- a/app/os/main.cpp
+++ b/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);
diff --git a/include/interface/libs/IMUKit.hpp b/include/interface/libs/IMUKit.hpp
new file mode 100644
index 0000000000..4d2f80459b
--- /dev/null
+++ b/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
diff --git a/libs/IMUKit/include/IMUKit.hpp b/libs/IMUKit/include/IMUKit.hpp
index 44dc217529..ababcfb531 100644
--- a/libs/IMUKit/include/IMUKit.hpp
+++ b/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
diff --git a/libs/IMUKit/source/IMUKit.cpp b/libs/IMUKit/source/IMUKit.cpp
index c1e2f5a3e8..d1681c2691 100644
--- a/libs/IMUKit/source/IMUKit.cpp
+++ b/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);
+	}
 };
diff --git a/libs/IMUKit/tests/IMUKit_test.cpp b/libs/IMUKit/tests/IMUKit_test.cpp
index a5e399382f..242bc8a86c 100644
--- a/libs/IMUKit/tests/IMUKit_test.cpp
+++ b/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 }
 	};
diff --git a/libs/MotionKit/CMakeLists.txt b/libs/MotionKit/CMakeLists.txt
index 50a3eaad3c..93dd4fd922 100644
--- a/libs/MotionKit/CMakeLists.txt
+++ b/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()
diff --git a/libs/MotionKit/include/MotionKit.hpp b/libs/MotionKit/include/MotionKit.hpp
index ffb7fcc985..95e115bbd7 100644
--- a/libs/MotionKit/include/MotionKit.hpp
+++ b/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
diff --git a/libs/MotionKit/include/PID.hpp b/libs/MotionKit/include/PID.hpp
deleted file mode 100644
index 3a6319223e..0000000000
--- a/libs/MotionKit/include/PID.hpp
+++ /dev/null
@@ -1,44 +0,0 @@
-// 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
diff --git a/libs/MotionKit/include/RotationControl.hpp b/libs/MotionKit/include/RotationControl.hpp
new file mode 100644
index 0000000000..c06b246cc9
--- /dev/null
+++ b/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
diff --git a/libs/MotionKit/include/StabilizationControl.hpp b/libs/MotionKit/include/StabilizationControl.hpp
new file mode 100644
index 0000000000..7896f44db3
--- /dev/null
+++ b/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
diff --git a/libs/MotionKit/source/MotionKit.cpp b/libs/MotionKit/source/MotionKit.cpp
index a9d05384ac..a05547ea20 100644
--- a/libs/MotionKit/source/MotionKit.cpp
+++ b/libs/MotionKit/source/MotionKit.cpp
@@ -4,127 +4,101 @@
 
 #include "MotionKit.hpp"
 
-#include "MathUtils.h"
 #include "ThisThread.h"
 
 using namespace leka;
 using namespace std::chrono_literals;
 
-void MotionKit::init()
-{
-	_event_loop.registerCallback([this] { run(); });
-}
-
 void MotionKit::stop()
 {
 	_timeout.stop();
 	_motor_left.stop();
 	_motor_right.stop();
-	_event_loop.stop();
 
-	_stabilisation_requested  = false;
+	_imukit.onEulerAnglesReady({});
+
 	_target_not_reached		  = false;
 	_rotate_x_turns_requested = false;
 }
 
-void MotionKit::rotate(uint8_t number_of_rotations, Rotation direction,
-					   const std::function<void()> &on_rotation_ended_callback)
+void MotionKit::startYawRotation(float degrees, Rotation direction,
+								 const std::function<void()> &on_rotation_ended_callback)
 {
 	stop();
 
-	_imukit.start();
-	_imukit.setOrigin();
+	auto starting_angle = _imukit.getEulerAngles();
+	_rotation_control.setTarget(starting_angle, degrees);
 
 	_target_not_reached		  = true;
-	_stabilisation_requested  = false;
 	_rotate_x_turns_requested = true;
 
-	_rotations_to_execute = number_of_rotations;
-
-	_motor_left.spin(direction, kPwmMaxValue);
-	_motor_right.spin(direction, kPwmMaxValue);
-
 	auto on_timeout = [this] { stop(); };
 
 	_timeout.onTimeout(on_timeout);
 	_timeout.start(10s);
 
-	_event_loop.start();
+	auto on_euler_angles_rdy_callback = [this, direction](const EulerAngles &euler_angles) {
+		processAngleForRotation(euler_angles, direction);
+	};
+
+	_imukit.onEulerAnglesReady(on_euler_angles_rdy_callback);
 
 	_on_rotation_ended_callback = on_rotation_ended_callback;
 }
 
-void MotionKit::startStabilisation()
+void MotionKit::startStabilization()
 {
 	stop();
 
-	_imukit.start();
-	_imukit.setOrigin();
+	auto starting_angle = _imukit.getEulerAngles();
+	_stabilization_control.setTarget(starting_angle);
 
-	_target_not_reached		  = false;
-	_stabilisation_requested  = true;
-	_rotate_x_turns_requested = false;
+	auto on_euler_angles_rdy_callback = [this](const EulerAngles &euler_angles) {
+		processAngleForStabilization(euler_angles);
+	};
 
-	_event_loop.start();
+	_imukit.onEulerAnglesReady(on_euler_angles_rdy_callback);
 }
 
 // LCOV_EXCL_START - Dynamic behavior, involving motors and time.
-void MotionKit::run()
+void MotionKit::processAngleForRotation(const EulerAngles &angles, Rotation direction)
 {
-	auto last_yaw			= kReferenceAngle;
-	auto rotations_executed = 0;
+	auto must_stop = [&] { return !_rotate_x_turns_requested && !_target_not_reached; };
 
-	auto must_rotate = [&] { return _rotate_x_turns_requested && rotations_executed != _rotations_to_execute; };
+	if (must_stop()) {
+		stop();
 
-	auto check_complete_rotations_executed = [&](auto current_yaw) {
-		if (std::abs(last_yaw - current_yaw) >= 300.F) {
-			++rotations_executed;
+		if (_on_rotation_ended_callback) {
+			_on_rotation_ended_callback();
 		}
-	};
-
-	while (must_rotate()) {
-		auto [current_pitch, current_roll, current_yaw] = _imukit.getEulerAngles();
-
-		check_complete_rotations_executed(current_yaw);
 
-		rtos::ThisThread::sleep_for(70ms);
-		last_yaw = current_yaw;
+		return;
 	}
 
-	_rotate_x_turns_requested = false;
-	_rotations_to_execute	  = 0;
-
-	while (_stabilisation_requested || _target_not_reached) {
-		auto [pitch, roll, yaw] = _imukit.getEulerAngles();
-		auto [speed, rotation]	= _pid.processPID(pitch, roll, yaw);
+	if (_rotate_x_turns_requested && _target_not_reached) {
+		auto speed = _rotation_control.processRotationAngle(angles);
 
-		executeSpeed(speed, rotation);
-
-		rtos::ThisThread::sleep_for(70ms);
-	}
-
-	if (_on_rotation_ended_callback != nullptr) {
-		_on_rotation_ended_callback();
+		setMotorsSpeedAndDirection(speed, direction);
 	}
-
-	_imukit.stop();
 }
 
-auto MotionKit::mapSpeed(float speed) const -> float
+void MotionKit::processAngleForStabilization(const EulerAngles &angles)
 {
-	return utils::math::map(speed, 0.F, kPIDMaxValue, kMinimalViableRobotPwm, kPwmMaxValue);
+	auto [speed, rotation] = _stabilization_control.processStabilizationAngle(angles);
+
+	setMotorsSpeedAndDirection(speed, rotation);
 }
 
-void MotionKit::executeSpeed(float speed, Rotation direction)
+void MotionKit::setMotorsSpeedAndDirection(float speed, Rotation direction)
 {
-	auto speed_bounded = mapSpeed(speed);
-	if (speed_bounded <= kMinimalViableRobotPwm + kEpsilon) {
+	if (speed == 0.F) {
 		_motor_left.stop();
 		_motor_right.stop();
-		_target_not_reached = false;
+		_target_not_reached		  = false;
+		_rotate_x_turns_requested = false;
 	} else {
-		_motor_left.spin(direction, speed_bounded);
-		_motor_right.spin(direction, speed_bounded);
+		_motor_left.spin(direction, speed);
+		_motor_right.spin(direction, speed);
 		_target_not_reached = true;
 	}
 }
diff --git a/libs/MotionKit/source/PID.cpp b/libs/MotionKit/source/PID.cpp
deleted file mode 100644
index 2317d2a10b..0000000000
--- a/libs/MotionKit/source/PID.cpp
+++ /dev/null
@@ -1,42 +0,0 @@
-// Leka - LekaOS
-// Copyright 2022 APF France handicap
-// SPDX-License-Identifier: Apache-2.0
-
-#include "PID.hpp"
-#include <algorithm>
-
-using namespace leka;
-
-auto PID::processPID([[maybe_unused]] float pitch, [[maybe_unused]] float roll, float yaw)
-	-> std::tuple<float, Rotation>
-{
-	auto direction			= Rotation {};
-	_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/source/RotationControl.cpp b/libs/MotionKit/source/RotationControl.cpp
new file mode 100644
index 0000000000..aa54808053
--- /dev/null
+++ b/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;
+}
diff --git a/libs/MotionKit/source/StabilizationControl.cpp b/libs/MotionKit/source/StabilizationControl.cpp
new file mode 100644
index 0000000000..5eaec6df65
--- /dev/null
+++ b/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;
+}
diff --git a/libs/MotionKit/tests/MotionKit_test.cpp b/libs/MotionKit/tests/MotionKit_test.cpp
index 878cb65299..958b486bd6 100644
--- a/libs/MotionKit/tests/MotionKit_test.cpp
+++ b/libs/MotionKit/tests/MotionKit_test.cpp
@@ -7,13 +7,13 @@
 #include "IMUKit.hpp"
 #include "gtest/gtest.h"
 #include "mocks/leka/CoreMotor.h"
-#include "mocks/leka/LSM6DSOX.h"
+#include "mocks/leka/IMUKit.hpp"
 #include "mocks/leka/Timeout.h"
-#include "stubs/leka/EventLoopKit.h"
 
 using namespace leka;
 
-using ::testing::MockFunction;
+using ::testing::_;
+using ::testing::AtLeast;
 
 // TODO(@leka/dev-embedded): temporary fix, changes are needed when updating fusion algorithm
 
@@ -27,28 +27,19 @@ class MotionKitTest : public ::testing::Test
   protected:
 	MotionKitTest() = default;
 
-	void SetUp() override
-	{
-		imukit.init();
-		motion.init();
-	}
+	// void SetUp() override {}
 	// void TearDown() override {}
 
-	stub::EventLoopKit stub_event_loop_motion {};
-
 	mock::CoreMotor mock_motor_left {};
 	mock::CoreMotor mock_motor_right {};
 
-	mock::LSM6DSOX lsm6dsox {};
-
 	mock::Timeout mock_timeout {};
 
-	std::tuple<float, float, float> accel_data = {0.F, 0.F, 0.F};
-	std::tuple<float, float, float> gyro_data  = {0.F, 0.F, 0.F};
+	const EulerAngles angles {0.F, 0.F, 0.F};
 
-	IMUKit imukit {lsm6dsox};
+	mock::IMUKit mock_imukit {};
 
-	MotionKit motion {mock_motor_left, mock_motor_right, imukit, stub_event_loop_motion, mock_timeout};
+	MotionKit motion {mock_motor_left, mock_motor_right, mock_imukit, mock_timeout};
 };
 
 TEST_F(MotionKitTest, initialization)
@@ -56,91 +47,86 @@ TEST_F(MotionKitTest, initialization)
 	ASSERT_NE(&motion, nullptr);
 }
 
-TEST_F(MotionKitTest, registerMockCallbackAndRotate)
+TEST_F(MotionKitTest, rotateClockwise)
 {
-	auto mock_function_motion = MockFunction<void(void)> {};
-	auto loop_motion		  = [&] { mock_function_motion.Call(); };
-
-	EXPECT_CALL(lsm6dsox, setPowerMode(interface::LSM6DSOX::PowerMode::Normal)).Times(1);
-	EXPECT_CALL(mock_function_motion, Call()).Times(1);
-
 	EXPECT_CALL(mock_timeout, stop).Times(1);
 	EXPECT_CALL(mock_motor_left, stop).Times(1);
 	EXPECT_CALL(mock_motor_right, stop).Times(1);
 
+	EXPECT_CALL(mock_imukit, getEulerAngles).Times(1);
+
 	EXPECT_CALL(mock_timeout, onTimeout).Times(1);
 	EXPECT_CALL(mock_timeout, start).Times(1);
 
-	EXPECT_CALL(mock_motor_left, spin(Rotation::clockwise, 1)).Times(1);
-	EXPECT_CALL(mock_motor_right, spin(Rotation::clockwise, 1)).Times(1);
+	EXPECT_CALL(mock_motor_left, spin(Rotation::clockwise, _)).Times(AtLeast(1));
+	EXPECT_CALL(mock_motor_right, spin(Rotation::clockwise, _)).Times(AtLeast(1));
+
+	motion.startYawRotation(1, Rotation::clockwise);
 
-	stub_event_loop_motion.registerCallback(loop_motion);
-	motion.rotate(1, Rotation::clockwise);
+	mock_imukit.call_angles_ready_callback(angles);
 }
 
-TEST_F(MotionKitTest, registerMockCallbackAndStartStabilisation)
+TEST_F(MotionKitTest, rotateCounterClockwise)
 {
-	auto mock_function_motion = MockFunction<void(void)> {};
-	auto loop_motion		  = [&] { mock_function_motion.Call(); };
-
-	EXPECT_CALL(lsm6dsox, setPowerMode(interface::LSM6DSOX::PowerMode::Normal)).Times(1);
-	EXPECT_CALL(mock_function_motion, Call()).Times(1);
-
 	EXPECT_CALL(mock_timeout, stop).Times(1);
 	EXPECT_CALL(mock_motor_left, stop).Times(1);
 	EXPECT_CALL(mock_motor_right, stop).Times(1);
 
-	stub_event_loop_motion.registerCallback(loop_motion);
-	motion.startStabilisation();
+	EXPECT_CALL(mock_imukit, getEulerAngles).Times(1);
+
+	EXPECT_CALL(mock_timeout, onTimeout).Times(1);
+	EXPECT_CALL(mock_timeout, start).Times(1);
+
+	EXPECT_CALL(mock_motor_left, spin(Rotation::counterClockwise, _)).Times(AtLeast(1));
+	EXPECT_CALL(mock_motor_right, spin(Rotation::counterClockwise, _)).Times(AtLeast(1));
+
+	motion.startYawRotation(1, Rotation::counterClockwise);
+
+	mock_imukit.call_angles_ready_callback(angles);
 }
 
 TEST_F(MotionKitTest, rotateAndStop)
 {
-	auto mock_function_motion = MockFunction<void(void)> {};
-	auto loop_motion		  = [&] {
-		 mock_function_motion.Call();
-		 motion.stop();
-	};
-
-	EXPECT_CALL(lsm6dsox, setPowerMode(interface::LSM6DSOX::PowerMode::Normal)).Times(1);
-	EXPECT_CALL(mock_function_motion, Call()).Times(1);
-
-	EXPECT_CALL(mock_timeout, stop).Times(2);
-	EXPECT_CALL(mock_motor_left, stop).Times(2);
-	EXPECT_CALL(mock_motor_right, stop).Times(2);
+	EXPECT_CALL(mock_timeout, stop).Times(1);
+	EXPECT_CALL(mock_motor_left, stop).Times(1);
+	EXPECT_CALL(mock_motor_right, stop).Times(1);
 
-	EXPECT_CALL(mock_motor_left, spin).Times(1);
-	EXPECT_CALL(mock_motor_right, spin).Times(1);
+	EXPECT_CALL(mock_imukit, getEulerAngles).Times(1);
 
 	EXPECT_CALL(mock_timeout, onTimeout).Times(1);
 	EXPECT_CALL(mock_timeout, start).Times(1);
 
-	stub_event_loop_motion.registerCallback(loop_motion);
-	motion.rotate(1, Rotation::clockwise);
+	EXPECT_CALL(mock_motor_left, spin(Rotation::clockwise, _)).Times(AtLeast(1));
+	EXPECT_CALL(mock_motor_right, spin(Rotation::clockwise, _)).Times(AtLeast(1));
+
+	motion.startYawRotation(1, Rotation::clockwise);
+	mock_imukit.call_angles_ready_callback(angles);
+
+	EXPECT_CALL(mock_timeout, stop).Times(1);
+	EXPECT_CALL(mock_motor_left, stop).Times(1);
+	EXPECT_CALL(mock_motor_right, stop).Times(1);
+
+	motion.stop();
 }
 
 TEST_F(MotionKitTest, rotateAndTimeOutOver)
 {
-	auto mock_function_motion = MockFunction<void(void)> {};
-	auto loop_motion		  = [&] { mock_function_motion.Call(); };
-
 	interface::Timeout::callback_t on_timeout_callback = {};
 
-	EXPECT_CALL(lsm6dsox, setPowerMode(interface::LSM6DSOX::PowerMode::Normal)).Times(1);
-	EXPECT_CALL(mock_function_motion, Call()).Times(1);
-
 	EXPECT_CALL(mock_timeout, stop).Times(1);
 	EXPECT_CALL(mock_motor_left, stop).Times(1);
 	EXPECT_CALL(mock_motor_right, stop).Times(1);
 
-	EXPECT_CALL(mock_motor_left, spin).Times(1);
-	EXPECT_CALL(mock_motor_right, spin).Times(1);
+	EXPECT_CALL(mock_imukit, getEulerAngles).Times(1);
 
 	EXPECT_CALL(mock_timeout, onTimeout).WillOnce(GetCallback<interface::Timeout::callback_t>(&on_timeout_callback));
 	EXPECT_CALL(mock_timeout, start).Times(1);
 
-	stub_event_loop_motion.registerCallback(loop_motion);
-	motion.rotate(1, Rotation::clockwise);
+	EXPECT_CALL(mock_motor_left, spin(Rotation::clockwise, _)).Times(AtLeast(1));
+	EXPECT_CALL(mock_motor_right, spin(Rotation::clockwise, _)).Times(AtLeast(1));
+
+	motion.startYawRotation(1, Rotation::clockwise);
+	mock_imukit.call_angles_ready_callback(angles);
 
 	EXPECT_CALL(mock_timeout, stop).Times(1);
 	EXPECT_CALL(mock_motor_left, stop).Times(1);
@@ -149,21 +135,42 @@ TEST_F(MotionKitTest, rotateAndTimeOutOver)
 	on_timeout_callback();
 }
 
-TEST_F(MotionKitTest, startStabilisationAndStop)
+TEST_F(MotionKitTest, startStabilization)
 {
-	auto mock_function_motion = MockFunction<void(void)> {};
-	auto loop_motion		  = [&] {
-		 mock_function_motion.Call();
-		 motion.stop();
-	};
+	const EulerAngles angles_quarter_left {0.0F, 0.0F, 90.F};
+
+	EXPECT_CALL(mock_timeout, stop).Times(1);
+	EXPECT_CALL(mock_motor_left, stop).Times(1);
+	EXPECT_CALL(mock_motor_right, stop).Times(1);
+
+	EXPECT_CALL(mock_imukit, getEulerAngles).Times(1);
 
-	EXPECT_CALL(lsm6dsox, setPowerMode(interface::LSM6DSOX::PowerMode::Normal)).Times(1);
-	EXPECT_CALL(mock_function_motion, Call()).Times(1);
+	EXPECT_CALL(mock_motor_left, spin).Times(AtLeast(1));
+	EXPECT_CALL(mock_motor_right, spin).Times(AtLeast(1));
 
-	EXPECT_CALL(mock_timeout, stop).Times(2);
-	EXPECT_CALL(mock_motor_left, stop).Times(2);
-	EXPECT_CALL(mock_motor_right, stop).Times(2);
+	motion.startStabilization();
+	mock_imukit.call_angles_ready_callback(angles_quarter_left);
+}
+
+TEST_F(MotionKitTest, startStabilizationAndStop)
+{
+	const EulerAngles angles_quarter_left {0.0F, 0.0F, 90.F};
+
+	EXPECT_CALL(mock_timeout, stop).Times(1);
+	EXPECT_CALL(mock_motor_left, stop).Times(1);
+	EXPECT_CALL(mock_motor_right, stop).Times(1);
+
+	EXPECT_CALL(mock_imukit, getEulerAngles).Times(1);
+
+	EXPECT_CALL(mock_motor_left, spin).Times(AtLeast(1));
+	EXPECT_CALL(mock_motor_right, spin).Times(AtLeast(1));
+
+	motion.startStabilization();
+	mock_imukit.call_angles_ready_callback(angles_quarter_left);
+
+	EXPECT_CALL(mock_timeout, stop).Times(1);
+	EXPECT_CALL(mock_motor_left, stop).Times(1);
+	EXPECT_CALL(mock_motor_right, stop).Times(1);
 
-	stub_event_loop_motion.registerCallback(loop_motion);
-	motion.startStabilisation();
+	motion.stop();
 }
diff --git a/libs/MotionKit/tests/PID_test.cpp b/libs/MotionKit/tests/PID_test.cpp
deleted file mode 100644
index 69e33374fb..0000000000
--- a/libs/MotionKit/tests/PID_test.cpp
+++ /dev/null
@@ -1,87 +0,0 @@
-// Leka - LekaOS
-// Copyright 2022 APF France handicap
-// SPDX-License-Identifier: Apache-2.0
-
-#include "PID.hpp"
-
-#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/libs/MotionKit/tests/RotationControl_test.cpp b/libs/MotionKit/tests/RotationControl_test.cpp
new file mode 100644
index 0000000000..fbd2139e01
--- /dev/null
+++ b/libs/MotionKit/tests/RotationControl_test.cpp
@@ -0,0 +1,105 @@
+// Leka - LekaOS
+// Copyright 2023 APF France handicap
+// SPDX-License-Identifier: Apache-2.0
+
+#include "RotationControl.hpp"
+
+#include "LogKit.h"
+#include "gtest/gtest.h"
+
+using namespace leka;
+
+class RotationControlTest : public ::testing::Test
+{
+  protected:
+	RotationControlTest() = default;
+
+	// void SetUp() override {}
+	//  void TearDown() override {}
+
+	RotationControl rotation_control {};
+
+	EulerAngles angle {.yaw = 0.F};
+
+	const uint8_t kDegreeBoundNullSpeedArea = 3;
+
+	float current_speed;
+	float previous_speed = 1000;
+};
+
+TEST_F(RotationControlTest, initialization)
+{
+	ASSERT_NE(&rotation_control, nullptr);
+}
+
+TEST_F(RotationControlTest, processRotationAngleCounterClockwise)
+{
+	rotation_control.setTarget(angle, 1.0);
+
+	for (auto i = 0; i < 180; i += 5) {
+		auto current_angle = EulerAngles {.yaw = static_cast<float>(i)};
+
+		current_speed = rotation_control.processRotationAngle(current_angle);
+
+		EXPECT_LE(current_speed, previous_speed);
+
+		previous_speed = current_speed;
+	}
+
+	for (auto i = -180; i < -kDegreeBoundNullSpeedArea; i += 5) {
+		auto current_angle = EulerAngles {.yaw = static_cast<float>(i)};
+
+		current_speed = rotation_control.processRotationAngle(current_angle);
+
+		EXPECT_LE(current_speed, previous_speed);
+
+		previous_speed = current_speed;
+	}
+
+	for (auto i = -kDegreeBoundNullSpeedArea; i < 0; i += 1) {
+		auto current_angle = EulerAngles {.yaw = static_cast<float>(i)};
+
+		current_speed = rotation_control.processRotationAngle(current_angle);
+
+		EXPECT_EQ(current_speed, 0.F);
+	}
+}
+
+TEST_F(RotationControlTest, processRotationAngleClockwise)
+{
+	rotation_control.setTarget(angle, 1.0);
+
+	for (auto i = 0.F; i > -180; i -= 5) {
+		auto current_angle = EulerAngles {.yaw = static_cast<float>(i)};
+
+		current_speed = rotation_control.processRotationAngle(current_angle);
+
+		EXPECT_LE(current_speed, previous_speed);
+
+		log_debug("1.");
+
+		previous_speed = current_speed;
+	}
+
+	for (auto i = 180; i > kDegreeBoundNullSpeedArea; i -= 5) {
+		auto current_angle = EulerAngles {.yaw = static_cast<float>(i)};
+
+		current_speed = rotation_control.processRotationAngle(current_angle);
+
+		EXPECT_LE(current_speed, previous_speed);
+
+		log_debug("2");
+
+		previous_speed = current_speed;
+	}
+
+	for (auto i = kDegreeBoundNullSpeedArea; i > 0; i -= 1) {
+		auto current_angle = EulerAngles {.yaw = static_cast<float>(i)};
+
+		current_speed = rotation_control.processRotationAngle(current_angle);
+
+		log_debug("3.");
+
+		EXPECT_EQ(current_speed, 0.F);
+	}
+}
diff --git a/libs/MotionKit/tests/StabilizationControl_test.cpp b/libs/MotionKit/tests/StabilizationControl_test.cpp
new file mode 100644
index 0000000000..3ab185ed4e
--- /dev/null
+++ b/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);
+}
diff --git a/libs/ReinforcerKit/source/ReinforcerKit.cpp b/libs/ReinforcerKit/source/ReinforcerKit.cpp
index 9d1c2bd93d..de5143a324 100644
--- a/libs/ReinforcerKit/source/ReinforcerKit.cpp
+++ b/libs/ReinforcerKit/source/ReinforcerKit.cpp
@@ -52,14 +52,14 @@ void ReinforcerKit::playBlinkGreen()
 {
 	_videokit.playVideoOnce("/fs/home/vid/system/robot-system-reinforcer-happy-no_eyebrows.avi");
 	_ledkit.start(&led::animation::blink_green);
-	_motionkit.rotate(3, Rotation::clockwise, [this] { _ledkit.stop(); });
+	_motionkit.startYawRotation(3, Rotation::clockwise, [this] { _ledkit.stop(); });
 }
 
 void ReinforcerKit::playSpinBlink()
 {
 	_videokit.playVideoOnce("/fs/home/vid/system/robot-system-reinforcer-happy-no_eyebrows.avi");
 	_ledkit.start(&led::animation::spin_blink);
-	_motionkit.rotate(3, Rotation::counterClockwise, [this] { _ledkit.stop(); });
+	_motionkit.startYawRotation(3, Rotation::counterClockwise, [this] { _ledkit.stop(); });
 }
 
 void ReinforcerKit::playFire()
diff --git a/libs/ReinforcerKit/tests/ReinforcerKit_test.cpp b/libs/ReinforcerKit/tests/ReinforcerKit_test.cpp
index 6559cba720..db34bcd9d3 100644
--- a/libs/ReinforcerKit/tests/ReinforcerKit_test.cpp
+++ b/libs/ReinforcerKit/tests/ReinforcerKit_test.cpp
@@ -9,17 +9,15 @@
 #include "gmock/gmock.h"
 #include "gtest/gtest.h"
 #include "mocks/leka/CoreMotor.h"
-#include "mocks/leka/LSM6DSOX.h"
+#include "mocks/leka/IMUKit.hpp"
 #include "mocks/leka/LedKit.h"
 #include "mocks/leka/Timeout.h"
 #include "mocks/leka/VideoKit.h"
-#include "stubs/leka/EventLoopKit.h"
 
 using namespace leka;
 
-using ::testing::AnyNumber;
+using ::testing::AtLeast;
 using ::testing::AtMost;
-using ::testing::Sequence;
 
 MATCHER_P(isSameAnimation, expected_animation, "")
 {
@@ -39,30 +37,28 @@ class ReinforcerkitTest : public ::testing::Test
 
 	mock::LedKit mock_ledkit;
 
-	stub::EventLoopKit stub_event_loop_motion {};
-
 	mock::CoreMotor mock_motor_left {};
 	mock::CoreMotor mock_motor_right {};
 
-	mock::LSM6DSOX lsm6dsox {};
-
 	mock::Timeout mock_timeout {};
 
-	IMUKit imukit {lsm6dsox};
+	mock::IMUKit mock_imukit {};
 
-	MotionKit motion {mock_motor_left, mock_motor_right, imukit, stub_event_loop_motion, mock_timeout};
+	MotionKit motion {mock_motor_left, mock_motor_right, mock_imukit, mock_timeout};
 
 	ReinforcerKit reinforcerkit;
 
+	const EulerAngles angles {0.0F, 0.0F, 0.F};
+
 	void expectedCallsMovingReinforcer(interface::LEDAnimation *animation)
 	{
 		EXPECT_CALL(mock_videokit, playVideoOnce);
 		EXPECT_CALL(mock_motor_left, stop).Times(1);
 		EXPECT_CALL(mock_motor_right, stop).Times(1);
-		EXPECT_CALL(mock_motor_left, spin).Times(1);
-		EXPECT_CALL(mock_motor_right, spin).Times(1);
-		EXPECT_CALL(lsm6dsox, setPowerMode(interface::LSM6DSOX::PowerMode::Normal)).Times(1);
+		EXPECT_CALL(mock_motor_left, spin).Times(AtLeast(1));
+		EXPECT_CALL(mock_motor_right, spin).Times(AtLeast(1));
 		EXPECT_CALL(mock_timeout, stop).Times(AtMost(1));
+		EXPECT_CALL(mock_imukit, getEulerAngles).Times(1);
 		EXPECT_CALL(mock_timeout, onTimeout).Times(AtMost(1));
 		EXPECT_CALL(mock_timeout, start).Times(AtMost(1));
 		EXPECT_CALL(mock_ledkit, start(isSameAnimation(animation)));
@@ -85,6 +81,8 @@ TEST_F(ReinforcerkitTest, playBlinkGreen)
 	expectedCallsMovingReinforcer(&led::animation::blink_green);
 
 	reinforcerkit.play(ReinforcerKit::Reinforcer::BlinkGreen);
+
+	mock_imukit.call_angles_ready_callback(angles);
 }
 
 TEST_F(ReinforcerkitTest, playSpinBlink)
@@ -92,6 +90,8 @@ TEST_F(ReinforcerkitTest, playSpinBlink)
 	expectedCallsMovingReinforcer(&led::animation::spin_blink);
 
 	reinforcerkit.play(ReinforcerKit::Reinforcer::SpinBlink);
+
+	mock_imukit.call_angles_ready_callback(angles);
 }
 
 TEST_F(ReinforcerkitTest, playFire)
@@ -125,6 +125,8 @@ TEST_F(ReinforcerkitTest, SetBlinkGreenAndPlayDefaultReinforcer)
 
 	reinforcerkit.setDefaultReinforcer(ReinforcerKit::Reinforcer::BlinkGreen);
 	reinforcerkit.playDefault();
+
+	mock_imukit.call_angles_ready_callback(angles);
 }
 
 TEST_F(ReinforcerkitTest, SetSpinBlinkAndPlayDefaultReinforcer)
@@ -133,6 +135,8 @@ TEST_F(ReinforcerkitTest, SetSpinBlinkAndPlayDefaultReinforcer)
 
 	reinforcerkit.setDefaultReinforcer(ReinforcerKit::Reinforcer::SpinBlink);
 	reinforcerkit.playDefault();
+
+	mock_imukit.call_angles_ready_callback(angles);
 }
 
 TEST_F(ReinforcerkitTest, SetFireAndPlayDefaultReinforcer)
diff --git a/spikes/lk_command_kit/main.cpp b/spikes/lk_command_kit/main.cpp
index 8282dfe00c..0b0061d0bb 100644
--- a/spikes/lk_command_kit/main.cpp
+++ b/spikes/lk_command_kit/main.cpp
@@ -32,6 +32,7 @@
 #include "EventLoopKit.h"
 #include "FATFileSystem.h"
 #include "HelloWorld.h"
+#include "IMUKit.hpp"
 #include "LedKit.h"
 #include "LogKit.h"
 #include "ReinforcerKit.h"
@@ -110,12 +111,11 @@ auto imukit = IMUKit {imu::lsm6dsox};
 
 namespace motion::internal {
 
-EventLoopKit event_loop {};
 CoreTimeout timeout {};
 
 }	// namespace motion::internal
 
-auto motionkit = MotionKit {motor::left, motor::right, imukit, motion::internal::event_loop, motion::internal::timeout};
+auto motionkit = MotionKit {motor::left, motor::right, imukit, motion::internal::timeout};
 
 namespace display {
 
@@ -253,7 +253,6 @@ auto main() -> int
 
 	imu::lsm6dsox.init();
 	imukit.init();
-	motionkit.init();
 
 	turnOff();
 
diff --git a/spikes/lk_motion_kit/main.cpp b/spikes/lk_motion_kit/main.cpp
index f3d92971da..5b019e169b 100644
--- a/spikes/lk_motion_kit/main.cpp
+++ b/spikes/lk_motion_kit/main.cpp
@@ -73,13 +73,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};
 
 namespace rfid {
 
@@ -96,33 +94,33 @@ void onMagicCardAvailable(const MagicCard &card)
 {
 	switch (card.getId()) {
 		case (MagicCard::number_1.getId()):
-			motionkit.rotate(1, Rotation::counterClockwise, [] { log_debug("Callback end of rotation"); });
+			motionkit.startYawRotation(1, Rotation::counterClockwise, [] { log_debug("Callback end of rotation"); });
 			break;
 		case (MagicCard::number_2.getId()):
-			motionkit.rotate(2, Rotation::clockwise);
+			motionkit.startYawRotation(2, Rotation::clockwise);
 			break;
 		case (MagicCard::number_3.getId()):
-			motionkit.rotate(3, Rotation::counterClockwise);
+			motionkit.startYawRotation(3, Rotation::counterClockwise);
 			break;
 		case (MagicCard::number_4.getId()):
-			motionkit.rotate(4, Rotation::clockwise);
+			motionkit.startYawRotation(4, Rotation::clockwise);
 			break;
 		case (MagicCard::number_5.getId()):
-			motionkit.rotate(5, Rotation::counterClockwise);
+			motionkit.startYawRotation(5, Rotation::counterClockwise);
 			break;
 		case (MagicCard::number_6.getId()):
-			motionkit.rotate(6, Rotation::clockwise);
+			motionkit.startYawRotation(6, Rotation::clockwise);
 			break;
 		case (MagicCard::number_7.getId()):
-			motionkit.rotate(7, Rotation::counterClockwise);
+			motionkit.startYawRotation(7, Rotation::counterClockwise);
 			break;
 		case (MagicCard::number_8.getId()):
-			motionkit.startStabilisation();
+			motionkit.startStabilization();
 			rtos::ThisThread::sleep_for(10s);
 			motionkit.stop();
 			break;
 		case (MagicCard::number_9.getId()):
-			motionkit.startStabilisation();
+			motionkit.startStabilization();
 			break;
 		case (MagicCard::number_10.getId()):
 			motionkit.stop();
@@ -143,7 +141,6 @@ auto main() -> int
 
 	imu::lsm6dsox.init();
 	imukit.init();
-	motionkit.init();
 	rfidkit.init();
 
 	rfidkit.onTagActivated(onMagicCardAvailable);
diff --git a/spikes/lk_reinforcer/main.cpp b/spikes/lk_reinforcer/main.cpp
index c2a0262ff1..4e6c5947df 100644
--- a/spikes/lk_reinforcer/main.cpp
+++ b/spikes/lk_reinforcer/main.cpp
@@ -150,12 +150,11 @@ auto imukit = IMUKit {imu::lsm6dsox};
 
 namespace motion::internal {
 
-	EventLoopKit event_loop {};
 	CoreTimeout timeout {};
 
 }	// namespace motion::internal
 
-auto motionkit = MotionKit {motor::left, motor::right, imukit, motion::internal::event_loop, motion::internal::timeout};
+auto motionkit = MotionKit {motor::left, motor::right, imukit, motion::internal::timeout};
 
 namespace display::internal {
 
@@ -204,7 +203,6 @@ auto main() -> int
 	videokit.initializeScreen();
 	imu::lsm6dsox.init();
 	imukit.init();
-	motionkit.init();
 
 	rtos::ThisThread::sleep_for(3s);
 
diff --git a/tests/unit/mocks/mocks/leka/IMUKit.hpp b/tests/unit/mocks/mocks/leka/IMUKit.hpp
new file mode 100644
index 0000000000..cb93b1b90f
--- /dev/null
+++ b/tests/unit/mocks/mocks/leka/IMUKit.hpp
@@ -0,0 +1,28 @@
+// Leka - LekaOS
+// Copyright 2023 APF France handicap
+// SPDX-License-Identifier: Apache-2.0
+
+#pragma once
+
+#include "gmock/gmock.h"
+#include "interface/libs/IMUKit.hpp"
+
+namespace leka::mock {
+
+class IMUKit : public interface::IMUKit
+{
+  public:
+	MOCK_METHOD(void, start, (), (override));
+	MOCK_METHOD(void, stop, (), (override));
+	MOCK_METHOD(void, setOrigin, (), (override));
+	MOCK_METHOD(EulerAngles, getEulerAngles, (), (const, override));
+
+	void onEulerAnglesReady(angles_ready_callback_t const &cb) override { angles_ready_callback = cb; }
+
+	void call_angles_ready_callback(const EulerAngles &data) { angles_ready_callback(data); }
+
+  private:
+	angles_ready_callback_t angles_ready_callback {};
+};
+
+}	// namespace leka::mock