diff --git a/spikes/lk_auto_charge/CMakeLists.txt b/spikes/lk_auto_charge/CMakeLists.txt index 3a0391375e..314d68610b 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 + SigmoidStrategy.cpp HappyToupie.cpp HappyFishy.cpp ) diff --git a/spikes/lk_auto_charge/SigmoidStrategy.cpp b/spikes/lk_auto_charge/SigmoidStrategy.cpp new file mode 100644 index 0000000000..0e39249ecf --- /dev/null +++ b/spikes/lk_auto_charge/SigmoidStrategy.cpp @@ -0,0 +1,151 @@ +// Leka - LekaOS +// Copyright 2023 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#include "SigmoidStrategy.h" + +#include "rtos/ThisThread.h" + +#include "LogKit.h" +#include "MathUtils.h" + +using namespace leka; +using namespace std::chrono; + +void SigmoidStrategy::init() +{ + _event_loop.registerCallback([this] { run(); }); +} + +auto SigmoidStrategy::sealConvertToPwmFrom(float angle) const -> float +{ + // auto res = utils::math::map(angle, kMinAngleInput, kMaxAngleInput, kMinPwmOutput, kMaxPwmOutput); + return 0.F; +} + +void SigmoidStrategy::start() +{ + auto angles = _imukit.getEulerAngles(); + + mid_point = angles.roll > 0 ? angles.roll / 2 : -angles.roll / 2; + right_movement = angles.roll > 0; + forward_movement = angles.pitch > 0; + + should_stop = false; + _event_loop.start(); +} + +void SigmoidStrategy::stop() +{ + should_stop = true; + _event_loop.stop(); + rtos::ThisThread::sleep_for(100ms); + stopMotors(); +} + +void SigmoidStrategy::run() +{ + if (should_stop || _battery.isCharging()) { + stopMotors(); + return; + } + + auto angles = _imukit.getEulerAngles(); + log_info("run"); + + auto abs_angle_roll = angles.roll > 0 ? angles.roll : -angles.roll; + if (abs_angle_roll > kRollTolerance) { + if (abs_angle_roll > mid_point) { + log_info("premier virage"); + if (right_movement) { + log_info("sur la droite"); + if (forward_movement) { + log_info("vers l'avant"); + spinLeft(kMinPwmOutput, 0.1f); + } else { + log_info("vers l'arrière"); + spinRight(kMinPwmOutput, 0.1f); + } + } else { + log_info("sur la gauche"); + if (forward_movement) { + log_info("vers l'avant"); + spinRight(0.1f, kMinPwmOutput); + } else { + log_info("vers l'arrière"); + spinLeft(0.1f, kMinPwmOutput); + } + } + } else { + log_info("second virage"); + if (right_movement) { + log_info("sur la droite"); + if (forward_movement) { + log_info("vers l'avant"); + // spinLeft(kMinPwmOutput, 0.1f); + spinRight(0.1f, kMinPwmOutput); + } else { + log_info("vers l'arrière"); + // spinRight(kMinPwmOutput, 0.1f); + spinLeft(0.1f, kMinPwmOutput); + } + } else { + log_info("sur la gauche"); + if (forward_movement) { + log_info("vers l'avant"); + // spinRight(0.1f, kMinPwmOutput); + spinLeft(kMinPwmOutput, 0.1f); + } else { + log_info("vers l'arrière"); + // spinLeft(0.1f, kMinPwmOutput); + spinRight(kMinPwmOutput, 0.1f); + } + } + } + } else if (abs(angles.pitch) > kPitchTolerance) { + auto should_move_forward = angles.pitch > 0; + auto speed = sealConvertToPwmFrom(angles.pitch > 0 ? angles.pitch : -angles.pitch); + if (should_move_forward) { + moveForward(speed); + } else { + moveBackward(speed); + } + } else { + stopMotors(); + } + + log_info("=================="); + + rtos::ThisThread::sleep_for(500ms); + _event_loop.start(); +} + +void SigmoidStrategy::stopMotors() +{ + _motor_right.stop(); + _motor_left.stop(); +} + +void SigmoidStrategy::spinLeft(float left_speed, float right_speed) +{ + _motor_left.spin(Rotation::clockwise, left_speed); + _motor_right.spin(Rotation::clockwise, right_speed); +} + +void SigmoidStrategy::spinRight(float left_speed, float right_speed) +{ + _motor_left.spin(Rotation::counterClockwise, left_speed); + _motor_right.spin(Rotation::counterClockwise, right_speed); +} + +void SigmoidStrategy::moveForward(float speed) +{ + _motor_left.spin(Rotation::counterClockwise, speed); + _motor_right.spin(Rotation::clockwise, speed); +} + +void SigmoidStrategy::moveBackward(float speed) +{ + _motor_left.spin(Rotation::clockwise, speed); + _motor_right.spin(Rotation::counterClockwise, speed); +} diff --git a/spikes/lk_auto_charge/SigmoidStrategy.h b/spikes/lk_auto_charge/SigmoidStrategy.h new file mode 100644 index 0000000000..8d0d156a47 --- /dev/null +++ b/spikes/lk_auto_charge/SigmoidStrategy.h @@ -0,0 +1,66 @@ +// 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 SigmoidStrategy +{ + public: + SigmoidStrategy(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) + { + } + + ~SigmoidStrategy() = 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.4F; // Min to move robot + const float kMaxPwmOutput = 0.70F; + + const float kRollTolerance = 3.F; // in degrees + const float kPitchTolerance = 3.F; // in degrees + + float mid_point = 0.F; + bool right_movement = false; + bool forward_movement = false; + + bool should_stop = true; +}; + +} // namespace leka diff --git a/spikes/lk_auto_charge/main.cpp b/spikes/lk_auto_charge/main.cpp index b54f48ff54..70c492d29d 100644 --- a/spikes/lk_auto_charge/main.cpp +++ b/spikes/lk_auto_charge/main.cpp @@ -23,6 +23,7 @@ #include "LogKit.h" #include "MathUtils.h" #include "SealStrategy.h" +#include "SigmoidStrategy.h" using namespace std::chrono; using namespace leka; @@ -123,7 +124,7 @@ auto services = std::to_array({&service_file_excha auto blekit = BLEKit {}; auto should_stop = true; -auto sealStrategy = SealStrategy {event_loop, battery::cells, motors::left::motor, motors::right::motor, imukit}; +auto sealStrategy = HappyFishy {event_loop, battery::cells, motors::left::motor, motors::right::motor, imukit}; auto main() -> int {