From 14e8859d2d3405c53e639e0dfd424466d90c1f0b Mon Sep 17 00:00:00 2001 From: Hugo Pezziardi Date: Mon, 6 Mar 2023 10:21:59 +0100 Subject: [PATCH] :sparkles: (RotationControl): Replace PID by ControlRotation --- libs/MotionKit/CMakeLists.txt | 4 +- libs/MotionKit/include/PID.hpp | 44 -------- libs/MotionKit/include/RotationControl.hpp | 57 ++++++++++ libs/MotionKit/source/PID.cpp | 42 ------- libs/MotionKit/source/RotationControl.cpp | 63 +++++++++++ libs/MotionKit/tests/PID_test.cpp | 87 --------------- libs/MotionKit/tests/RotationControl_test.cpp | 105 ++++++++++++++++++ 7 files changed, 227 insertions(+), 175 deletions(-) delete mode 100644 libs/MotionKit/include/PID.hpp create mode 100644 libs/MotionKit/include/RotationControl.hpp delete mode 100644 libs/MotionKit/source/PID.cpp create mode 100644 libs/MotionKit/source/RotationControl.cpp delete mode 100644 libs/MotionKit/tests/PID_test.cpp create mode 100644 libs/MotionKit/tests/RotationControl_test.cpp diff --git a/libs/MotionKit/CMakeLists.txt b/libs/MotionKit/CMakeLists.txt index 50a3eaad3c..3ced4c564f 100644 --- a/libs/MotionKit/CMakeLists.txt +++ b/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() 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 - -#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; - - 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/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 - -using namespace leka; - -auto PID::processPID([[maybe_unused]] float pitch, [[maybe_unused]] float roll, float yaw) - -> std::tuple -{ - 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 + +#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/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(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(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(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(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(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(i)}; + + current_speed = rotation_control.processRotationAngle(current_angle); + + log_debug("3."); + + EXPECT_EQ(current_speed, 0.F); + } +}