Skip to content

Commit

Permalink
♻️ (MotionKit): Refactor with ControlRotation and IMUKit changes
Browse files Browse the repository at this point in the history
  • Loading branch information
@HPezz
HPezz committed Mar 6, 2023
1 parent 700151a commit 9bd7ea9
Show file tree
Hide file tree
Showing 8 changed files with 105 additions and 218 deletions.
5 changes: 1 addition & 4 deletions app/os/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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};
Expand Down Expand Up @@ -563,7 +561,6 @@ auto main() -> int

imu::lsm6dsox.init();
imukit.init();
motionkit.init();

robot::controller.initializeComponents();
robot::controller.registerOnUpdateLoadedCallback(firmware::setPendingUpdate);
Expand Down
46 changes: 13 additions & 33 deletions libs/MotionKit/include/MotionKit.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,65 +5,45 @@
#pragma once

#include <interface/drivers/Motor.h>
#include <interface/libs/EventLoop.h>

#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<void()> &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;

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;

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
99 changes: 26 additions & 73 deletions libs/MotionKit/source/MotionKit.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<void()> &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;
}
}
Expand Down
Loading

0 comments on commit 9bd7ea9

Please sign in to comment.