✨ (MotionKit): Add MotionKit lib

Lib using IMUKit angle computation to get angle control on reinforcer, responsive movement, etc...

libs/MotionKit/CMakeLists.txt

@@ -11,14 +11,18 @@ target_include_directories(MotionKit
 
 target_sources(MotionKit
 	PRIVATE
+	source/MotionKit.cpp
 	source/PID.cpp
 )
 
 target_link_libraries(MotionKit
+	CoreIMU
+	IMUKit
 )
 
 if(${CMAKE_PROJECT_NAME} STREQUAL "LekaOSUnitTests")
 	leka_unit_tests_sources(
+		tests/MotionKit_test.cpp
 		tests/PID_test.cpp
 	)
 endif()

libs/MotionKit/include/MotionKit.h

@@ -0,0 +1,53 @@
+// Leka - LekaOS
+// Copyright 2022 APF France handicap
+// SPDX-License-Identifier: Apache-2.0
+
+#pragma once
+
+#include <interface/drivers/Motor.h>
+
+#include "IMUKit.h"
+#include "PID.h"
+#include "interface/drivers/Timeout.h"
+
+namespace leka {
+
+class MotionKit
+{
+  public:
+	MotionKit(interface::Motor &motor_left, interface::Motor &motor_right, IMUKit &imu_kit,
+			  interface::EventLoop &event_loop)
+		: _motor_left(motor_left), _motor_right(motor_right), _imukit(imu_kit), _event_loop(event_loop) {};
+
+	void init();
+
+	void rotate(uint8_t number_of_rotations, Rotation direction);
+	void startStabilisation();
+
+	void stop();
+
+  private:
+	void run();
+
+	[[nodiscard]] auto mapSpeed(float speed) const -> float;
+	void executeSpeed(float speed, Rotation direction);
+
+	interface::Motor &_motor_left;
+	interface::Motor &_motor_right;
+	IMUKit &_imukit;
+	interface::EventLoop &_event_loop;
+	PID _pid;
+
+	uint8_t _rotations_to_execute = 0;
+
+	bool _target_not_reached	   = false;
+	bool _stabilisation_requested  = false;
+	bool _rotate_x_turns_requested = false;
+
+	const float kReferenceAngle		   = 180.F;
+	const float kMinimalViableRobotPwm = 0.25F;	  // ? Under this pwm value, torque is too low to spin the wheel
+	const float kPwmMaxValue		   = 1.F;
+	const float kPwmMarginLimit		   = 0.28F;
+};
+
+}	// namespace leka

libs/MotionKit/source/MotionKit.cpp

@@ -0,0 +1,117 @@
+// Leka - LekaOS
+// Copyright 2022 APF France handicap
+// SPDX-License-Identifier: Apache-2.0
+
+#include "MotionKit.h"
+
+#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()
+{
+	_motor_left.stop();
+	_motor_right.stop();
+	_event_loop.stop();
+
+	_stabilisation_requested  = false;
+	_target_not_reached		  = false;
+	_rotate_x_turns_requested = false;
+}
+
+void MotionKit::rotate(uint8_t number_of_rotations, Rotation direction)
+{
+	stop();
+
+	_imukit.start();
+	_imukit.reset();
+
+	_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);
+
+	_event_loop.start();
+}
+
+void MotionKit::startStabilisation()
+{
+	stop();
+
+	_imukit.start();
+	_imukit.reset();
+
+	_target_not_reached		  = false;
+	_stabilisation_requested  = true;
+	_rotate_x_turns_requested = false;
+
+	_event_loop.start();
+}
+
+// LCOV_EXCL_START - Dynamic behavior, involving motors and time.
+void MotionKit::run()
+{
+	auto last_yaw			= kReferenceAngle;
+	auto rotations_executed = 0;
+
+	auto must_rotate = [&] { return _rotate_x_turns_requested && rotations_executed != _rotations_to_execute; };
+
+	auto check_complete_rotations_executed = [&](auto last_yaw, auto current_yaw) {
+		if (std::abs(last_yaw - current_yaw) >= 300.F) {
+			++rotations_executed;
+		}
+	};
+
+	while (must_rotate()) {
+		auto [current_pitch, current_roll, current_yaw] = _imukit.getAngles();
+
+		check_complete_rotations_executed(last_yaw, current_yaw);
+
+		rtos::ThisThread::sleep_for(70ms);
+		last_yaw = current_yaw;
+	}
+
+	_rotate_x_turns_requested = false;
+	_rotations_to_execute	  = 0;
+
+	while (_stabilisation_requested || _target_not_reached) {
+		auto [pitch, roll, yaw] = _imukit.getAngles();
+		auto [speed, rotation]	= _pid.processPID(pitch, roll, yaw);
+
+		executeSpeed(speed, rotation);
+
+		rtos::ThisThread::sleep_for(70ms);
+	}
+	_imukit.stop();
+}
+
+auto MotionKit::mapSpeed(float speed) const -> float
+{
+	return utils::math::map(speed, 0.F, 1.F, kMinimalViableRobotPwm, kPwmMaxValue);
+}
+
+void MotionKit::executeSpeed(float speed, Rotation direction)
+{
+	auto speed_bounded = mapSpeed(speed);
+	if (speed_bounded <= kPwmMarginLimit) {
+		_motor_left.stop();
+		_motor_right.stop();
+		_target_not_reached = false;
+	} else {
+		_motor_left.spin(direction, speed_bounded);
+		_motor_right.spin(direction, speed_bounded);
+		_target_not_reached = true;
+	}
+}
+// LCOV_EXCL_STOP

libs/MotionKit/tests/MotionKit_test.cpp

@@ -0,0 +1,127 @@
+// Leka - LekaOS
+// Copyright 2022 APF France handicap
+// SPDX-License-Identifier: Apache-2.0
+
+#include "MotionKit.h"
+
+#include "IMUKit.h"
+#include "gtest/gtest.h"
+#include "mocks/leka/Accelerometer.h"
+#include "mocks/leka/CoreMotor.h"
+#include "mocks/leka/Gyroscope.h"
+#include "stubs/leka/EventLoopKit.h"
+
+using namespace leka;
+
+using ::testing::MockFunction;
+
+class MotionKitTest : public ::testing::Test
+{
+  protected:
+	MotionKitTest() = default;
+
+	void SetUp() override
+	{
+		imukit.init();
+		motion.init();
+	}
+	// void TearDown() override {}
+
+	stub::EventLoopKit stub_event_loop_imu {};
+	stub::EventLoopKit stub_event_loop_motion {};
+
+	mock::CoreMotor mock_motor_left {};
+	mock::CoreMotor mock_motor_right {};
+
+	mock::Accelerometer accel {};
+	mock::Gyroscope gyro {};
+
+	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};
+
+	IMUKit imukit {stub_event_loop_imu, accel, gyro};
+
+	MotionKit motion {mock_motor_left, mock_motor_right, imukit, stub_event_loop_motion};
+};
+
+TEST_F(MotionKitTest, initialization)
+{
+	ASSERT_NE(&motion, nullptr);
+}
+
+TEST_F(MotionKitTest, registerMockCallbackAndControlledSpin)
+{
+	auto mock_function_imu = MockFunction<void(void)> {};
+	auto loop_imu		   = [&] { mock_function_imu.Call(); };
+
+	auto mock_function_motion = MockFunction<void(void)> {};
+	auto loop_motion		  = [&] { mock_function_motion.Call(); };
+
+	EXPECT_CALL(mock_function_imu, Call()).Times(1);
+	EXPECT_CALL(mock_function_motion, Call()).Times(1);
+
+	EXPECT_CALL(mock_motor_left, stop).Times(1);
+	EXPECT_CALL(mock_motor_right, stop).Times(1);
+
+	EXPECT_CALL(mock_motor_left, spin(Rotation::clockwise, 1)).Times(1);
+	EXPECT_CALL(mock_motor_right, spin(Rotation::clockwise, 1)).Times(1);
+
+	stub_event_loop_imu.registerCallback(loop_imu);
+	stub_event_loop_motion.registerCallback(loop_motion);
+	motion.rotate(1, Rotation::clockwise);
+}
+
+TEST_F(MotionKitTest, registerMockCallbackAndResponsive)
+{
+	auto mock_function_imu = MockFunction<void(void)> {};
+	auto loop_imu		   = [&] { mock_function_imu.Call(); };
+
+	auto mock_function_motion = MockFunction<void(void)> {};
+	auto loop_motion		  = [&] { mock_function_motion.Call(); };
+
+	EXPECT_CALL(mock_function_imu, Call()).Times(1);
+	EXPECT_CALL(mock_function_motion, Call()).Times(1);
+
+	EXPECT_CALL(mock_motor_left, stop).Times(1);
+	EXPECT_CALL(mock_motor_right, stop).Times(1);
+
+	stub_event_loop_imu.registerCallback(loop_imu);
+	stub_event_loop_motion.registerCallback(loop_motion);
+	motion.startStabilisation();
+}
+
+TEST_F(MotionKitTest, controlledSpinAndstop)
+{
+	auto mock_function_imu = MockFunction<void(void)> {};
+	auto loop_imu		   = [&] { mock_function_imu.Call(); };
+
+	auto mock_function_motion = MockFunction<void(void)> {};
+	auto loop_motion		  = [&] {
+		 mock_function_motion.Call();
+		 motion.stop();
+	};
+
+	EXPECT_CALL(mock_function_motion, Call()).Times(1);
+
+	stub_event_loop_imu.registerCallback(loop_imu);
+	stub_event_loop_motion.registerCallback(loop_motion);
+	motion.rotate(1, Rotation::clockwise);
+}
+
+TEST_F(MotionKitTest, ResponsiveAndstop)
+{
+	auto mock_function_imu = MockFunction<void(void)> {};
+	auto loop_imu		   = [&] { mock_function_imu.Call(); };
+
+	auto mock_function_motion = MockFunction<void(void)> {};
+	auto loop_motion		  = [&] {
+		 mock_function_motion.Call();
+		 motion.stop();
+	};
+
+	EXPECT_CALL(mock_function_motion, Call()).Times(1);
+
+	stub_event_loop_imu.registerCallback(loop_imu);
+	stub_event_loop_motion.registerCallback(loop_motion);
+	motion.startStabilisation();
+}