diff --git a/spikes/lk_auto_charge/CMakeLists.txt b/spikes/lk_auto_charge/CMakeLists.txt index 457440eb70..3a0391375e 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..c24e837650 --- /dev/null +++ b/spikes/lk_auto_charge/HappyFishy.cpp @@ -0,0 +1,86 @@ +// 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; + +void HappyFishy::init() +{ + _event_loop.registerCallback([this] { run(); }); +} + +auto HappyFishy::sealConvertToPwmFrom(float angle) const -> float +{ + auto res = utils::math::map(angle, kMinAngleInput, kMaxAngleInput, kMinPwmOutput, kMaxPwmOutput); + return res; +} + +void HappyFishy::start() +{ + should_stop = false; + _event_loop.start(); +} + +void HappyFishy::stop() +{ + should_stop = true; + _event_loop.stop(); + rtos::ThisThread::sleep_for(100ms); + stopMotors(); +} + +void HappyFishy::run() +{ + if (should_stop || _battery.isCharging()) { + stopMotors(); + 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..6b7962d986 --- /dev/null +++ b/spikes/lk_auto_charge/HappyFishy.h @@ -0,0 +1,64 @@ +// 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/libs/EventLoop.h" +#include "interface/libs/IMUKit.hpp" + +namespace leka { + +class HappyFishy +{ + public: + HappyFishy(interface::EventLoop &event_loop, interface::Battery &battery, interface::Motor &motor_left, + interface::Motor &motor_right, interface::IMUKit &imu_kit) + : _event_loop(event_loop), + _battery(battery), + _motor_left(motor_left), + _motor_right(motor_right), + _imukit(imu_kit) + { + } + + ~HappyFishy() = default; + + void init(); + void start(); + void stop(); + + private: + [[nodiscard]] auto sealConvertToPwmFrom(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::Battery &_battery; + interface::Motor &_motor_left; + interface::Motor &_motor_right; + interface::IMUKit &_imukit; + + const float kMinAngleInput = 0.F; + const float kMaxAngleInput = 30.F; + const float kMinPwmOutput = 0.15F; // Min to move robot + const float kMaxPwmOutput = 0.70F; + + const float kRollTolerance = 3.F; // in degrees + const float kPitchTolerance = 3.F; // in degrees + + 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 e1a380007f..b54f48ff54 100644 --- a/spikes/lk_auto_charge/main.cpp +++ b/spikes/lk_auto_charge/main.cpp @@ -16,6 +16,7 @@ #include "CoreMotor.h" #include "CorePwm.h" #include "EventLoopKit.h" +#include "HappyFishy.h" #include "HappyToupie.h" #include "HelloWorld.h" #include "IMUKit.hpp"