From b66c27ae6741c1695ce52726def63139e0e3a288 Mon Sep 17 00:00:00 2001 From: Alise Au <20424172+ahiuchingau@users.noreply.github.com> Date: Mon, 16 Sep 2024 16:56:56 -0400 Subject: [PATCH] feat(flex-stacker): make move params persist (#464) * make move params optional for mm-controlled moves * add gcode to set microsteps * add Gcode to query move parms --- .../flex-stacker/gcodes_motor.hpp | 128 ++++++++++++-- .../flex-stacker/host_comms_task.hpp | 91 +++++++++- .../flex-stacker/flex-stacker/messages.hpp | 51 ++++-- .../flex-stacker/motor_driver_task.hpp | 18 ++ .../flex-stacker/flex-stacker/motor_task.hpp | 160 ++++++++++++++++-- .../flex-stacker/flex-stacker/tmc2160.hpp | 8 + 6 files changed, 404 insertions(+), 52 deletions(-) diff --git a/stm32-modules/include/flex-stacker/flex-stacker/gcodes_motor.hpp b/stm32-modules/include/flex-stacker/flex-stacker/gcodes_motor.hpp index aefcf4d4..cdd6330b 100644 --- a/stm32-modules/include/flex-stacker/flex-stacker/gcodes_motor.hpp +++ b/stm32-modules/include/flex-stacker/flex-stacker/gcodes_motor.hpp @@ -87,6 +87,61 @@ struct GetTMCRegister { } }; +struct SetMicrosteps { + MotorID motor_id; + uint8_t microsteps_power; + + using ParseResult = std::optional; + static constexpr auto prefix = std::array{'M', '9', '0', '9', ' '}; + static constexpr const char* response = "M909 OK\n"; + + template + requires std::forward_iterator && + std::sized_sentinel_for + static auto parse(const InputIt& input, Limit limit) + -> std::pair { + MotorID motor_id_val = MotorID::MOTOR_X; + auto working = prefix_matches(input, limit, prefix); + if (working == input) { + return std::make_pair(ParseResult(), input); + } + switch (*working) { + case 'X': + motor_id_val = MotorID::MOTOR_X; + break; + case 'Z': + motor_id_val = MotorID::MOTOR_Z; + break; + case 'L': + motor_id_val = MotorID::MOTOR_L; + break; + default: + return std::make_pair(ParseResult(), input); + } + std::advance(working, 1); + if (working == limit) { + return std::make_pair(ParseResult(), input); + } + + auto ustep_res = gcode::parse_value(working, limit); + if (!ustep_res.first.has_value()) { + return std::make_pair(ParseResult(), input); + } + + return std::make_pair(ParseResult(SetMicrosteps{ + .motor_id = motor_id_val, + .microsteps_power = ustep_res.first.value()}), + ustep_res.second); + } + + template + requires std::forward_iterator && + std::sized_sentinel_for + static auto write_response_into(InputIt buf, InLimit limit) -> InputIt { + return write_string_to_iterpair(buf, limit, response); + } +}; + struct SetTMCRegister { MotorID motor_id; uint8_t reg; @@ -480,9 +535,7 @@ struct MoveMotorInSteps { struct MoveMotorInMm { MotorID motor_id; float mm; - float mm_per_second; - float mm_per_second_sq; - float mm_per_second_discont; + std::optional mm_per_second, mm_per_second_sq, mm_per_second_discont; using ParseResult = std::optional; static constexpr auto prefix = std::array{'G', '0', ' '}; @@ -508,7 +561,7 @@ struct MoveMotorInMm { }; struct VelArg { static constexpr auto prefix = std::array{'V'}; - static constexpr bool required = true; + static constexpr bool required = false; bool present = false; float value = 0; }; @@ -541,9 +594,9 @@ struct MoveMotorInMm { auto ret = MoveMotorInMm{ .motor_id = MotorID::MOTOR_X, .mm = 0.0, - .mm_per_second = 0.0, - .mm_per_second_sq = 0.0, - .mm_per_second_discont = 0.0, + .mm_per_second = std::nullopt, + .mm_per_second_sq = std::nullopt, + .mm_per_second_discont = std::nullopt, }; auto arguments = res.first.value(); @@ -561,8 +614,6 @@ struct MoveMotorInMm { if (std::get<3>(arguments).present) { ret.mm_per_second = std::get<3>(arguments).value; - } else { - return std::make_pair(ParseResult(), input); } if (std::get<4>(arguments).present) { @@ -587,9 +638,7 @@ struct MoveMotorInMm { struct MoveToLimitSwitch { MotorID motor_id; bool direction; - float mm_per_second; - float mm_per_second_sq; - float mm_per_second_discont; + std::optional mm_per_second, mm_per_second_sq, mm_per_second_discont; using ParseResult = std::optional; static constexpr auto prefix = std::array{'G', '5', ' '}; @@ -646,9 +695,9 @@ struct MoveToLimitSwitch { auto ret = MoveToLimitSwitch{ .motor_id = MotorID::MOTOR_X, .direction = false, - .mm_per_second = 0.0, - .mm_per_second_sq = 0.0, - .mm_per_second_discont = 0.0, + .mm_per_second = std::nullopt, + .mm_per_second_sq = std::nullopt, + .mm_per_second_discont = std::nullopt, }; auto arguments = res.first.value(); @@ -826,4 +875,53 @@ struct GetLimitSwitches { } }; +struct GetMoveParams { + MotorID motor_id; + using ParseResult = std::optional; + static constexpr auto prefix = std::array{'M', '1', '2', '0'}; + + template + requires std::forward_iterator && + std::sized_sentinel_for + static auto parse(const InputIt& input, Limit limit) + -> std::pair { + MotorID motor = MotorID::MOTOR_X; + auto res = gcode::SingleParser::parse_gcode( + input, limit, prefix); + if (!res.first.has_value()) { + return std::make_pair(ParseResult(), input); + } + auto arguments = res.first.value(); + if (std::get<1>(arguments).present) { + motor = MotorID::MOTOR_Z; + } else if (std::get<2>(arguments).present) { + motor = MotorID::MOTOR_L; + } else if (!std::get<0>(arguments).present) { + return std::make_pair(ParseResult(), input); + } + return std::make_pair(ParseResult(GetMoveParams{.motor_id = motor}), + res.second); + } + + template + requires std::forward_iterator && + std::sized_sentinel_for + static auto write_response_into(InputIt buf, InLimit limit, + MotorID motor_id, float velocity, + float accel, float velocity_discont) + -> InputIt { + char motor_char = motor_id == MotorID::MOTOR_X ? 'X' + : motor_id == MotorID::MOTOR_Z ? 'Z' + : 'L'; + int res = 0; + res = + snprintf(&*buf, (limit - buf), "M120 %c V:%.3f A:%.3f D:%.3f OK\n", + motor_char, velocity, accel, velocity_discont); + if (res <= 0) { + return buf; + } + return buf + res; + } +}; + } // namespace gcode diff --git a/stm32-modules/include/flex-stacker/flex-stacker/host_comms_task.hpp b/stm32-modules/include/flex-stacker/flex-stacker/host_comms_task.hpp index d4242d19..0209dabb 100644 --- a/stm32-modules/include/flex-stacker/flex-stacker/host_comms_task.hpp +++ b/stm32-modules/include/flex-stacker/flex-stacker/host_comms_task.hpp @@ -35,23 +35,24 @@ class HostCommsTask { public: using Queue = QueueImpl; using Aggregator = typename tasks::Tasks::QueueAggregator; + using Queues = typename tasks::Tasks; private: - using GCodeParser = - gcode::GroupParser; + using GCodeParser = gcode::GroupParser< + gcode::GetTMCRegister, gcode::SetTMCRegister, gcode::SetRunCurrent, + gcode::SetHoldCurrent, gcode::EnableMotor, gcode::DisableMotor, + gcode::MoveMotorInSteps, gcode::MoveToLimitSwitch, gcode::MoveMotorInMm, + gcode::GetLimitSwitches, gcode::SetMicrosteps, gcode::GetMoveParams>; using AckOnlyCache = AckCache<8, gcode::EnterBootloader, gcode::SetSerialNumber, gcode::SetTMCRegister, gcode::SetRunCurrent, gcode::SetHoldCurrent, gcode::EnableMotor, gcode::DisableMotor, gcode::MoveMotorInSteps, gcode::MoveToLimitSwitch, - gcode::MoveMotorInMm>; + gcode::MoveMotorInMm, gcode::SetMicrosteps>; using GetSystemInfoCache = AckCache<8, gcode::GetSystemInfo>; using GetTMCRegisterCache = AckCache<8, gcode::GetTMCRegister>; using GetLimitSwitchesCache = AckCache<8, gcode::GetLimitSwitches>; + using GetMoveParamsCache = AckCache<8, gcode::GetMoveParams>; public: static constexpr size_t TICKS_TO_WAIT_ON_SEND = 10; @@ -66,7 +67,9 @@ class HostCommsTask { // NOLINTNEXTLINE(readability-redundant-member-init) get_tmc_register_cache(), // NOLINTNEXTLINE(readability-redundant-member-init) - get_limit_switches_cache() {} + get_limit_switches_cache(), + // NOLINTNEXTLINE(readability-redundant-member-init) + get_move_params_cache() {} HostCommsTask(const HostCommsTask& other) = delete; auto operator=(const HostCommsTask& other) -> HostCommsTask& = delete; HostCommsTask(HostCommsTask&& other) noexcept = delete; @@ -316,6 +319,29 @@ class HostCommsTask { cache_entry); } + template + requires std::forward_iterator && + std::sized_sentinel_for + auto visit_message(const messages::GetMoveParamsResponse& response, + InputIt tx_into, InputLimit tx_limit) -> InputIt { + auto cache_entry = + get_move_params_cache.remove_if_present(response.responding_to_id); + return std::visit( + [tx_into, tx_limit, response](auto cache_element) { + using T = std::decay_t; + if constexpr (std::is_same_v) { + return errors::write_into( + tx_into, tx_limit, + errors::ErrorCode::BAD_MESSAGE_ACKNOWLEDGEMENT); + } else { + return cache_element.write_response_into( + tx_into, tx_limit, response.motor_id, response.velocity, + response.acceleration, response.velocity_discont); + } + }, + cache_entry); + } + template requires std::forward_iterator && std::sized_sentinel_for @@ -424,6 +450,32 @@ class HostCommsTask { return std::make_pair(true, tx_into); } + template + requires std::forward_iterator && + std::sized_sentinel_for + auto visit_gcode(const gcode::SetMicrosteps& gcode, InputIt tx_into, + InputLimit tx_limit) -> std::pair { + auto id = ack_only_cache.add(gcode); + if (id == 0) { + return std::make_pair( + false, errors::write_into(tx_into, tx_limit, + errors::ErrorCode::GCODE_CACHE_FULL)); + } + auto message = messages::SetMicrostepsMessage{ + .id = id, + .motor_id = gcode.motor_id, + .microsteps_power = + std::min(gcode.microsteps_power, static_cast(8))}; + if (!task_registry->send_to_address(message, Queues::MotorDriverAddress, + TICKS_TO_WAIT_ON_SEND)) { + auto wrote_to = errors::write_into( + tx_into, tx_limit, errors::ErrorCode::INTERNAL_QUEUE_FULL); + ack_only_cache.remove_if_present(id); + return std::make_pair(false, wrote_to); + } + return std::make_pair(true, tx_into); + } + template requires std::forward_iterator && std::sized_sentinel_for @@ -614,6 +666,28 @@ class HostCommsTask { return std::make_pair(true, tx_into); } + template + requires std::forward_iterator && + std::sized_sentinel_for + auto visit_gcode(const gcode::GetMoveParams& gcode, InputIt tx_into, + InputLimit tx_limit) -> std::pair { + auto id = get_move_params_cache.add(gcode); + if (id == 0) { + return std::make_pair( + false, errors::write_into(tx_into, tx_limit, + errors::ErrorCode::GCODE_CACHE_FULL)); + } + auto message = messages::GetMoveParamsMessage{ + .id = id, .motor_id = gcode.motor_id}; + if (!task_registry->send(message, TICKS_TO_WAIT_ON_SEND)) { + auto wrote_to = errors::write_into( + tx_into, tx_limit, errors::ErrorCode::INTERNAL_QUEUE_FULL); + get_move_params_cache.remove_if_present(id); + return std::make_pair(false, wrote_to); + } + return std::make_pair(true, tx_into); + } + // Our error handler just writes an error and bails template requires std::forward_iterator && @@ -632,6 +706,7 @@ class HostCommsTask { GetSystemInfoCache get_system_info_cache; GetTMCRegisterCache get_tmc_register_cache; GetLimitSwitchesCache get_limit_switches_cache; + GetMoveParamsCache get_move_params_cache; bool may_connect_latch = true; }; diff --git a/stm32-modules/include/flex-stacker/flex-stacker/messages.hpp b/stm32-modules/include/flex-stacker/flex-stacker/messages.hpp index 49801728..7936d953 100644 --- a/stm32-modules/include/flex-stacker/flex-stacker/messages.hpp +++ b/stm32-modules/include/flex-stacker/flex-stacker/messages.hpp @@ -96,6 +96,12 @@ struct SetMotorCurrentMessage { float hold_current; }; +struct SetMicrostepsMessage { + uint32_t id; + MotorID motor_id; + uint8_t microsteps_power; +}; + struct SetTMCRegisterMessage { uint32_t id; MotorID motor_id; @@ -142,21 +148,21 @@ struct MoveMotorInStepsMessage { }; struct MoveMotorInMmMessage { - uint32_t id; - MotorID motor_id; - float mm; - float mm_per_second; - float mm_per_second_sq; - float mm_per_second_discont; + uint32_t id = 0; + MotorID motor_id = MotorID::MOTOR_X; + float mm = 0; + std::optional mm_per_second = std::nullopt; + std::optional mm_per_second_sq = std::nullopt; + std::optional mm_per_second_discont = std::nullopt; }; struct MoveToLimitSwitchMessage { - uint32_t id; - MotorID motor_id; - bool direction; - float mm_per_second; - float mm_per_second_sq; - float mm_per_second_discont; + uint32_t id = 0; + MotorID motor_id = MotorID::MOTOR_X; + bool direction = false; + std::optional mm_per_second = std::nullopt; + std::optional mm_per_second_sq = std::nullopt; + std::optional mm_per_second_discont = std::nullopt; }; struct GetLimitSwitchesMessage { @@ -188,10 +194,24 @@ struct StopMotorMessage { uint32_t id; }; +struct GetMoveParamsMessage { + uint32_t id; + MotorID motor_id; +}; + +struct GetMoveParamsResponse { + uint32_t responding_to_id; + MotorID motor_id; + float velocity; + float acceleration; + float velocity_discont; +}; + using HostCommsMessage = ::std::variant; + GetTMCRegisterResponse, GetLimitSwitchesResponses, + GetMoveParamsResponse>; using SystemMessage = ::std::variant; + SetMotorCurrentMessage, SetMicrostepsMessage>; using MotorMessage = ::std::variant; + MoveMotorInMmMessage, SetMicrostepsMessage, + GetMoveParamsMessage>; }; // namespace messages diff --git a/stm32-modules/include/flex-stacker/flex-stacker/motor_driver_task.hpp b/stm32-modules/include/flex-stacker/flex-stacker/motor_driver_task.hpp index 8b7050bd..bc297e6e 100644 --- a/stm32-modules/include/flex-stacker/flex-stacker/motor_driver_task.hpp +++ b/stm32-modules/include/flex-stacker/flex-stacker/motor_driver_task.hpp @@ -239,6 +239,24 @@ class MotorDriverTask { static_cast(tmc2160_interface); } + template + auto visit_message(const messages::SetMicrostepsMessage& m, + tmc2160::TMC2160Interface& tmc2160_interface) + -> void { + driver_conf_from_id(m.motor_id).chopconf.mres = m.microsteps_power; + if (!_tmc2160.update_chopconf(driver_conf_from_id(m.motor_id), + tmc2160_interface, m.motor_id)) { + auto response = messages::AcknowledgePrevious{ + .responding_to_id = m.id, + .with_error = errors::ErrorCode::TMC2160_WRITE_ERROR}; + static_cast(_task_registry->send_to_address( + response, Queues::HostCommsAddress)); + return; + }; + static_cast( + _task_registry->send_to_address(m, Queues::MotorAddress)); + } + template auto visit_message(const messages::SetMotorCurrentMessage& m, tmc2160::TMC2160Interface& tmc2160_interface) 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 d41d3fb9..0643535c 100644 --- a/stm32-modules/include/flex-stacker/flex-stacker/motor_task.hpp +++ b/stm32-modules/include/flex-stacker/flex-stacker/motor_task.hpp @@ -48,6 +48,46 @@ static constexpr struct lms::LinearMotionSystemConfig .steps_per_rev = 200, .microstep = 16, }; +struct MotorState { + // NOLINTNEXTLINE(misc-non-private-member-variables-in-classes) + float steps_per_mm; + // NOLINTNEXTLINE(misc-non-private-member-variables-in-classes) + float speed_mm_per_sec; + // NOLINTNEXTLINE(misc-non-private-member-variables-in-classes) + float accel_mm_per_sec_sq; + // NOLINTNEXTLINE(misc-non-private-member-variables-in-classes) + float speed_mm_per_sec_discont; + [[nodiscard]] auto get_speed() const -> float { + return speed_mm_per_sec * steps_per_mm; + } + [[nodiscard]] auto get_accel() const -> float { + return accel_mm_per_sec_sq * steps_per_mm * steps_per_mm; + } + [[nodiscard]] auto get_speed_discont() const -> float { + return speed_mm_per_sec_discont * steps_per_mm; + } + [[nodiscard]] auto get_distance(float mm) const -> float { + return mm * steps_per_mm; + } +}; + +struct XState { + static constexpr float DEFAULT_SPEED = 200.0; + static constexpr float DEFAULT_ACCELERATION = 50.0; + static constexpr float DEFAULT_SPEED_DISCONT = 5.0; +}; + +struct ZState { + static constexpr float DEFAULT_SPEED = 200.0; + static constexpr float DEFAULT_ACCELERATION = 50.0; + static constexpr float DEFAULT_SPEED_DISCONT = 5.0; +}; +struct LState { + static constexpr float DEFAULT_SPEED = 200.0; + static constexpr float DEFAULT_ACCELERATION = 50.0; + static constexpr float DEFAULT_SPEED_DISCONT = 5.0; +}; + template