diff --git a/spikes/lk_auto_charge/CMakeLists.txt b/spikes/lk_auto_charge/CMakeLists.txt index 7c531492e7..457440eb70 100644 --- a/spikes/lk_auto_charge/CMakeLists.txt +++ b/spikes/lk_auto_charge/CMakeLists.txt @@ -13,6 +13,7 @@ target_sources(spike_lk_auto_charge PRIVATE main.cpp SealStrategy.cpp + HappyToupie.cpp ) target_link_libraries(spike_lk_auto_charge diff --git a/spikes/lk_auto_charge/HappyToupie.cpp b/spikes/lk_auto_charge/HappyToupie.cpp new file mode 100644 index 0000000000..6d8879c1a2 --- /dev/null +++ b/spikes/lk_auto_charge/HappyToupie.cpp @@ -0,0 +1,80 @@ +// Leka - LekaOS +// Copyright 2023 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#include "HappyToupie.h" + +#include "rtos/ThisThread.h" + +#include "MathUtils.h" + +using namespace leka; +using namespace std::chrono; + +void HappyToupie::init() +{ + _event_loop.registerCallback([this] { run(); }); +} + +auto HappyToupie::sealConvertToPwmFrom(float angle) const -> float +{ + auto res = utils::math::map(angle, kMinAngleInput, kMaxAngleInput, kMinPwmOutput, kMaxPwmOutput); + return res; +} + +void HappyToupie::start() +{ + should_stop = false; + _event_loop.start(); +} + +void HappyToupie::stop() +{ + should_stop = true; + _event_loop.stop(); + rtos::ThisThread::sleep_for(100ms); + stopMotors(); +} + +void HappyToupie::run() +{ + if (should_stop || _battery.isCharging()) { + stopMotors(); + return; + } + + spinLeft(1.F, 1.F); + + rtos::ThisThread::sleep_for(10ms); + _event_loop.start(); +} + +void HappyToupie::stopMotors() +{ + _motor_right.stop(); + _motor_left.stop(); +} + +void HappyToupie::spinLeft(float left_speed, float right_speed) +{ + _motor_left.spin(Rotation::clockwise, left_speed); + _motor_right.spin(Rotation::clockwise, right_speed); +} + +void HappyToupie::spinRight(float left_speed, float right_speed) +{ + _motor_left.spin(Rotation::counterClockwise, left_speed); + _motor_right.spin(Rotation::counterClockwise, right_speed); +} + +void HappyToupie::moveForward(float speed) +{ + _motor_left.spin(Rotation::counterClockwise, speed); + _motor_right.spin(Rotation::clockwise, speed); +} + +void HappyToupie::moveBackward(float speed) +{ + _motor_left.spin(Rotation::clockwise, speed); + _motor_right.spin(Rotation::counterClockwise, speed); +} diff --git a/spikes/lk_auto_charge/HappyToupie.h b/spikes/lk_auto_charge/HappyToupie.h new file mode 100644 index 0000000000..d2cbd11355 --- /dev/null +++ b/spikes/lk_auto_charge/HappyToupie.h @@ -0,0 +1,62 @@ +// 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 HappyToupie +{ + public: + HappyToupie(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) + { + } + + ~HappyToupie() = 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 should_stop = true; +}; + +} // namespace leka diff --git a/spikes/lk_auto_charge/main.cpp b/spikes/lk_auto_charge/main.cpp index e05dd672d8..e1a380007f 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 "HappyToupie.h" #include "HelloWorld.h" #include "IMUKit.hpp" #include "LogKit.h"