Skip to content

Commit

Permalink
♻️ (MotionKit): Refactor MotionKit according to PID changes
Browse files Browse the repository at this point in the history
  • Loading branch information
@HPezz
HPezz committed Mar 1, 2023
1 parent 40a5f70 commit 111fb99
Show file tree
Hide file tree
Showing 2 changed files with 46 additions and 79 deletions.
40 changes: 15 additions & 25 deletions libs/MotionKit/include/MotionKit.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,9 +7,10 @@
#include <interface/drivers/Motor.h>
#include <interface/libs/EventLoop.h>

#include "PID.hpp"
#include "PIDRotation.hpp"
#include "PIDStabilization.hpp"
#include "interface/drivers/Timeout.h"
#include "interface/libs/IMUKit.h"
#include "interface/libs/IMUKit.hpp"

namespace leka {

Expand All @@ -22,47 +23,36 @@ class MotionKit
{
}

void rotate(uint8_t number_of_rotations, Rotation direction,
void rotate(float number_of_rotations, Rotation direction,
const std::function<void()> &on_rotation_ended_callback = {});
void startStabilisation();

void stop();

private:
void iterateOverRotation(const interface::EulerAngles &angles, Rotation direction = {});
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 calculateTotalYawRotation(float yaw);
void setMotorsSpeedAndDirection(float speed, Rotation direction);

interface::Motor &_motor_left;
interface::Motor &_motor_right;
interface::IMUKit &_imukit;
interface::Timeout &_timeout;
PID _pid;

float _angle_rotation_target = 0;
float _angle_rotation_sum = 0;
PIDRotation _pid_rotation;
PIDStabilization _pid_stabilization;

float _angle_rotation_target = 0.F;
float _angle_rotation_sum = 0.F;
std::function<void()> _on_rotation_ended_callback {};

bool _target_not_reached = false;
bool _stabilisation_requested = false;
bool _rotate_x_turns_requested = false;

interface::EulerAngles _euler_angles_previous
{
};

// ? Calculated with following PID parameters : ((270-180)*Kp + (360-180)*Kd)/KDeltaT
const float kEntryLimitSpeed = 3.15F;
const float minimum_viable_position_error = 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

const float kMinimalViableRobotPwm = 0.15F;
const float kPwmMaxValue = 1.F;
const float kEpsilon = 0.005F;
EulerAngles _euler_angles_previous {};
EulerAngles _target_angles {};
};

} // namespace leka
85 changes: 31 additions & 54 deletions libs/MotionKit/source/MotionKit.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,6 @@

#include "MotionKit.hpp"

#include "MathUtils.h"
#include "ThisThread.h"

using namespace leka;
Expand All @@ -16,33 +15,32 @@ void MotionKit::stop()
_motor_left.stop();
_motor_right.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<void()> &on_rotation_ended_callback)
{
stop();

_euler_angles_previous = _imukit.getEulerAngles();
_pid.setTargetYaw(_euler_angles_previous.yaw);

_target_not_reached = true;
_stabilisation_requested = false;
_rotate_x_turns_requested = true;

_angle_rotation_sum = 0;
_angle_rotation_target = static_cast<float>(number_of_rotations) * 360.F;
_angle_rotation_target = number_of_rotations * 360.F;

auto on_timeout = [this] { stop(); };

_timeout.onTimeout(on_timeout);
_timeout.start(10s);

auto on_euler_angles_rdy_callback = [this, direction](const interface::EulerAngles &euler_angles) {
iterateOverRotation(euler_angles, direction);
auto on_euler_angles_rdy_callback = [this, direction](const EulerAngles &euler_angles) {
processAngleForRotation(euler_angles, direction);
};

_imukit.onEulerAnglesReady(on_euler_angles_rdy_callback);
Expand All @@ -54,28 +52,22 @@ void MotionKit::startStabilisation()
{
stop();

_euler_angles_previous = _imukit.getEulerAngles();
_pid.setTargetYaw(_euler_angles_previous.yaw);

_target_not_reached = false;
_stabilisation_requested = true;
_rotate_x_turns_requested = false;
_target_angles = _imukit.getEulerAngles();

auto on_euler_angles_rdy_callback = [this](const interface::EulerAngles &euler_angles) {
iterateOverRotation(euler_angles);
auto on_euler_angles_rdy_callback = [this](const EulerAngles &euler_angles) {
processAngleForStabilization(euler_angles);
};

_imukit.onEulerAnglesReady(on_euler_angles_rdy_callback);
}

// LCOV_EXCL_START - Dynamic behavior, involving motors and time.
void MotionKit::iterateOverRotation(const interface::EulerAngles &angles, Rotation direction)
void MotionKit::processAngleForRotation(const EulerAngles &angles, Rotation direction)
{
auto must_stop = [&] { return !_stabilisation_requested && !_rotate_x_turns_requested && !_target_not_reached; };
auto must_stop = [&] { return !_rotate_x_turns_requested && !_target_not_reached; };

if (must_stop()) {
stop();
_imukit.onEulerAnglesReady({});

if (_on_rotation_ended_callback) {
_on_rotation_ended_callback();
Expand All @@ -84,57 +76,42 @@ void MotionKit::iterateOverRotation(const interface::EulerAngles &angles, Rotati
return;
}

auto counting_rotations = [&](auto current_yaw) {
if (auto abs_yaw_delta = std::abs(_euler_angles_previous.yaw - current_yaw); abs_yaw_delta >= 300.F) {
_angle_rotation_sum += 360.F - abs_yaw_delta;
} else {
_angle_rotation_sum += abs_yaw_delta;
}
_euler_angles_previous.yaw = current_yaw;
};

counting_rotations(angles.yaw);
calculateTotalYawRotation(angles.yaw);

if (_stabilisation_requested && _target_not_reached) {
auto [speed, rotation] = _pid.processPID(angles.pitch, angles.roll, angles.yaw);
if (_rotate_x_turns_requested && _target_not_reached) {
auto speed = _pid_rotation.processRotationAngle(_angle_rotation_target, _angle_rotation_sum);

executeSpeed(speed, rotation);
setMotorsSpeedAndDirection(speed, direction);
}
}

// Without regulation
// if (_rotate_x_turns_requested && _target_not_reached) {
// if (auto error_position_current = _angle_rotation_target - _angle_rotation_sum;
// error_position_current > minimum_viable_position_error) {
// return;
// }
// _target_not_reached = false;
// _rotate_x_turns_requested = false;
// }

// With regulation
if (_rotate_x_turns_requested && _target_not_reached) {
auto speed = _pid.processPIDByError(_angle_rotation_target - _angle_rotation_sum);
void MotionKit::processAngleForStabilization(const EulerAngles &angles)
{
auto [speed, rotation] = _pid_stabilization.processStabilizationAngle(_target_angles, angles);

executeSpeed(speed, direction);
}
setMotorsSpeedAndDirection(speed, rotation);
}

auto MotionKit::mapSpeed(float speed) const -> float
void MotionKit::calculateTotalYawRotation(float yaw)
{
return utils::math::map(speed, 0.F, kEntryLimitSpeed, kMinimalViableRobotPwm, kPwmMaxValue);
if (auto abs_yaw_delta = std::abs(_euler_angles_previous.yaw - yaw); abs_yaw_delta >= 300.F) {
_angle_rotation_sum += 360.F - abs_yaw_delta;
} else {
_angle_rotation_sum += abs_yaw_delta;
}
_euler_angles_previous.yaw = yaw;
}

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;
_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;
}
}
Expand Down

0 comments on commit 111fb99

Please sign in to comment.