diff --git a/app/os/main.cpp b/app/os/main.cpp index aa65b7270b..4c64b55ce7 100644 --- a/app/os/main.cpp +++ b/app/os/main.cpp @@ -271,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}; @@ -563,7 +561,6 @@ auto main() -> int imu::lsm6dsox.init(); imukit.init(); - motionkit.init(); robot::controller.initializeComponents(); robot::controller.registerOnUpdateLoadedCallback(firmware::setPendingUpdate); diff --git a/libs/MotionKit/include/MotionKit.hpp b/libs/MotionKit/include/MotionKit.hpp index ffb7fcc985..964b82ef78 100644 --- a/libs/MotionKit/include/MotionKit.hpp +++ b/libs/MotionKit/include/MotionKit.hpp @@ -5,65 +5,42 @@ #pragma once #include -#include -#include "IMUKit.hpp" -#include "PID.hpp" +#include "RotationControl.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, + void rotate(float number_of_rotations, Rotation direction, const std::function &on_rotation_ended_callback = {}); - void startStabilisation(); void stop(); private: - void run(); + void processAngleForRotation(const EulerAngles &angles, Rotation direction); - [[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; std::function _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/source/MotionKit.cpp b/libs/MotionKit/source/MotionKit.cpp index a9d05384ac..ca48edc05d 100644 --- a/libs/MotionKit/source/MotionKit.cpp +++ b/libs/MotionKit/source/MotionKit.cpp @@ -4,127 +4,80 @@ #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, +void MotionKit::rotate(float number_of_rotations, Rotation direction, const std::function &on_rotation_ended_callback) { stop(); - _imukit.start(); - _imukit.setOrigin(); + auto starting_angle = _imukit.getEulerAngles(); + _rotation_control.init(starting_angle, number_of_rotations); _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(); - - _on_rotation_ended_callback = on_rotation_ended_callback; -} - -void MotionKit::startStabilisation() -{ - stop(); + auto on_euler_angles_rdy_callback = [this, direction](const EulerAngles &euler_angles) { + processAngleForRotation(euler_angles, direction); + }; - _imukit.start(); - _imukit.setOrigin(); + _imukit.onEulerAnglesReady(on_euler_angles_rdy_callback); - _target_not_reached = false; - _stabilisation_requested = true; - _rotate_x_turns_requested = false; - - _event_loop.start(); + _on_rotation_ended_callback = on_rotation_ended_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 -{ - return utils::math::map(speed, 0.F, kPIDMaxValue, kMinimalViableRobotPwm, kPwmMaxValue); } -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/tests/MotionKit_test.cpp b/libs/MotionKit/tests/MotionKit_test.cpp index 878cb65299..22be6768cd 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 accel_data = {0.F, 0.F, 0.F}; - std::tuple 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 {}; - 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)); - 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 {}; - 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.rotate(1, Rotation::counterClockwise); + + mock_imukit.call_angles_ready_callback(angles); } TEST_F(MotionKitTest, rotateAndStop) { - auto mock_function_motion = MockFunction {}; - 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); + EXPECT_CALL(mock_motor_left, spin(Rotation::clockwise, _)).Times(AtLeast(1)); + EXPECT_CALL(mock_motor_right, spin(Rotation::clockwise, _)).Times(AtLeast(1)); + motion.rotate(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 {}; - 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(&on_timeout_callback)); EXPECT_CALL(mock_timeout, start).Times(1); - stub_event_loop_motion.registerCallback(loop_motion); + EXPECT_CALL(mock_motor_left, spin(Rotation::clockwise, _)).Times(AtLeast(1)); + EXPECT_CALL(mock_motor_right, spin(Rotation::clockwise, _)).Times(AtLeast(1)); + motion.rotate(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); @@ -148,22 +134,3 @@ TEST_F(MotionKitTest, rotateAndTimeOutOver) on_timeout_callback(); } - -TEST_F(MotionKitTest, startStabilisationAndStop) -{ - auto mock_function_motion = MockFunction {}; - 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); - - stub_event_loop_motion.registerCallback(loop_motion); - motion.startStabilisation(); -} diff --git a/libs/ReinforcerKit/tests/ReinforcerKit_test.cpp b/libs/ReinforcerKit/tests/ReinforcerKit_test.cpp index 6559cba720..c2dfc86500 100644 --- a/libs/ReinforcerKit/tests/ReinforcerKit_test.cpp +++ b/libs/ReinforcerKit/tests/ReinforcerKit_test.cpp @@ -9,7 +9,7 @@ #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" @@ -17,9 +17,8 @@ using namespace leka; -using ::testing::AnyNumber; +using ::testing::AtLeast; using ::testing::AtMost; -using ::testing::Sequence; MATCHER_P(isSameAnimation, expected_animation, "") { @@ -39,30 +38,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 +82,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 +91,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 +126,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 +136,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 248a1acc0b..0b0061d0bb 100644 --- a/spikes/lk_command_kit/main.cpp +++ b/spikes/lk_command_kit/main.cpp @@ -111,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 { @@ -254,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..665d98f685 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 { @@ -116,14 +114,6 @@ void onMagicCardAvailable(const MagicCard &card) case (MagicCard::number_7.getId()): motionkit.rotate(7, Rotation::counterClockwise); break; - case (MagicCard::number_8.getId()): - motionkit.startStabilisation(); - rtos::ThisThread::sleep_for(10s); - motionkit.stop(); - break; - case (MagicCard::number_9.getId()): - motionkit.startStabilisation(); - break; case (MagicCard::number_10.getId()): motionkit.stop(); break; @@ -143,7 +133,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);