From 8854a44f6d627203470e3d3babf7862b09d77fd1 Mon Sep 17 00:00:00 2001 From: Yann Locatelli Date: Wed, 22 Feb 2023 19:54:53 +0100 Subject: [PATCH 1/4] :alembic: (spike): Add autocharge of robot seal strategy --- spikes/CMakeLists.txt | 2 + spikes/lk_auto_charge/CMakeLists.txt | 31 ++++++ spikes/lk_auto_charge/SealStrategy.cpp | 112 ++++++++++++++++++++ spikes/lk_auto_charge/SealStrategy.h | 64 +++++++++++ spikes/lk_auto_charge/main.cpp | 140 +++++++++++++++++++++++++ 5 files changed, 349 insertions(+) create mode 100644 spikes/lk_auto_charge/CMakeLists.txt create mode 100644 spikes/lk_auto_charge/SealStrategy.cpp create mode 100644 spikes/lk_auto_charge/SealStrategy.h create mode 100644 spikes/lk_auto_charge/main.cpp diff --git a/spikes/CMakeLists.txt b/spikes/CMakeLists.txt index cc3bd94479..d9dabd86ad 100644 --- a/spikes/CMakeLists.txt +++ b/spikes/CMakeLists.txt @@ -4,6 +4,7 @@ add_subdirectory(${SPIKES_DIR}/lk_activity_kit) add_subdirectory(${SPIKES_DIR}/lk_audio) +add_subdirectory(${SPIKES_DIR}/lk_auto_charge) add_subdirectory(${SPIKES_DIR}/lk_behavior_kit) add_subdirectory(${SPIKES_DIR}/lk_ble) add_subdirectory(${SPIKES_DIR}/lk_bluetooth) @@ -50,6 +51,7 @@ add_subdirectory(${SPIKES_DIR}/stl_cxxsupport) add_custom_target(spikes_leka) add_dependencies(spikes_leka spike_lk_activity_kit + spike_lk_auto_charge spike_lk_ble spike_lk_bluetooth spike_lk_cg_animations diff --git a/spikes/lk_auto_charge/CMakeLists.txt b/spikes/lk_auto_charge/CMakeLists.txt new file mode 100644 index 0000000000..af7d115332 --- /dev/null +++ b/spikes/lk_auto_charge/CMakeLists.txt @@ -0,0 +1,31 @@ +# Leka - LekaOS +# Copyright 2023 APF France handicap +# SPDX-License-Identifier: Apache-2.0 + +add_mbed_executable(spike_lk_auto_charge) + +target_include_directories(spike_lk_auto_charge + PRIVATE + . +) + +target_sources(spike_lk_auto_charge + PRIVATE + main.cpp + SealStrategy.cpp +) + +target_link_libraries(spike_lk_auto_charge + EventLoopKit + CoreTimeout + CoreBattery + BatteryKit + CorePwm + CoreMotor + CoreIMU + CoreI2C + IMUKit + BLEKit +) + +target_link_custom_leka_targets(spike_lk_auto_charge) diff --git a/spikes/lk_auto_charge/SealStrategy.cpp b/spikes/lk_auto_charge/SealStrategy.cpp new file mode 100644 index 0000000000..c766371440 --- /dev/null +++ b/spikes/lk_auto_charge/SealStrategy.cpp @@ -0,0 +1,112 @@ +// Leka - LekaOS +// Copyright 2023 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#include "SealStrategy.h" + +#include "rtos/ThisThread.h" + +#include "MathUtils.h" + +using namespace leka; +using namespace std::chrono; + +auto SealStrategy::convertToPwmFrom(float angle) const -> float +{ + auto res = utils::math::map(angle, kMinAngleInput, kMaxAngleInput, kMinPwmOutput, kMaxPwmOutput); + return res; +} + +void SealStrategy::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 SealStrategy::stop() +{ + should_stop = true; + _event_loop.stop(); + rtos::ThisThread::sleep_for(100ms); + stopMotors(); + _timeout.stop(); +} + +void SealStrategy::run() +{ + if (should_stop || _battery.isCharging()) { + stop(); + return; + } + + auto angles = _imukit.getEulerAngles(); + auto is_right_tilted = angles.roll > 0; + auto should_move_forward = angles.pitch > 0; + + auto abs_float = [](float value) { return value > 0 ? value : -value; }; + + if (abs_float(angles.roll) > kRollTolerance) { + auto speed_offset = convertToPwmFrom(angles.roll > 0 ? angles.roll : -angles.roll); + if (is_right_tilted) { + if (should_move_forward) { + spinRight(kMinPwmOutput, kMinPwmOutput + speed_offset); + } else { + spinLeft(kMinPwmOutput, kMinPwmOutput + speed_offset); + } + } else { + if (should_move_forward) { + spinLeft(kMinPwmOutput + speed_offset, kMinPwmOutput); + } else { + spinRight(kMinPwmOutput + speed_offset, kMinPwmOutput); + } + } + } else if (abs_float(angles.pitch) > kPitchTolerance) { + auto speed = convertToPwmFrom(angles.pitch > 0 ? angles.pitch : -angles.pitch); + if (should_move_forward) { + moveForward(speed); + } else { + moveBackward(speed); + } + } else { + stopMotors(); + } + + rtos::ThisThread::sleep_for(10ms); + _event_loop.start(); +} + +void SealStrategy::stopMotors() +{ + _motor_right.stop(); + _motor_left.stop(); +} + +void SealStrategy::spinLeft(float left_speed, float right_speed) +{ + _motor_left.spin(Rotation::clockwise, left_speed); + _motor_right.spin(Rotation::clockwise, right_speed); +} + +void SealStrategy::spinRight(float left_speed, float right_speed) +{ + _motor_left.spin(Rotation::counterClockwise, left_speed); + _motor_right.spin(Rotation::counterClockwise, right_speed); +} + +void SealStrategy::moveForward(float speed) +{ + _motor_left.spin(Rotation::counterClockwise, speed); + _motor_right.spin(Rotation::clockwise, speed); +} + +void SealStrategy::moveBackward(float speed) +{ + _motor_left.spin(Rotation::clockwise, speed); + _motor_right.spin(Rotation::counterClockwise, speed); +} diff --git a/spikes/lk_auto_charge/SealStrategy.h b/spikes/lk_auto_charge/SealStrategy.h new file mode 100644 index 0000000000..469facdaaf --- /dev/null +++ b/spikes/lk_auto_charge/SealStrategy.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/drivers/Timeout.h" +#include "interface/libs/EventLoop.h" +#include "interface/libs/IMUKit.hpp" + +namespace leka { + +class SealStrategy +{ + public: + SealStrategy(interface::EventLoop &event_loop, interface::Timeout &timeout, interface::Battery &battery, + interface::Motor &motor_left, interface::Motor &motor_right, interface::IMUKit &imu_kit) + : _event_loop(event_loop), + _timeout(timeout), + _battery(battery), + _motor_left(motor_left), + _motor_right(motor_right), + _imukit(imu_kit) + { + } + + ~SealStrategy() = 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; + 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 new file mode 100644 index 0000000000..49403e1efa --- /dev/null +++ b/spikes/lk_auto_charge/main.cpp @@ -0,0 +1,140 @@ +// Leka - LekaOS +// Copyright 2023 APF France handicap +// SPDX-License-Identifier: Apache-2.0 + +#include "drivers/DigitalOut.h" +#include "drivers/InterruptIn.h" +#include "rtos/ThisThread.h" + +#include "BLEKit.h" +#include "BLEServiceCommands.h" + +#include "BatteryKit.h" +#include "CoreBattery.h" +#include "CoreI2C.h" +#include "CoreLSM6DSOX.hpp" +#include "CoreMotor.h" +#include "CorePwm.h" +#include "CoreTimeout.h" +#include "EventLoopKit.h" +#include "HelloWorld.h" +#include "IMUKit.hpp" +#include "LogKit.h" +#include "MathUtils.h" +#include "SealStrategy.h" + +using namespace std::chrono; +using namespace leka; + +namespace { + +auto event_loop = EventLoopKit {}; +auto timeout = CoreTimeout {}; + +namespace battery { + + namespace charge { + + auto status_input = mbed::InterruptIn {PinName::BATTERY_CHARGE_STATUS}; + + } + + auto cells = CoreBattery {PinName::BATTERY_VOLTAGE, battery::charge::status_input}; + +} // namespace battery + +namespace motors { + + namespace left { + + namespace internal { + + auto dir_1 = mbed::DigitalOut {MOTOR_LEFT_DIRECTION_1}; + auto dir_2 = mbed::DigitalOut {MOTOR_LEFT_DIRECTION_2}; + auto speed = CorePwm {MOTOR_LEFT_PWM}; + + } // namespace internal + + auto motor = CoreMotor {internal::dir_1, internal::dir_2, internal::speed}; + + } // namespace left + + namespace right { + + namespace internal { + + auto dir_1 = mbed::DigitalOut {MOTOR_RIGHT_DIRECTION_1}; + auto dir_2 = mbed::DigitalOut {MOTOR_RIGHT_DIRECTION_2}; + auto speed = CorePwm {MOTOR_RIGHT_PWM}; + + } // namespace internal + + auto motor = CoreMotor {internal::dir_1, internal::dir_2, internal::speed}; + + } // namespace right + +} // namespace motors + +namespace imu { + + namespace internal { + + auto drdy_irq = CoreInterruptIn {PinName::SENSOR_IMU_IRQ}; + auto i2c = CoreI2C(PinName::SENSOR_IMU_TH_I2C_SDA, PinName::SENSOR_IMU_TH_I2C_SCL); + + } // namespace internal + + CoreLSM6DSOX lsm6dsox(internal::i2c, internal::drdy_irq); + +} // namespace imu + +IMUKit imukit(imu::lsm6dsox); + +} // namespace + +auto mainboard_led = mbed::DigitalOut {LED1}; + +auto service_commands = BLEServiceCommands {}; +auto services = std::to_array({&service_commands}); +auto blekit = BLEKit {}; + +auto seal_strategy = + SealStrategy {event_loop, timeout, battery::cells, motors::left::motor, motors::right::motor, imukit}; + +auto last_strategy = uint8_t {0x00}; + +void runStrategy(uint8_t id) +{ + if (id == 0x01) { + seal_strategy.start(); + last_strategy = id; + } else { + seal_strategy.stop(); + } +} + +auto main() -> int +{ + logger::init(); + + HelloWorld hello; + hello.start(); + + log_info("Hello world!"); + + imu::lsm6dsox.init(); + + imukit.stop(); + imukit.init(); + imukit.start(); + + battery::cells.onChargeDidStop([] { runStrategy(last_strategy); }); + + blekit.setServices(services); + service_commands.onCommandsReceived([](std::span _buffer) { runStrategy(_buffer[0]); }); + blekit.init(); + + while (true) { + rtos::ThisThread::sleep_for(10min); + } +} From 272519b6b15b68fd96714a35fbcf51aba7265180 Mon Sep 17 00:00:00 2001 From: Yann Locatelli Date: Tue, 12 Dec 2023 17:21:06 +0100 Subject: [PATCH 2/4] :alembic: (spike): Autocharge - HappyToupie 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/HappyToupie.cpp | 82 +++++++++++++++++++++++++++ spikes/lk_auto_charge/HappyToupie.h | 58 +++++++++++++++++++ spikes/lk_auto_charge/main.cpp | 6 ++ 4 files changed, 147 insertions(+) create mode 100644 spikes/lk_auto_charge/HappyToupie.cpp create mode 100644 spikes/lk_auto_charge/HappyToupie.h diff --git a/spikes/lk_auto_charge/CMakeLists.txt b/spikes/lk_auto_charge/CMakeLists.txt index af7d115332..f942443be6 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..877b535008 --- /dev/null +++ b/spikes/lk_auto_charge/HappyToupie.cpp @@ -0,0 +1,82 @@ +// 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; + +auto HappyToupie::convertToPwmFrom(float angle) const -> float +{ + auto res = utils::math::map(angle, kMinAngleInput, kMaxAngleInput, kMinPwmOutput, kMaxPwmOutput); + return res; +} + +void HappyToupie::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 HappyToupie::stop() +{ + should_stop = true; + _event_loop.stop(); + rtos::ThisThread::sleep_for(100ms); + stopMotors(); + _timeout.stop(); +} + +void HappyToupie::run() +{ + if (should_stop || _battery.isCharging()) { + stop(); + 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..8777a6ee33 --- /dev/null +++ b/spikes/lk_auto_charge/HappyToupie.h @@ -0,0 +1,58 @@ +// 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 HappyToupie +{ + public: + HappyToupie(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) + { + } + + ~HappyToupie() = 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 should_stop = true; +}; + +} // namespace leka diff --git a/spikes/lk_auto_charge/main.cpp b/spikes/lk_auto_charge/main.cpp index 49403e1efa..0ebad93e77 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 "HappyToupie.h" #include "HelloWorld.h" #include "IMUKit.hpp" #include "LogKit.h" @@ -100,6 +101,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 last_strategy = uint8_t {0x00}; @@ -108,8 +110,12 @@ void runStrategy(uint8_t id) if (id == 0x01) { seal_strategy.start(); last_strategy = id; + } else if (id == 0x02) { + happy_toupie.start(); + last_strategy = id; } else { seal_strategy.stop(); + happy_toupie.stop(); } } From da5d3fd072b9045e1398c56c8d3d578ebc89cbd2 Mon Sep 17 00:00:00 2001 From: Yann Locatelli Date: Tue, 12 Dec 2023 17:21:38 +0100 Subject: [PATCH 3/4] :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(); } } From c1cc2f092d8d5af4c3069d869b8b23ba3e674536 Mon Sep 17 00:00:00 2001 From: Yann Locatelli Date: Thu, 14 Dec 2023 17:54:32 +0100 Subject: [PATCH 4/4] Apply suggestion from review --- spikes/lk_auto_charge/SealStrategy.cpp | 46 ++++++++++++++++---------- spikes/lk_auto_charge/SealStrategy.h | 3 ++ 2 files changed, 31 insertions(+), 18 deletions(-) diff --git a/spikes/lk_auto_charge/SealStrategy.cpp b/spikes/lk_auto_charge/SealStrategy.cpp index c766371440..908add48a3 100644 --- a/spikes/lk_auto_charge/SealStrategy.cpp +++ b/spikes/lk_auto_charge/SealStrategy.cpp @@ -53,26 +53,10 @@ void SealStrategy::run() if (abs_float(angles.roll) > kRollTolerance) { auto speed_offset = convertToPwmFrom(angles.roll > 0 ? angles.roll : -angles.roll); - if (is_right_tilted) { - if (should_move_forward) { - spinRight(kMinPwmOutput, kMinPwmOutput + speed_offset); - } else { - spinLeft(kMinPwmOutput, kMinPwmOutput + speed_offset); - } - } else { - if (should_move_forward) { - spinLeft(kMinPwmOutput + speed_offset, kMinPwmOutput); - } else { - spinRight(kMinPwmOutput + speed_offset, kMinPwmOutput); - } - } + spinToFixRoll(is_right_tilted, should_move_forward, speed_offset); } else if (abs_float(angles.pitch) > kPitchTolerance) { auto speed = convertToPwmFrom(angles.pitch > 0 ? angles.pitch : -angles.pitch); - if (should_move_forward) { - moveForward(speed); - } else { - moveBackward(speed); - } + moveToFixPitch(should_move_forward, speed); } else { stopMotors(); } @@ -110,3 +94,29 @@ void SealStrategy::moveBackward(float speed) _motor_left.spin(Rotation::clockwise, speed); _motor_right.spin(Rotation::counterClockwise, speed); } + +void SealStrategy::spinToFixRoll(bool is_right_tilted, bool should_move_forward, float speed_offset) +{ + if (is_right_tilted) { + if (should_move_forward) { + spinRight(kMinPwmOutput, kMinPwmOutput + speed_offset); + } else { + spinLeft(kMinPwmOutput, kMinPwmOutput + speed_offset); + } + } else { + if (should_move_forward) { + spinLeft(kMinPwmOutput + speed_offset, kMinPwmOutput); + } else { + spinRight(kMinPwmOutput + speed_offset, kMinPwmOutput); + } + } +} + +void SealStrategy::moveToFixPitch(bool should_move_forward, float speed) +{ + if (should_move_forward) { + moveForward(speed); + } else { + moveBackward(speed); + } +} diff --git a/spikes/lk_auto_charge/SealStrategy.h b/spikes/lk_auto_charge/SealStrategy.h index 469facdaaf..72b16d07b9 100644 --- a/spikes/lk_auto_charge/SealStrategy.h +++ b/spikes/lk_auto_charge/SealStrategy.h @@ -42,6 +42,9 @@ class SealStrategy void moveForward(float speed); void moveBackward(float speed); + void spinToFixRoll(bool is_right_tilted, bool should_move_forward, float speed_offset); + void moveToFixPitch(bool should_move_forward, float speed); + interface::EventLoop &_event_loop; interface::Timeout &_timeout;