From dc99b43812e536861fb1a3b6058c701c63343e5d Mon Sep 17 00:00:00 2001 From: Yann Locatelli Date: Tue, 12 Dec 2023 17:21:38 +0100 Subject: [PATCH] :alembic: (spike): Autocharge - HappyFishy An original suggestion from @HPezz Co-Authored-By: Hugo Pezziardi <84374761+HPezz@users.noreply.github.com> --- spikes/lk_auto_charge/CMakeLists.txt | 1 + spikes/lk_auto_charge/HappyFishy.cpp | 88 ++++++++++++++++++++++++++++ spikes/lk_auto_charge/HappyFishy.h | 60 +++++++++++++++++++ spikes/lk_auto_charge/main.cpp | 6 ++ 4 files changed, 155 insertions(+) create mode 100644 spikes/lk_auto_charge/HappyFishy.cpp create mode 100644 spikes/lk_auto_charge/HappyFishy.h diff --git a/spikes/lk_auto_charge/CMakeLists.txt b/spikes/lk_auto_charge/CMakeLists.txt index f942443be6..3b7d6a4be4 100644 --- a/spikes/lk_auto_charge/CMakeLists.txt +++ b/spikes/lk_auto_charge/CMakeLists.txt @@ -14,6 +14,7 @@ target_sources(spike_lk_auto_charge main.cpp SealStrategy.cpp HappyToupie.cpp + HappyFishy.cpp ) target_link_libraries(spike_lk_auto_charge diff --git a/spikes/lk_auto_charge/HappyFishy.cpp b/spikes/lk_auto_charge/HappyFishy.cpp new file mode 100644 index 0000000000..933648b5c0 --- /dev/null +++ b/spikes/lk_auto_charge/HappyFishy.cpp @@ -0,0 +1,88 @@ +// Leka - LekaOS +// Copyright 2023 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#include "HappyFishy.h" + +#include "rtos/ThisThread.h" + +#include "MathUtils.h" + +using namespace leka; +using namespace std::chrono; + +auto HappyFishy::convertToPwmFrom(float angle) const -> float +{ + auto res = utils::math::map(angle, kMinAngleInput, kMaxAngleInput, kMinPwmOutput, kMaxPwmOutput); + return res; +} + +void HappyFishy::start() +{ + auto on_timeout = [this] { stop(); }; + _timeout.onTimeout(on_timeout); + _timeout.start(20s); + + should_stop = false; + + _event_loop.registerCallback([this] { run(); }); + _event_loop.start(); +} + +void HappyFishy::stop() +{ + should_stop = true; + _event_loop.stop(); + rtos::ThisThread::sleep_for(100ms); + stopMotors(); + _timeout.stop(); +} + +void HappyFishy::run() +{ + if (should_stop || _battery.isCharging()) { + stop(); + return; + } + + if (move_left) { + spinLeft(0.25F, 0.25F); + rtos::ThisThread::sleep_for(300ms); + } else { + spinRight(0.25F, 0.25F); + rtos::ThisThread::sleep_for(320ms); + } + move_left = !move_left; + + _event_loop.start(); +} + +void HappyFishy::stopMotors() +{ + _motor_right.stop(); + _motor_left.stop(); +} + +void HappyFishy::spinLeft(float left_speed, float right_speed) +{ + _motor_left.spin(Rotation::clockwise, left_speed); + _motor_right.spin(Rotation::clockwise, right_speed); +} + +void HappyFishy::spinRight(float left_speed, float right_speed) +{ + _motor_left.spin(Rotation::counterClockwise, left_speed); + _motor_right.spin(Rotation::counterClockwise, right_speed); +} + +void HappyFishy::moveForward(float speed) +{ + _motor_left.spin(Rotation::counterClockwise, speed); + _motor_right.spin(Rotation::clockwise, speed); +} + +void HappyFishy::moveBackward(float speed) +{ + _motor_left.spin(Rotation::clockwise, speed); + _motor_right.spin(Rotation::counterClockwise, speed); +} diff --git a/spikes/lk_auto_charge/HappyFishy.h b/spikes/lk_auto_charge/HappyFishy.h new file mode 100644 index 0000000000..7ab1329799 --- /dev/null +++ b/spikes/lk_auto_charge/HappyFishy.h @@ -0,0 +1,60 @@ +// Leka - LekaOS +// Copyright 2023 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "interface/drivers/Battery.h" +#include "interface/drivers/Motor.h" +#include "interface/drivers/Timeout.h" +#include "interface/libs/EventLoop.h" + +namespace leka { + +class HappyFishy +{ + public: + HappyFishy(interface::EventLoop &event_loop, interface::Timeout &timeout, interface::Battery &battery, + interface::Motor &motor_left, interface::Motor &motor_right) + : _event_loop(event_loop), + _timeout(timeout), + _battery(battery), + _motor_left(motor_left), + _motor_right(motor_right) + { + } + + ~HappyFishy() = default; + + void start(); + void stop(); + + private: + [[nodiscard]] auto convertToPwmFrom(float angle) const -> float; + + void run(); + + void stopMotors(); + void spinRight(float left_speed, float right_speed); + void spinLeft(float left_speed, float right_speed); + void moveForward(float speed); + void moveBackward(float speed); + + interface::EventLoop &_event_loop; + interface::Timeout &_timeout; + + interface::Battery &_battery; + interface::Motor &_motor_left; + interface::Motor &_motor_right; + + const float kMinAngleInput = 0.F; + const float kMaxAngleInput = 30.F; + const float kMinPwmOutput = 0.15F; // Min to move robot + const float kMaxPwmOutput = 0.70F; + + bool move_left = true; + + bool should_stop = true; +}; + +} // namespace leka diff --git a/spikes/lk_auto_charge/main.cpp b/spikes/lk_auto_charge/main.cpp index 0ebad93e77..b291afa727 100644 --- a/spikes/lk_auto_charge/main.cpp +++ b/spikes/lk_auto_charge/main.cpp @@ -17,6 +17,7 @@ #include "CorePwm.h" #include "CoreTimeout.h" #include "EventLoopKit.h" +#include "HappyFishy.h" #include "HappyToupie.h" #include "HelloWorld.h" #include "IMUKit.hpp" @@ -102,6 +103,7 @@ auto blekit = BLEKit {}; auto seal_strategy = SealStrategy {event_loop, timeout, battery::cells, motors::left::motor, motors::right::motor, imukit}; auto happy_toupie = HappyToupie {event_loop, timeout, battery::cells, motors::left::motor, motors::right::motor}; +auto happy_fishy = HappyFishy {event_loop, timeout, battery::cells, motors::left::motor, motors::right::motor}; auto last_strategy = uint8_t {0x00}; @@ -113,9 +115,13 @@ void runStrategy(uint8_t id) } else if (id == 0x02) { happy_toupie.start(); last_strategy = id; + } else if (id == 0x03) { + happy_fishy.start(); + last_strategy = id; } else { seal_strategy.stop(); happy_toupie.stop(); + happy_fishy.stop(); } }