From 2bcd6bb70b817a78e18b577427359476f4e01d66 Mon Sep 17 00:00:00 2001 From: Alise Au <20424172+ahiuchingau@users.noreply.github.com> Date: Tue, 20 Aug 2024 13:51:31 -0400 Subject: [PATCH] feat(flex-stacker): add Gcodes to enable motor movements (#455) Add Gcodes for flex stacker * M921: write to register * M17/M18: enable/disable motor * G6: move motor at frequency * M0: stop --- .../motor_control/freertos_motor_task.cpp | 3 - .../firmware/motor_control/motor_hardware.c | 27 +- .../firmware/motor_control/motor_policy.cpp | 8 +- stm32-modules/flex-stacker/src/errors.cpp | 9 + .../flex-stacker/src/motor_utils.cpp | 77 ++++ .../flex-stacker/firmware/motor_hardware.h | 4 +- .../flex-stacker/firmware/motor_policy.hpp | 4 +- .../flex-stacker/flex-stacker/errors.hpp | 5 + .../flex-stacker/flex-stacker/gcodes.hpp | 67 +-- .../flex-stacker/gcodes_motor.hpp | 380 ++++++++++++++++++ .../flex-stacker/host_comms_task.hpp | 125 +++++- .../flex-stacker/flex-stacker/messages.hpp | 24 +- .../flex-stacker/motor_driver_task.hpp | 14 +- .../flex-stacker/flex-stacker/motor_task.hpp | 54 ++- .../flex-stacker/flex-stacker/motor_utils.hpp | 110 +++++ 15 files changed, 806 insertions(+), 105 deletions(-) create mode 100644 stm32-modules/flex-stacker/src/motor_utils.cpp create mode 100644 stm32-modules/include/flex-stacker/flex-stacker/gcodes_motor.hpp create mode 100644 stm32-modules/include/flex-stacker/flex-stacker/motor_utils.hpp diff --git a/stm32-modules/flex-stacker/firmware/motor_control/freertos_motor_task.cpp b/stm32-modules/flex-stacker/firmware/motor_control/freertos_motor_task.cpp index 7164fa12..5b24889e 100644 --- a/stm32-modules/flex-stacker/firmware/motor_control/freertos_motor_task.cpp +++ b/stm32-modules/flex-stacker/firmware/motor_control/freertos_motor_task.cpp @@ -58,9 +58,6 @@ auto run(tasks::FirmwareTasks::QueueAggregator* aggregator) -> void { motor_hardware_init(); initialize_callbacks(callback_glue); auto policy = motor_policy::MotorPolicy(); - policy.enable_motor(MotorID::MOTOR_L); - policy.enable_motor(MotorID::MOTOR_X); - policy.enable_motor(MotorID::MOTOR_Z); while (true) { _top_task.run_once(policy); } diff --git a/stm32-modules/flex-stacker/firmware/motor_control/motor_hardware.c b/stm32-modules/flex-stacker/firmware/motor_control/motor_hardware.c index 42b3f8b3..ff486db4 100644 --- a/stm32-modules/flex-stacker/firmware/motor_control/motor_hardware.c +++ b/stm32-modules/flex-stacker/firmware/motor_control/motor_hardware.c @@ -3,6 +3,8 @@ #include "stm32g4xx_it.h" #include "systemwide.h" +#include + /******************* Motor Z *******************/ /** Motor hardware **/ #define Z_STEP_PIN (GPIO_PIN_2) @@ -276,7 +278,7 @@ void motor_hardware_init(void){ motor_hardware_interrupt_init(); } -void hw_enable_motor(MotorID motor_id) { +bool hw_enable_motor(MotorID motor_id) { void* port; uint16_t pin; HAL_StatusTypeDef status = HAL_OK; @@ -285,7 +287,7 @@ void hw_enable_motor(MotorID motor_id) { port = Z_EN_PORT; pin = Z_EN_PIN; status = HAL_TIM_Base_Start_IT(&htim20); - HAL_NVIC_SetPriority(TIM20_UP_IRQn, 5, 0); + HAL_NVIC_SetPriority(TIM20_UP_IRQn, 10, 0); HAL_NVIC_EnableIRQ(TIM20_UP_IRQn); if (status == HAL_OK) { @@ -295,7 +297,7 @@ void hw_enable_motor(MotorID motor_id) { port = X_EN_PORT; pin = X_EN_PIN; status = HAL_TIM_Base_Start_IT(&htim17); - HAL_NVIC_SetPriority(TIM1_TRG_COM_TIM17_IRQn, 6, 0); + HAL_NVIC_SetPriority(TIM1_TRG_COM_TIM17_IRQn, 10, 0); HAL_NVIC_EnableIRQ(TIM1_TRG_COM_TIM17_IRQn); if (status == HAL_OK) { @@ -305,19 +307,20 @@ void hw_enable_motor(MotorID motor_id) { port = L_EN_PORT; pin = L_EN_PIN; status = HAL_TIM_Base_Start_IT(&htim3); - HAL_NVIC_SetPriority(TIM3_IRQn, 6, 0); + HAL_NVIC_SetPriority(TIM3_IRQn, 10, 0); HAL_NVIC_EnableIRQ(TIM3_IRQn); if (status == HAL_OK) { } break; default: - return; + return false; } HAL_GPIO_WritePin(port, pin, GPIO_PIN_SET); + return status == HAL_OK; } -void hw_disable_motor(MotorID motor_id) { +bool hw_disable_motor(MotorID motor_id) { void* port; uint16_t pin; HAL_StatusTypeDef status = HAL_OK; @@ -326,30 +329,22 @@ void hw_disable_motor(MotorID motor_id) { port = Z_EN_PORT; pin = Z_EN_PIN; status = HAL_TIM_Base_Stop_IT(&htim20); - if (!status == HAL_OK){ - Error_Handler(); - } break; case MOTOR_X: port = X_EN_PORT; pin = X_EN_PIN; status = HAL_TIM_Base_Stop_IT(&htim17); - if (!status == HAL_OK){ - Error_Handler(); - } break; case MOTOR_L: port = L_EN_PORT; pin = L_EN_PIN; status = HAL_TIM_Base_Stop_IT(&htim3); - if (!status == HAL_OK){ - Error_Handler(); - } break; default: - return; + return false; } HAL_GPIO_WritePin(port, pin, GPIO_PIN_RESET); + return status == HAL_OK; } void step_motor(MotorID motor_id) { diff --git a/stm32-modules/flex-stacker/firmware/motor_control/motor_policy.cpp b/stm32-modules/flex-stacker/firmware/motor_control/motor_policy.cpp index 787e2b53..5510b9e2 100644 --- a/stm32-modules/flex-stacker/firmware/motor_control/motor_policy.cpp +++ b/stm32-modules/flex-stacker/firmware/motor_control/motor_policy.cpp @@ -5,13 +5,13 @@ using namespace motor_policy; // NOLINTNEXTLINE(readability-convert-member-functions-to-static) -auto MotorPolicy::enable_motor(MotorID motor_id) -> void { - hw_enable_motor(motor_id); +auto MotorPolicy::enable_motor(MotorID motor_id) -> bool { + return hw_enable_motor(motor_id); } // NOLINTNEXTLINE(readability-convert-member-functions-to-static) -auto MotorPolicy::disable_motor(MotorID motor_id) -> void { - hw_disable_motor(motor_id); +auto MotorPolicy::disable_motor(MotorID motor_id) -> bool { + return hw_disable_motor(motor_id); } // NOLINTNEXTLINE(readability-convert-member-functions-to-static) diff --git a/stm32-modules/flex-stacker/src/errors.cpp b/stm32-modules/flex-stacker/src/errors.cpp index 2316cba7..00613621 100644 --- a/stm32-modules/flex-stacker/src/errors.cpp +++ b/stm32-modules/flex-stacker/src/errors.cpp @@ -16,6 +16,11 @@ const char* const SYSTEM_SERIAL_NUMBER_HAL_ERROR = const char* const SYSTEM_EEPROM_ERROR = "ERR303:system:EEPROM communication error\n"; const char* const TMC2160_READ_ERROR = "ERR901:TMC2160 driver read error\n"; +const char* const TMC2160_WRITE_ERROR = "ERR902:TMC2160 driver write error\n"; +const char* const TMC2160_INVALID_ADDRESS = "ERR903:TMC2160 invalid address\n"; + +const char* const MOTOR_ENABLE_FAILED = "ERR401:motor enable error\n"; +const char* const MOTOR_DISABLE_FAILED = "ERR401:motor disable error\n"; const char* const UNKNOWN_ERROR = "ERR-1:unknown error code\n"; @@ -36,6 +41,10 @@ auto errors::errorstring(ErrorCode code) -> const char* { HANDLE_CASE(SYSTEM_SERIAL_NUMBER_HAL_ERROR); HANDLE_CASE(SYSTEM_EEPROM_ERROR); HANDLE_CASE(TMC2160_READ_ERROR); + HANDLE_CASE(TMC2160_WRITE_ERROR); + HANDLE_CASE(TMC2160_INVALID_ADDRESS); + HANDLE_CASE(MOTOR_ENABLE_FAILED); + HANDLE_CASE(MOTOR_DISABLE_FAILED); } return UNKNOWN_ERROR; } diff --git a/stm32-modules/flex-stacker/src/motor_utils.cpp b/stm32-modules/flex-stacker/src/motor_utils.cpp new file mode 100644 index 00000000..45827d61 --- /dev/null +++ b/stm32-modules/flex-stacker/src/motor_utils.cpp @@ -0,0 +1,77 @@ +#include "flex-stacker/motor_utils.hpp" + +using namespace motor_util; + +MovementProfile::MovementProfile(uint32_t ticks_per_second, + double start_velocity, double peak_velocity, + double acceleration, MovementType type, + ticks distance) + : _ticks_per_second(ticks_per_second), + _type(type), + _target_distance(distance) { + // Clamp ticks_per_second to at least 1 + _ticks_per_second = std::max(_ticks_per_second, static_cast(1)); + + auto tick_freq = static_cast(_ticks_per_second); + + // Clamp the peak velocity so it is not under start velocity + start_velocity = std::max(start_velocity, static_cast(0.0F)); + acceleration = std::max(acceleration, static_cast(0.0F)); + peak_velocity = std::max(start_velocity, peak_velocity); + + // Convert velocities by just dividing by the tick frequency + _start_velocity = convert_to_fixed_point(start_velocity / tick_freq, radix); + _peak_velocity = convert_to_fixed_point(peak_velocity / tick_freq, radix); + // Acceleration must be dividied by (tick/sec)^2 for unit conversion + _acceleration = + convert_to_fixed_point(acceleration / (tick_freq * tick_freq), radix); + + if (_acceleration <= 0) { + _start_velocity = _peak_velocity; + } + + // Ensures that all movement variables are initialized properly + reset(); +} + +auto MovementProfile::reset() -> void { + _velocity = _start_velocity; + _current_distance = 0; + _tick_tracker = 0; +} + +auto MovementProfile::tick() -> TickReturn { + bool step = false; + // Acceleration gets clamped to _peak_velocity + if (_velocity < _peak_velocity) { + _velocity += _acceleration; + if (_velocity > _peak_velocity) { + _velocity = _peak_velocity; + } + } + auto old_tick_track = _tick_tracker; + _tick_tracker += _velocity; + // The bit _tick_flag represents a "whole" step. Once this flips, + // the code signals that a step should occur + if (((old_tick_track ^ _tick_tracker) & _tick_flag) != 0U) { + step = true; + ++_current_distance; + } + return TickReturn{.done = (_current_distance >= _target_distance && + _type == MovementType::FixedDistance), + .step = step}; +} + +[[nodiscard]] auto MovementProfile::current_velocity() const -> steps_per_tick { + return _velocity; +} + +/** Returns the number of ticks that have been taken.*/ +[[nodiscard]] auto MovementProfile::target_distance() const -> ticks { + return _target_distance; +} + +/** Returns the number of ticks that have been taken.*/ +[[nodiscard]] auto MovementProfile::current_distance() const -> ticks { + return _current_distance; +} diff --git a/stm32-modules/include/flex-stacker/firmware/motor_hardware.h b/stm32-modules/include/flex-stacker/firmware/motor_hardware.h index 129acbc4..a1cb0e78 100644 --- a/stm32-modules/include/flex-stacker/firmware/motor_hardware.h +++ b/stm32-modules/include/flex-stacker/firmware/motor_hardware.h @@ -17,8 +17,8 @@ bool motor_spi_sendreceive(MotorID motor_id, uint8_t *tx_data, uint8_t *rx_data, void step_motor(MotorID motor_id); void unstep_motor(MotorID motor_id); -void hw_enable_motor(MotorID motor_id); -void hw_disable_motor(MotorID motor_id); +bool hw_enable_motor(MotorID motor_id); +bool hw_disable_motor(MotorID motor_id); #ifdef __cplusplus } // extern "C" diff --git a/stm32-modules/include/flex-stacker/firmware/motor_policy.hpp b/stm32-modules/include/flex-stacker/firmware/motor_policy.hpp index e9919dba..0a4452e2 100644 --- a/stm32-modules/include/flex-stacker/firmware/motor_policy.hpp +++ b/stm32-modules/include/flex-stacker/firmware/motor_policy.hpp @@ -8,8 +8,8 @@ namespace motor_policy { class MotorPolicy { public: - auto enable_motor(MotorID motor_id) -> void; - auto disable_motor(MotorID motor_id) -> void; + auto enable_motor(MotorID motor_id) -> bool; + auto disable_motor(MotorID motor_id) -> bool; auto step(MotorID motor_id) -> void; }; diff --git a/stm32-modules/include/flex-stacker/flex-stacker/errors.hpp b/stm32-modules/include/flex-stacker/flex-stacker/errors.hpp index 83693ec0..b43253ed 100644 --- a/stm32-modules/include/flex-stacker/flex-stacker/errors.hpp +++ b/stm32-modules/include/flex-stacker/flex-stacker/errors.hpp @@ -20,6 +20,11 @@ enum class ErrorCode { SYSTEM_EEPROM_ERROR = 303, // 9xx - TMC2160 TMC2160_READ_ERROR = 901, + TMC2160_WRITE_ERROR = 902, + TMC2160_INVALID_ADDRESS = 903, + // 4xx - Motor Errors + MOTOR_ENABLE_FAILED = 401, + MOTOR_DISABLE_FAILED = 402, }; auto errorstring(ErrorCode code) -> const char*; diff --git a/stm32-modules/include/flex-stacker/flex-stacker/gcodes.hpp b/stm32-modules/include/flex-stacker/flex-stacker/gcodes.hpp index fa09b4b8..8c409297 100644 --- a/stm32-modules/include/flex-stacker/flex-stacker/gcodes.hpp +++ b/stm32-modules/include/flex-stacker/flex-stacker/gcodes.hpp @@ -18,16 +18,11 @@ #include "core/gcode_parser.hpp" #include "core/utility.hpp" #include "flex-stacker/errors.hpp" +#include "flex-stacker/gcodes_motor.hpp" #include "systemwide.h" namespace gcode { -auto inline motor_id_to_char(MotorID motor_id) -> const char* { - return static_cast(motor_id == MotorID::MOTOR_X ? "X" - : motor_id == MotorID::MOTOR_Z ? "Z" - : "L"); -} - struct EnterBootloader { /** * EnterBootloader uses the command string "dfu" instead of a gcode to be @@ -237,64 +232,4 @@ struct GetBoardRevision { } }; -struct GetTMCRegister { - MotorID motor_id; - uint8_t reg; - - using ParseResult = std::optional; - static constexpr auto prefix = std::array{'M', '9', '2', '0', ' '}; - - 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 reg_res = gcode::parse_value(working, limit); - if (!reg_res.first.has_value()) { - return std::make_pair(ParseResult(), input); - } - return std::make_pair( - ParseResult(GetTMCRegister{.motor_id = motor_id_val, - .reg = reg_res.first.value()}), - reg_res.second); - } - - template - requires std::forward_iterator && - std::sized_sentinel_for - static auto write_response_into(InputIt buf, InLimit limit, - MotorID motor_id, uint8_t reg, - uint32_t data) -> InputIt { - auto res = snprintf(&*buf, (limit - buf), "M920 %s%u %lu OK\n", - motor_id_to_char(motor_id), reg, data); - if (res <= 0) { - return buf; - } - return buf + res; - } -}; - } // namespace gcode diff --git a/stm32-modules/include/flex-stacker/flex-stacker/gcodes_motor.hpp b/stm32-modules/include/flex-stacker/flex-stacker/gcodes_motor.hpp new file mode 100644 index 00000000..2237319d --- /dev/null +++ b/stm32-modules/include/flex-stacker/flex-stacker/gcodes_motor.hpp @@ -0,0 +1,380 @@ +/* +** Messages that are specific to the motors. +*/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "core/gcode_parser.hpp" +#include "core/utility.hpp" +#include "flex-stacker/errors.hpp" +#include "systemwide.h" + +namespace gcode { + +auto inline motor_id_to_char(MotorID motor_id) -> const char* { + return static_cast(motor_id == MotorID::MOTOR_X ? "X" + : motor_id == MotorID::MOTOR_Z ? "Z" + : "L"); +} + +struct GetTMCRegister { + MotorID motor_id; + uint8_t reg; + + using ParseResult = std::optional; + static constexpr auto prefix = std::array{'M', '9', '2', '0', ' '}; + + 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 reg_res = gcode::parse_value(working, limit); + if (!reg_res.first.has_value()) { + return std::make_pair(ParseResult(), input); + } + return std::make_pair( + ParseResult(GetTMCRegister{.motor_id = motor_id_val, + .reg = reg_res.first.value()}), + reg_res.second); + } + + template + requires std::forward_iterator && + std::sized_sentinel_for + static auto write_response_into(InputIt buf, InLimit limit, + MotorID motor_id, uint8_t reg, + uint32_t data) -> InputIt { + auto res = snprintf(&*buf, (limit - buf), "M920 %s%u %lu OK\n", + motor_id_to_char(motor_id), reg, data); + if (res <= 0) { + return buf; + } + return buf + res; + } +}; + +struct SetTMCRegister { + MotorID motor_id; + uint8_t reg; + uint32_t data; + + using ParseResult = std::optional; + static constexpr auto prefix = std::array{'M', '9', '2', '1', ' '}; + static constexpr const char* response = "M921 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 reg_res = gcode::parse_value(working, limit); + if (!reg_res.first.has_value()) { + return std::make_pair(ParseResult(), input); + } + + std::advance(working, 1); + if (working == limit) { + return std::make_pair(ParseResult(), input); + } + + auto data_res = gcode::parse_value(working, limit); + if (!data_res.first.has_value()) { + return std::make_pair(ParseResult(), input); + } + + return std::make_pair( + ParseResult(SetTMCRegister{.motor_id = motor_id_val, + .reg = reg_res.first.value(), + .data = data_res.first.value()}), + data_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 ArgX { + static constexpr auto prefix = std::array{'X'}; + static constexpr bool required = false; + bool present = false; +}; + +struct ArgZ { + static constexpr auto prefix = std::array{'Z'}; + static constexpr bool required = false; + bool present = false; +}; + +struct ArgL { + static constexpr auto prefix = std::array{'L'}; + static constexpr bool required = false; + bool present = false; +}; + +struct EnableMotor { + std::optional x, z, l; + + using ParseResult = std::optional; + static constexpr auto prefix = std::array{'M', '1', '7', ' '}; + static constexpr const char* response = "M17 OK\n"; + + template + requires std::forward_iterator && + std::sized_sentinel_for + static auto parse(const InputIt& input, Limit limit) + -> std::pair { + // parse for motor engage + std::optional x; + std::optional z; + std::optional l; + 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<0>(arguments).present) { + x = true; + } + if (std::get<1>(arguments).present) { + z = true; + } + if (std::get<2>(arguments).present) { + l = true; + } + return std::make_pair(EnableMotor{.x = x, .z = z, .l = l}, 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 DisableMotor { + std::optional x, z, l; + + using ParseResult = std::optional; + static constexpr auto prefix = std::array{'M', '1', '8', ' '}; + static constexpr const char* response = "M18 OK\n"; + + template + requires std::forward_iterator && + std::sized_sentinel_for + static auto parse(const InputIt& input, Limit limit) + -> std::pair { + std::optional x; + std::optional z; + std::optional l; + 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<0>(arguments).present) { + x = false; + } + if (std::get<1>(arguments).present) { + z = false; + } + if (std::get<2>(arguments).present) { + l = false; + } + return std::make_pair(DisableMotor{.x = x, .z = z, .l = l}, 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 MoveMotorAtFrequency { + MotorID motor_id; + bool direction; + int32_t steps; + uint32_t frequency; + + using ParseResult = std::optional; + static constexpr auto prefix = std::array{'G', '6', ' '}; + static constexpr const char* response = "G6 OK\n"; + + struct XArg { + static constexpr auto prefix = std::array{'X'}; + static constexpr bool required = false; + bool present = false; + }; + struct ZArg { + static constexpr auto prefix = std::array{'Z'}; + static constexpr bool required = false; + bool present = false; + }; + struct LArg { + static constexpr auto prefix = std::array{'L'}; + static constexpr bool required = false; + bool present = false; + }; + struct DirArg { + static constexpr bool required = true; + bool present = false; + bool value = false; + }; + + struct StepArg { + static constexpr auto prefix = std::array{' ', 'S'}; + static constexpr bool required = true; + bool present = false; + int32_t value = 0; + }; + struct FreqArg { + static constexpr auto prefix = std::array{' ', 'F'}; + static constexpr bool required = false; + bool present = false; + uint32_t value = 0; + }; + + template + requires std::forward_iterator && + std::sized_sentinel_for + static auto parse(const InputIt& input, Limit limit) + -> std::pair { + auto res = + gcode::SingleParser::parse_gcode(input, limit, prefix); + if (!res.first.has_value()) { + return std::make_pair(ParseResult(), input); + } + auto ret = MoveMotorAtFrequency{ + .motor_id = MotorID::MOTOR_X, + .direction = false, + .steps = 0, + .frequency = 0, + }; + + auto arguments = res.first.value(); + if (std::get<1>(arguments).present) { + ret.motor_id = MotorID::MOTOR_Z; + } else if (std::get<2>(arguments).present) { + ret.motor_id = MotorID::MOTOR_L; + } else if (!std::get<0>(arguments).present) { + return std::make_pair(ParseResult(), input); + } + + if (std::get<3>(arguments).present) { + ret.direction = std::get<3>(arguments).value; + } else { + return std::make_pair(ParseResult(), input); + } + + if (std::get<4>(arguments).present) { + ret.steps = std::get<4>(arguments).value; + } else { + return std::make_pair(ParseResult(), input); + } + + if (std::get<4>(arguments).present) { + ret.frequency = std::get<4>(arguments).value; + } + return std::make_pair(ret, 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 StopMotor { + using ParseResult = std::optional; + static constexpr auto prefix = std::array{'M', '0'}; + static constexpr const char* response = "M0 OK\n"; + + 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); + } + + template + requires std::forward_iterator && + std::sized_sentinel_for + static auto parse(const InputIt& input, Limit limit) + -> std::pair { + auto working = prefix_matches(input, limit, prefix); + if (working == input) { + return std::make_pair(ParseResult(), input); + } + return std::make_pair(ParseResult(StopMotor()), working); + } +}; + +} // 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 05e9aea7..de700f0f 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 @@ -37,9 +37,14 @@ class HostCommsTask { using Aggregator = typename tasks::Tasks::QueueAggregator; private: - using GCodeParser = gcode::GroupParser; + using GCodeParser = + gcode::GroupParser; using AckOnlyCache = - AckCache<8, gcode::EnterBootloader, gcode::SetSerialNumber>; + AckCache<8, gcode::EnterBootloader, gcode::SetSerialNumber, + gcode::SetTMCRegister, gcode::EnableMotor, gcode::DisableMotor, + gcode::MoveMotorAtFrequency>; using GetSystemInfoCache = AckCache<8, gcode::GetSystemInfo>; using GetTMCRegisterCache = AckCache<8, gcode::GetTMCRegister>; @@ -310,6 +315,122 @@ class HostCommsTask { return std::make_pair(true, tx_into); } + template + requires std::forward_iterator && + std::sized_sentinel_for + auto visit_gcode(const gcode::SetTMCRegister& 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::SetTMCRegisterMessage{.id = id, + .motor_id = gcode.motor_id, + .reg = gcode.reg, + .data = gcode.data}; + 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); + 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 + auto visit_gcode(const gcode::EnableMotor& 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::MotorEnableMessage{ + .id = id, .x = gcode.x, .z = gcode.z, .l = gcode.l}; + 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); + 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 + auto visit_gcode(const gcode::DisableMotor& 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::MotorEnableMessage{ + .id = id, .x = gcode.x, .z = gcode.z, .l = gcode.l}; + 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); + 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 + auto visit_gcode(const gcode::MoveMotorAtFrequency& 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::MoveMotorAtFrequencyMessage{.id = id, + .motor_id = gcode.motor_id, + .direction = gcode.direction, + .steps = gcode.steps, + .frequency = gcode.frequency}; + 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); + 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 + auto visit_gcode(const gcode::StopMotor& 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::StopMotorMessage{.id = 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); + ack_only_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 && diff --git a/stm32-modules/include/flex-stacker/flex-stacker/messages.hpp b/stm32-modules/include/flex-stacker/flex-stacker/messages.hpp index 5c5e0f94..f579d8c6 100644 --- a/stm32-modules/include/flex-stacker/flex-stacker/messages.hpp +++ b/stm32-modules/include/flex-stacker/flex-stacker/messages.hpp @@ -119,6 +119,25 @@ struct GetTMCRegisterResponse { uint32_t data; }; +struct MotorEnableMessage { + uint32_t id = 0; + std::optional x = std::nullopt; + std::optional z = std::nullopt; + std::optional l = std::nullopt; +}; + +struct MoveMotorAtFrequencyMessage { + uint32_t id; + MotorID motor_id; + bool direction; + int32_t steps; + uint32_t frequency; +}; + +struct StopMotorMessage { + uint32_t id; +}; + using HostCommsMessage = ::std::variant; -using MotorMessage = ::std::variant; + +using MotorMessage = + ::std::variant; }; // 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 e7cc0376..b294ef6e 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 @@ -171,8 +171,18 @@ class MotorDriverTask { auto visit_message(const messages::SetTMCRegisterMessage& m, tmc2160::TMC2160Interface& tmc2160_interface) -> void { - static_cast(m); - static_cast(tmc2160_interface); + auto response = messages::AcknowledgePrevious{.responding_to_id = m.id}; + if (!tmc2160::is_valid_address(m.reg)) { + response.with_error = errors::ErrorCode::TMC2160_INVALID_ADDRESS; + } else { + auto result = tmc2160_interface.write(tmc2160::Registers(m.reg), + m.data, m.motor_id); + if (!result) { + response.with_error = errors::ErrorCode::TMC2160_WRITE_ERROR; + } + } + static_cast(_task_registry->send_to_address( + response, Queues::HostCommsAddress)); } template 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 3f8538a4..ce07fddb 100644 --- a/stm32-modules/include/flex-stacker/flex-stacker/motor_task.hpp +++ b/stm32-modules/include/flex-stacker/flex-stacker/motor_task.hpp @@ -9,6 +9,7 @@ #include "core/queue_aggregator.hpp" #include "core/version.hpp" #include "firmware/motor_policy.hpp" +#include "flex-stacker/errors.hpp" #include "flex-stacker/messages.hpp" #include "flex-stacker/tasks.hpp" #include "flex-stacker/tmc2160_registers.hpp" @@ -18,8 +19,8 @@ namespace motor_task { template concept MotorControlPolicy = requires(P p, MotorID motor_id) { - { p.enable_motor(motor_id) } -> std::same_as; - { p.disable_motor(motor_id) } -> std::same_as; + { p.enable_motor(motor_id) } -> std::same_as; + { p.disable_motor(motor_id) } -> std::same_as; }; using Message = messages::MotorMessage; @@ -47,7 +48,6 @@ class MotorTask { template auto run_once(Policy& policy) -> void { - std::ignore = policy; if (!_task_registry) { return; } @@ -55,15 +55,55 @@ class MotorTask { auto message = Message(std::monostate()); _message_queue.recv(&message); - auto visit_helper = [this](auto& message) -> void { - this->visit_message(message); + auto visit_helper = [this, &policy](auto& message) -> void { + this->visit_message(message, policy); }; std::visit(visit_helper, message); } private: - auto visit_message(const std::monostate& message) -> void { - static_cast(message); + template + auto visit_message(const std::monostate& m, Policy& policy) -> void { + static_cast(m); + static_cast(policy); + } + + template + auto visit_message(const messages::MotorEnableMessage& m, Policy& policy) + -> void { + auto response = messages::AcknowledgePrevious{.responding_to_id = m.id}; + if (!(motor_enable(MotorID::MOTOR_X, m.x, policy) && + motor_enable(MotorID::MOTOR_Z, m.z, policy) && + motor_enable(MotorID::MOTOR_L, m.l, policy))) { + response.with_error = m.x ? errors::ErrorCode::MOTOR_ENABLE_FAILED + : errors::ErrorCode::MOTOR_DISABLE_FAILED; + }; + static_cast(_task_registry->send_to_address( + response, Queues::HostCommsAddress)); + } + + template + auto motor_enable(MotorID id, std::optional engage, Policy& policy) + -> bool { + if (!engage.has_value()) { + return true; + } + return engage.value() ? policy.enable_motor(id) + : policy.disable_motor(id); + } + + template + auto visit_message(const messages::MoveMotorAtFrequencyMessage& m, + Policy& policy) -> void { + static_cast(m); + static_cast(policy); + } + + template + auto visit_message(const messages::StopMotorMessage& m, Policy& policy) + -> void { + static_cast(m); + static_cast(policy); } Queue& _message_queue; diff --git a/stm32-modules/include/flex-stacker/flex-stacker/motor_utils.hpp b/stm32-modules/include/flex-stacker/flex-stacker/motor_utils.hpp new file mode 100644 index 00000000..128cd65c --- /dev/null +++ b/stm32-modules/include/flex-stacker/flex-stacker/motor_utils.hpp @@ -0,0 +1,110 @@ +#pragma once + +#include +#include + +#include "core/fixed_point.hpp" + +namespace motor_util { + +enum class Parameter : char { + Velocity = 'V', + Acceleration = 'A', + RunCurrent = 'R', + HoldCurrent = 'H' +}; + +enum class MovementType { + FixedDistance, // This movement goes for a fixed number of steps. + OpenLoop, // This movement goes until a stop switch is hit +}; + +/** + * @brief Encapsulates information about a motor movement profile, and + * generates information about when steps should occur and when the + * movement should end based on a periodic \c tick() function. + * + * @details + * The \c tick() function should be invoked at a fixed frequency, defined + * in the constructor. With each tick, the MovementProfile will: + * + * 1. Accelerate the velocity, if the peak acceleration isn't reached \n + * 2. Return \c step=true if a motor step should occur \n + * 3. Return \c done=true if the movement is over (has reached the requested + * number of steps and is a \c FixedDistance movement) + * + * This class does \e not directly call any functions to move the motor. The + * caller of \c tick() should handle actual signal generation based off of the + * return values. + * + */ +class MovementProfile { + public: + using ticks = uint64_t; + using steps_per_tick = sq0_31; + using steps_per_tick_sq = sq0_31; + + // Radix for all fixed point values + static constexpr int radix = 31; + + struct TickReturn { + bool done; // If true, this movement is done + bool step; // If true, motor should step + }; + + /** + * @brief Construct a new Movement Profile object + * + * @param[in] ticks_per_second Frequency of the motor interrupt + * @param[in] start_velocity Starting velocity in steps per second + * @param[in] peak_velocity Max velocity in steps per second + * @param[in] acceleration Acceleration in steps per second^2. Set to 0 + * or lower for instant acceleration. + * @param[in] type The type of movement to perform. A FixedDistance + * movement will have no deceleration profile. + * @param[in] distance The number of ticks to move. Irrelevant for + * OpenLoop movements. + */ + MovementProfile(uint32_t ticks_per_second, double start_velocity, + double peak_velocity, double acceleration, + MovementType type, ticks distance); + + auto reset() -> void; + + /** + * @brief Call this function for every timer interrupt signalling a tick, + * which should be at the rate \c ticks_per_second + * @note If called after a movement is completed, steps will keep being + * generated. The caller should monitor the return value to know when + * to stop calling tick() + * + * @return TickReturn with information for how hardware driver should act + */ + auto tick() -> TickReturn __attribute__((optimize(3))); + + /** Returns the current motor velocity in steps_per_tick.*/ + [[nodiscard]] auto current_velocity() const -> steps_per_tick; + + /** Returns the target number of ticks for this movement.*/ + [[nodiscard]] auto target_distance() const -> ticks; + + /** Returns the number of ticks that have been taken.*/ + [[nodiscard]] auto current_distance() const -> ticks; + + private: + uint32_t _ticks_per_second; // Tick frequency + steps_per_tick _velocity = 0; // Current velocity + steps_per_tick _start_velocity = 0; // Velocity to start a movement + steps_per_tick _peak_velocity = 0; // Velocity to ramp up to + steps_per_tick_sq _acceleration = 0; // Acceleration in steps/tick^2 + MovementType _type; // Type of movement + ticks _target_distance; // Distance for the movement + ticks _current_distance = 0; // Distance this movement has reached + q31_31 _tick_tracker = 0; // Running tracker for the tick motion + + // When incrementing position tracker, if this bit changes then + // a step should take place. + static constexpr q31_31 _tick_flag = (1 << radix); +}; + +} // namespace motor_util