Skip to content

Commit

Permalink
feat(flex-stacker): make move params persist (#464)
Browse files Browse the repository at this point in the history 
* make move params optional for mm-controlled moves

* add gcode to set microsteps

* add Gcode to query move parms
  • Loading branch information
@ahiuchingau
ahiuchingau authored Sep 16, 2024
1 parent 2dcd24c commit b66c27a
Show file tree
Hide file tree
Showing 6 changed files with 404 additions and 52 deletions.
128 changes: 113 additions & 15 deletions stm32-modules/include/flex-stacker/flex-stacker/gcodes_motor.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,6 +87,61 @@ struct GetTMCRegister {
}
};

struct SetMicrosteps {
MotorID motor_id;
uint8_t microsteps_power;

using ParseResult = std::optional<SetMicrosteps>;
static constexpr auto prefix = std::array{'M', '9', '0', '9', ' '};
static constexpr const char* response = "M909 OK\n";

template <typename InputIt, typename Limit>
requires std::forward_iterator<InputIt> &&
std::sized_sentinel_for<Limit, InputIt>
static auto parse(const InputIt& input, Limit limit)
-> std::pair<ParseResult, InputIt> {
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<uint8_t>(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 <typename InputIt, typename InLimit>
requires std::forward_iterator<InputIt> &&
std::sized_sentinel_for<InputIt, InLimit>
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;
Expand Down Expand Up @@ -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<float> mm_per_second, mm_per_second_sq, mm_per_second_discont;

using ParseResult = std::optional<MoveMotorInMm>;
static constexpr auto prefix = std::array{'G', '0', ' '};
Expand All @@ -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;
};
Expand Down Expand Up @@ -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();
Expand All @@ -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) {
Expand All @@ -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<float> mm_per_second, mm_per_second_sq, mm_per_second_discont;

using ParseResult = std::optional<MoveToLimitSwitch>;
static constexpr auto prefix = std::array{'G', '5', ' '};
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -826,4 +875,53 @@ struct GetLimitSwitches {
}
};

struct GetMoveParams {
MotorID motor_id;
using ParseResult = std::optional<GetMoveParams>;
static constexpr auto prefix = std::array{'M', '1', '2', '0'};

template <typename InputIt, typename Limit>
requires std::forward_iterator<InputIt> &&
std::sized_sentinel_for<Limit, InputIt>
static auto parse(const InputIt& input, Limit limit)
-> std::pair<ParseResult, InputIt> {
MotorID motor = MotorID::MOTOR_X;
auto res = gcode::SingleParser<ArgX, ArgZ, ArgL>::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 <typename InputIt, typename InLimit>
requires std::forward_iterator<InputIt> &&
std::sized_sentinel_for<InputIt, InLimit>
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
91 changes: 83 additions & 8 deletions stm32-modules/include/flex-stacker/flex-stacker/host_comms_task.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,23 +35,24 @@ class HostCommsTask {
public:
using Queue = QueueImpl<Message>;
using Aggregator = typename tasks::Tasks<QueueImpl>::QueueAggregator;
using Queues = typename tasks::Tasks<QueueImpl>;

private:
using GCodeParser =
gcode::GroupParser<gcode::GetTMCRegister, gcode::SetTMCRegister,
gcode::SetRunCurrent, gcode::SetHoldCurrent,
gcode::EnableMotor, gcode::DisableMotor,
gcode::MoveMotorInSteps, gcode::MoveToLimitSwitch,
gcode::MoveMotorInMm, gcode::GetLimitSwitches>;
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;
Expand All @@ -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;
Expand Down Expand Up @@ -316,6 +319,29 @@ class HostCommsTask {
cache_entry);
}

template <typename InputIt, typename InputLimit>
requires std::forward_iterator<InputIt> &&
std::sized_sentinel_for<InputLimit, InputIt>
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<decltype(cache_element)>;
if constexpr (std::is_same_v<std::monostate, T>) {
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 <typename InputIt, typename InputLimit>
requires std::forward_iterator<InputIt> &&
std::sized_sentinel_for<InputLimit, InputIt>
Expand Down Expand Up @@ -424,6 +450,32 @@ class HostCommsTask {
return std::make_pair(true, tx_into);
}

template <typename InputIt, typename InputLimit>
requires std::forward_iterator<InputIt> &&
std::sized_sentinel_for<InputLimit, InputIt>
auto visit_gcode(const gcode::SetMicrosteps& gcode, InputIt tx_into,
InputLimit tx_limit) -> std::pair<bool, InputIt> {
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<uint8_t>(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 <typename InputIt, typename InputLimit>
requires std::forward_iterator<InputIt> &&
std::sized_sentinel_for<InputLimit, InputIt>
Expand Down Expand Up @@ -614,6 +666,28 @@ class HostCommsTask {
return std::make_pair(true, tx_into);
}

template <typename InputIt, typename InputLimit>
requires std::forward_iterator<InputIt> &&
std::sized_sentinel_for<InputLimit, InputIt>
auto visit_gcode(const gcode::GetMoveParams& gcode, InputIt tx_into,
InputLimit tx_limit) -> std::pair<bool, InputIt> {
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 <typename InputIt, typename InputLimit>
requires std::forward_iterator<InputIt> &&
Expand All @@ -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;
};

Expand Down
Loading

0 comments on commit b66c27a

Please sign in to comment.