From 9e076b1c4de20ee35301b111fec5bf6ccb5d6fde Mon Sep 17 00:00:00 2001 From: ahiuchingau <20424172+ahiuchingau@users.noreply.github.com> Date: Wed, 28 Aug 2024 00:25:18 -0400 Subject: [PATCH] move motor at frequency works --- .../flex-stacker/src/motor_utils.cpp | 5 ++++ .../flex-stacker/firmware/motor_interrupt.hpp | 29 +++++++++++++++---- .../flex-stacker/flex-stacker/motor_task.hpp | 9 +++--- .../flex-stacker/flex-stacker/motor_utils.hpp | 3 ++ 4 files changed, 36 insertions(+), 10 deletions(-) diff --git a/stm32-modules/flex-stacker/src/motor_utils.cpp b/stm32-modules/flex-stacker/src/motor_utils.cpp index 45827d611..2eb54dbda 100644 --- a/stm32-modules/flex-stacker/src/motor_utils.cpp +++ b/stm32-modules/flex-stacker/src/motor_utils.cpp @@ -75,3 +75,8 @@ auto MovementProfile::tick() -> TickReturn { [[nodiscard]] auto MovementProfile::current_distance() const -> ticks { return _current_distance; } + +/** Returns the movement type.*/ +[[nodiscard]] auto MovementProfile::movement_type() const -> MovementType { + return _type; +} diff --git a/stm32-modules/include/flex-stacker/firmware/motor_interrupt.hpp b/stm32-modules/include/flex-stacker/firmware/motor_interrupt.hpp index 827f7b14a..c258d26ed 100644 --- a/stm32-modules/include/flex-stacker/firmware/motor_interrupt.hpp +++ b/stm32-modules/include/flex-stacker/firmware/motor_interrupt.hpp @@ -1,4 +1,5 @@ #pragma once +#include #include "firmware/motor_hardware.h" #include "firmware/motor_policy.hpp" @@ -17,19 +18,24 @@ static constexpr double DEFAULT_ACCEL = 50000; // steps per second^2 class MotorInterruptController { public: - MotorInterruptController(MotorID id, MotorPolicy* policy) + explicit MotorInterruptController(MotorID id, MotorPolicy* policy) : _id(id), _policy(policy), _initialized(false), _profile(TIMER_FREQ, 0, 0, 0, motor_util::MovementType::OpenLoop, 0) { } + MotorInterruptController(MotorInterruptController const&) = delete; + void operator=(MotorInterruptController const&) = delete; + MotorInterruptController(MotorInterruptController const&&) = delete; + void operator=(MotorInterruptController const&&) = delete; + ~MotorInterruptController() = default; auto tick() -> bool { if (!_initialized) { return false; } auto ret = _profile.tick(); - if (ret.step) { + if (ret.step && !stop_condition_met()) { _policy->step(_id); } if (ret.done) { @@ -38,7 +44,10 @@ class MotorInterruptController { return ret.done; } auto set_freq(uint32_t freq) -> void { step_freq = freq; } - auto initialize(MotorPolicy* policy) -> void { _policy = policy; } + auto initialize(MotorPolicy* policy) -> void { + _policy = policy; + _initialized = true; + } auto start_movement(uint32_t id, long steps, uint32_t frequency) -> void { _profile = motor_util::MovementProfile( TIMER_FREQ, 0, frequency, 0, @@ -48,20 +57,30 @@ class MotorInterruptController { } auto set_direction(bool direction) -> void { _policy->set_direction(_id, direction); + _direction = direction; + } + auto limit_switch_triggered() -> bool { + return _policy->check_limit_switch(_id, _direction); } - [[nodiscard]] auto get_response_id() const -> uint32_t { return _response_id; } + auto stop_condition_met() -> bool { + if (_profile.movement_type() == motor_util::MovementType::OpenLoop) { + return limit_switch_triggered(); + } + return false; + } private: MotorID _id; MotorPolicy* _policy; - bool _initialized; + std::atomic_bool _initialized; motor_util::MovementProfile _profile; uint32_t step_count = 0; uint32_t step_freq = DEFAULT_MOTOR_FREQ; uint32_t _response_id = 0; + bool _direction = false; }; } // namespace motor_interrupt_controller diff --git a/stm32-modules/include/flex-stacker/flex-stacker/motor_task.hpp b/stm32-modules/include/flex-stacker/flex-stacker/motor_task.hpp index d7f40753b..b76545ca0 100644 --- a/stm32-modules/include/flex-stacker/flex-stacker/motor_task.hpp +++ b/stm32-modules/include/flex-stacker/flex-stacker/motor_task.hpp @@ -144,9 +144,9 @@ class MotorTask { auto visit_message(const messages::MoveMotorAtFrequencyMessage& m, Policy& policy) -> void { static_cast(policy); - auto controller = controller_from_id(m.motor_id); - controller.set_direction(m.direction); - controller.start_movement(m.id, m.steps, m.frequency); +// auto controller = controller_from_id(m.motor_id); + controller_from_id(m.motor_id).set_direction(m.direction); + controller_from_id(m.motor_id).start_movement(m.id, m.steps, m.frequency); } template @@ -160,9 +160,8 @@ class MotorTask { auto visit_message(const messages::MoveCompleteMessage& m, Policy& policy) -> void { static_cast(policy); - auto controller = controller_from_id(m.motor_id); auto response = messages::AcknowledgePrevious{ - .responding_to_id = controller.get_response_id()}; + .responding_to_id = controller_from_id(m.motor_id).get_response_id()}; static_cast(_task_registry->send_to_address( response, Queues::HostCommsAddress)); } diff --git a/stm32-modules/include/flex-stacker/flex-stacker/motor_utils.hpp b/stm32-modules/include/flex-stacker/flex-stacker/motor_utils.hpp index 128cd65cc..594726c9e 100644 --- a/stm32-modules/include/flex-stacker/flex-stacker/motor_utils.hpp +++ b/stm32-modules/include/flex-stacker/flex-stacker/motor_utils.hpp @@ -91,6 +91,9 @@ class MovementProfile { /** Returns the number of ticks that have been taken.*/ [[nodiscard]] auto current_distance() const -> ticks; + /** Returns the movement type.*/ + [[nodiscard]] auto movement_type() const -> MovementType; + private: uint32_t _ticks_per_second; // Tick frequency steps_per_tick _velocity = 0; // Current velocity