Skip to content

Commit

Permalink
add stream stallguard as an optional param for move messages
Browse files Browse the repository at this point in the history
  • Loading branch information
@ahiuchingau
ahiuchingau committed Oct 4, 2024
1 parent 6ac3e65 commit e2a3ab2
Show file tree
Hide file tree
Showing 5 changed files with 89 additions and 32 deletions.
35 changes: 18 additions & 17 deletions stm32-modules/flex-stacker/firmware/motor_control/freertos_motor_driver_task.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,24 +40,25 @@ static void run_stallguard_task(void* arg) {
while (true) {
// wait for a new notification
xTaskNotifyWait(0, ULONG_MAX, &ulNotifiedValue, portMAX_DELAY);
motor_id = (ulNotifiedValue == 1) ? MotorID::MOTOR_X
: (ulNotifiedValue == 2) ? MotorID::MOTOR_Z
: MotorID::MOTOR_L;
static_cast<void>(interface->read_stallguard(motor_id));
for (;;) {
if (xTaskNotifyWait(ULONG_MAX, ULONG_MAX, &ulNotifiedValue, 0) ==
pdPASS) {
// Received a notification! This notification should only be
// used to break the for loop and the value would not be read.
// This, together with the task suspend issued by the top task,
// makes sure that we can't just switch from the current motor
// to another one immediately
break;
if (ulNotifiedValue > 0) {
motor_id = (ulNotifiedValue == 1) ? MotorID::MOTOR_X
: (ulNotifiedValue == 2) ? MotorID::MOTOR_Z
: MotorID::MOTOR_L;
static_cast<void>(interface->read_stallguard(motor_id));
for (;;) {
if (xTaskNotifyWait(ULONG_MAX, ULONG_MAX, NULL, 0) == pdPASS) {
// Received a notification! This notification should only be
// used to break the for loop and the value would not be
// read. This, together with the task suspend issued by the
// top task, makes sure that we can't just switch from the
// current motor to another one immediately
break;
}
auto result = interface->read_stallguard(motor_id);
auto msg = messages::StallGuardResultMessage{.data = result};
static_cast<void>(_queue.try_send_from_isr(msg));
vTaskDelay(pdMS_TO_TICKS(FREQ_MS));
}
auto result = interface->read_stallguard(motor_id);
auto msg = messages::StallGuardResultMessage{.data = result};
static_cast<void>(_queue.try_send_from_isr(msg));
vTaskDelay(pdMS_TO_TICKS(FREQ_MS));
}
}
}
Expand Down
41 changes: 35 additions & 6 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 @@ -440,6 +440,7 @@ struct MoveMotorInSteps {
int32_t steps;
uint32_t steps_per_second;
uint32_t steps_per_second_sq;
bool stream_stallguard;

using ParseResult = std::optional<MoveMotorInSteps>;
static constexpr auto prefix = std::array{'G', '0', '.', 'S', ' '};
Expand All @@ -450,15 +451,16 @@ struct MoveMotorInSteps {
using LArg = Arg<int32_t, 'L'>;
using FreqArg = Arg<uint32_t, 'F'>;
using RampArg = Arg<uint32_t, 'R'>;
using StreamArg = ArgNoVal<'S'>;

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> {
auto res =
gcode::SingleParser<XArg, ZArg, LArg, FreqArg,
RampArg>::parse_gcode(input, limit, prefix);
gcode::SingleParser<XArg, ZArg, LArg, FreqArg, RampArg,
StreamArg>::parse_gcode(input, limit, prefix);
if (!res.first.has_value()) {
return std::make_pair(ParseResult(), input);
}
Expand All @@ -467,6 +469,7 @@ struct MoveMotorInSteps {
.steps = 0,
.steps_per_second = 0,
.steps_per_second_sq = 0,
.stream_stallguard = false,
};

auto arguments = res.first.value();
Expand All @@ -491,6 +494,10 @@ struct MoveMotorInSteps {
if (std::get<4>(arguments).present) {
ret.steps_per_second_sq = std::get<4>(arguments).value;
}

if (std::get<5>(arguments).present) {
ret.stream_stallguard = true;
}
return std::make_pair(ret, res.second);
}

Expand All @@ -505,6 +512,7 @@ struct MoveMotorInSteps {
struct MoveMotorInMm {
MotorID motor_id;
float mm;
bool stream_stallguard;
std::optional<float> mm_per_second, mm_per_second_sq, mm_per_second_discont;

using ParseResult = std::optional<MoveMotorInMm>;
Expand All @@ -517,21 +525,23 @@ struct MoveMotorInMm {
using VelArg = Arg<float, 'V'>;
using AccelArg = Arg<float, 'A'>;
using DiscontArg = Arg<float, 'D'>;
using StreamArg = ArgNoVal<'S'>;

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> {
auto res =
gcode::SingleParser<XArg, ZArg, LArg, VelArg, AccelArg,
DiscontArg>::parse_gcode(input, limit, prefix);
gcode::SingleParser<XArg, ZArg, LArg, VelArg, AccelArg, DiscontArg,
StreamArg>::parse_gcode(input, limit, prefix);
if (!res.first.has_value()) {
return std::make_pair(ParseResult(), input);
}
auto ret = MoveMotorInMm{
.motor_id = MotorID::MOTOR_X,
.mm = 0.0,
.stream_stallguard = false,
.mm_per_second = std::nullopt,
.mm_per_second_sq = std::nullopt,
.mm_per_second_discont = std::nullopt,
Expand Down Expand Up @@ -562,6 +572,10 @@ struct MoveMotorInMm {
// NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
ret.mm_per_second_discont = std::get<5>(arguments).value;
}
if (std::get<6>(arguments).present) {
// NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
ret.stream_stallguard = true;
}
return std::make_pair(ret, res.second);
}

Expand All @@ -576,6 +590,7 @@ struct MoveMotorInMm {
struct MoveToLimitSwitch {
MotorID motor_id;
bool direction;
bool stream_stallguard;
std::optional<float> mm_per_second, mm_per_second_sq, mm_per_second_discont;

using ParseResult = std::optional<MoveToLimitSwitch>;
Expand All @@ -588,21 +603,23 @@ struct MoveToLimitSwitch {
using VelArg = Arg<float, 'V'>;
using AccelArg = Arg<float, 'A'>;
using DiscontArg = Arg<float, 'D'>;
using StreamArg = ArgNoVal<'S'>;

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> {
auto res =
gcode::SingleParser<XArg, ZArg, LArg, VelArg, AccelArg,
DiscontArg>::parse_gcode(input, limit, prefix);
gcode::SingleParser<XArg, ZArg, LArg, VelArg, AccelArg, DiscontArg,
StreamArg>::parse_gcode(input, limit, prefix);
if (!res.first.has_value()) {
return std::make_pair(ParseResult(), input);
}
auto ret = MoveToLimitSwitch{
.motor_id = MotorID::MOTOR_X,
.direction = false,
.stream_stallguard = false,
.mm_per_second = std::nullopt,
.mm_per_second_sq = std::nullopt,
.mm_per_second_discont = std::nullopt,
Expand Down Expand Up @@ -635,6 +652,12 @@ struct MoveToLimitSwitch {
// NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
ret.mm_per_second_discont = std::get<5>(arguments).value;
}

// NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
if (std::get<6>(arguments).present) {
// NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
ret.stream_stallguard = true;
}
return std::make_pair(ret, res.second);
}

Expand All @@ -650,6 +673,7 @@ struct MoveMotor {
MotorID motor_id;
bool direction;
uint32_t frequency;
bool stream_stallguard;

using ParseResult = std::optional<MoveMotor>;
static constexpr auto prefix = std::array{'G', '5', ' '};
Expand All @@ -659,6 +683,7 @@ struct MoveMotor {
using ZArg = Arg<int, 'Z'>;
using LArg = Arg<int, 'L'>;
using FreqArg = Arg<uint32_t, 'F'>;
using StreamArg = ArgNoVal<'S'>;

template <typename InputIt, typename Limit>
requires std::forward_iterator<InputIt> &&
Expand All @@ -674,6 +699,7 @@ struct MoveMotor {
.motor_id = MotorID::MOTOR_X,
.direction = false,
.frequency = 0,
.stream_stallguard = false,
};

auto arguments = res.first.value();
Expand All @@ -692,6 +718,9 @@ struct MoveMotor {
if (std::get<3>(arguments).present) {
ret.frequency = std::get<3>(arguments).value;
}
if (std::get<4>(arguments).present) {
ret.stream_stallguard = true;
}
return std::make_pair(ret, res.second);
}

Expand Down
15 changes: 10 additions & 5 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 @@ -545,7 +545,8 @@ class HostCommsTask {
.motor_id = gcode.motor_id,
.steps = gcode.steps,
.steps_per_second = gcode.steps_per_second,
.steps_per_second_sq = gcode.steps_per_second_sq};
.steps_per_second_sq = gcode.steps_per_second_sq,
.stream_stallguard = gcode.stream_stallguard};
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);
Expand All @@ -570,6 +571,7 @@ class HostCommsTask {
.id = id,
.motor_id = gcode.motor_id,
.mm = gcode.mm,
.stream_stallguard = gcode.stream_stallguard,
.mm_per_second = gcode.mm_per_second,
.mm_per_second_sq = gcode.mm_per_second_sq,
.mm_per_second_discont = gcode.mm_per_second_discont};
Expand Down Expand Up @@ -597,6 +599,7 @@ class HostCommsTask {
.id = id,
.motor_id = gcode.motor_id,
.direction = gcode.direction,
.stream_stallguard = gcode.stream_stallguard,
.mm_per_second = gcode.mm_per_second,
.mm_per_second_sq = gcode.mm_per_second_sq,
.mm_per_second_discont = gcode.mm_per_second_discont};
Expand All @@ -620,10 +623,12 @@ class HostCommsTask {
false, errors::write_into(tx_into, tx_limit,
errors::ErrorCode::GCODE_CACHE_FULL));
}
auto message = messages::MoveMotorMessage{.id = id,
.motor_id = gcode.motor_id,
.direction = gcode.direction,
.frequency = gcode.frequency};
auto message = messages::MoveMotorMessage{
.id = id,
.motor_id = gcode.motor_id,
.direction = gcode.direction,
.frequency = gcode.frequency,
.stream_stallguard = gcode.stream_stallguard};
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);
Expand Down
4 changes: 4 additions & 0 deletions stm32-modules/include/flex-stacker/flex-stacker/messages.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -146,12 +146,14 @@ struct MoveMotorInStepsMessage {
int32_t steps;
uint32_t steps_per_second;
uint32_t steps_per_second_sq;
bool stream_stallguard;
};

struct MoveMotorInMmMessage {
uint32_t id = 0;
MotorID motor_id = MotorID::MOTOR_X;
float mm = 0;
bool stream_stallguard = false;
std::optional<float> mm_per_second = std::nullopt;
std::optional<float> mm_per_second_sq = std::nullopt;
std::optional<float> mm_per_second_discont = std::nullopt;
Expand All @@ -161,6 +163,7 @@ struct MoveToLimitSwitchMessage {
uint32_t id = 0;
MotorID motor_id = MotorID::MOTOR_X;
bool direction = false;
bool stream_stallguard = false;
std::optional<float> mm_per_second = std::nullopt;
std::optional<float> mm_per_second_sq = std::nullopt;
std::optional<float> mm_per_second_discont = std::nullopt;
Expand Down Expand Up @@ -189,6 +192,7 @@ struct MoveMotorMessage {
MotorID motor_id;
bool direction;
uint32_t frequency;
bool stream_stallguard;
};

struct StopMotorMessage {
Expand Down
26 changes: 22 additions & 4 deletions stm32-modules/include/flex-stacker/flex-stacker/motor_task.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -202,6 +202,12 @@ class MotorTask {
controller_from_id(m.motor_id)
.start_fixed_movement(m.id, direction, std::abs(m.steps), 0,
m.steps_per_second, m.steps_per_second_sq);
if (m.stream_stallguard) {
auto driver_message = messages::PollStallGuardMessage{
.id = m.id, .motor_id = m.motor_id};
static_cast<void>(_task_registry->send_to_address(
driver_message, Queues::MotorDriverAddress));
}
}

template <MotorControlPolicy Policy>
Expand All @@ -228,10 +234,12 @@ class MotorTask {
motor_state(m.motor_id).get_speed(),
motor_state(m.motor_id).get_accel());

auto driver_message =
messages::PollStallGuardMessage{.id = m.id, .motor_id = m.motor_id};
static_cast<void>(_task_registry->send_to_address(
driver_message, Queues::MotorDriverAddress));
if (m.stream_stallguard) {
auto driver_message = messages::PollStallGuardMessage{
.id = m.id, .motor_id = m.motor_id};
static_cast<void>(_task_registry->send_to_address(
driver_message, Queues::MotorDriverAddress));
}
}

template <MotorControlPolicy Policy>
Expand All @@ -254,13 +262,23 @@ class MotorTask {
motor_state(m.motor_id).get_speed_discont(),
motor_state(m.motor_id).get_speed(),
motor_state(m.motor_id).get_accel());
if (m.stream_stallguard) {
auto driver_message = messages::PollStallGuardMessage{
.id = m.id, .motor_id = m.motor_id};
static_cast<void>(_task_registry->send_to_address(
driver_message, Queues::MotorDriverAddress));
}
}

template <MotorControlPolicy Policy>
auto visit_message(const messages::StopMotorMessage& m, Policy& policy)
-> void {
static_cast<void>(m);
static_cast<void>(policy);
// stops stallguard streaming task if it's running
auto driver_message = messages::StopPollStallGuardMessage{};
static_cast<void>(_task_registry->send_to_address(
driver_message, Queues::MotorDriverAddress));
}

template <MotorControlPolicy Policy>
Expand Down

0 comments on commit e2a3ab2

Please sign in to comment.