Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

refactor(flex-stacker): linear motion system and motor task #473

Merged
merged 6 commits into from
Nov 7, 2024
Merged

refactor(flex-stacker): linear motion system and motor task #473

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
04420b1
simplify linear motion system config
ahiuchingau Nov 7, 2024
5afa00c
update funcs
ahiuchingau Nov 7, 2024
a3cdc8a
update MotorState
ahiuchingau Nov 7, 2024
124658f
Merge branch 'edge' into refactor_motor-task
ahiuchingau Nov 7, 2024
911b4da
addressing nits
ahiuchingau Nov 7, 2024
4898ab4
Merge branch 'edge' into refactor_motor-task
ahiuchingau Nov 7, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
52 changes: 15 additions & 37 deletions stm32-modules/include/common/core/linear_motion_system.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,61 +5,39 @@

namespace lms {

struct BeltConfig {
// NOLINTNEXTLINE(misc-non-private-member-variables-in-classes)
float pulley_diameter; // mm
[[nodiscard]] constexpr auto get_mm_per_rev() const -> float {
class BeltConfig {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

[nit] If everything is going to be public might as well keep it a struct

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

yes yes

public:
static constexpr auto mm_per_rev(float pulley_diameter) -> float {
return static_cast<float>(pulley_diameter * std::numbers::pi);
}
};

struct LeadScrewConfig {
// NOLINTNEXTLINE(misc-non-private-member-variables-in-classes)
float lead_screw_pitch; // mm/rev
// NOLINTNEXTLINE(misc-non-private-member-variables-in-classes)
float gear_reduction_ratio; // large teeth / small teeth
[[nodiscard]] constexpr auto get_mm_per_rev() const -> float {
class LeadScrewConfig {
public:
static constexpr auto mm_per_rev(float lead_screw_pitch,
float gear_reduction_ratio) -> float {
return lead_screw_pitch / gear_reduction_ratio;
}
};

struct GearBoxConfig {
// NOLINTNEXTLINE(misc-non-private-member-variables-in-classes)
float gear_diameter; // mm
// NOLINTNEXTLINE(misc-non-private-member-variables-in-classes)
float gear_reduction_ratio; // large teeth / small teeth
[[nodiscard]] constexpr auto get_mm_per_rev() const -> float {
class GearBoxConfig {
public:
static constexpr auto mm_per_rev(float gear_diameter,
float gear_reduction_ratio) -> float {
return static_cast<float>((gear_diameter * std::numbers::pi) /
gear_reduction_ratio);
}
};

template <typename MC>
concept MotorMechanicalConfig = requires {
std::is_same_v<MC, BeltConfig> || std::is_same_v<MC, LeadScrewConfig> ||
std::is_same_v<MC, GearBoxConfig>;
};

template <MotorMechanicalConfig MEConfig>
struct LinearMotionSystemConfig {
// NOLINTNEXTLINE(misc-non-private-member-variables-in-classes)
MEConfig mech_config{};
float mm_per_rev;
// NOLINTNEXTLINE(misc-non-private-member-variables-in-classes)
float steps_per_rev{};
float steps_per_rev;
// NOLINTNEXTLINE(misc-non-private-member-variables-in-classes)
float microstep{};
float microstep;
[[nodiscard]] constexpr auto get_usteps_per_mm() const -> float {
return (steps_per_rev * microstep) / (mech_config.get_mm_per_rev());
}
[[nodiscard]] constexpr auto get_usteps_per_um() const -> float {
return (steps_per_rev * microstep) /
// NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
(mech_config.get_mm_per_rev() * 1000.0);
}
[[nodiscard]] constexpr auto get_um_per_step() const -> float {
return (mech_config.get_mm_per_rev()) / (steps_per_rev * microstep) *
// NOLINTNEXTLINE(cppcoreguidelines-avoid-magic-numbers)
1000;
return (steps_per_rev * microstep) / mm_per_rev;
}
};

Expand Down
177 changes: 78 additions & 99 deletions stm32-modules/include/flex-stacker/flex-stacker/motor_task.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,63 +30,65 @@ concept MotorControlPolicy = requires(P p, MotorID motor_id) {
using Message = messages::MotorMessage;
using Controller = motor_interrupt_controller::MotorInterruptController;

static constexpr struct lms::LinearMotionSystemConfig<lms::LeadScrewConfig>
motor_x_config = {
.mech_config = lms::LeadScrewConfig{.lead_screw_pitch = 9.7536,
.gear_reduction_ratio = 1.0},
.steps_per_rev = 200, .microstep = 16,
};
static constexpr struct lms::LinearMotionSystemConfig<lms::LeadScrewConfig>
motor_z_config = {
.mech_config = lms::LeadScrewConfig{.lead_screw_pitch = 9.7536,
.gear_reduction_ratio = 1.0},
.steps_per_rev = 200, .microstep = 16,
};
static constexpr struct lms::LinearMotionSystemConfig<lms::GearBoxConfig>
motor_l_config = {
.mech_config = lms::GearBoxConfig{.gear_diameter = 16.0,
.gear_reduction_ratio = 16.0 / 30.0},
.steps_per_rev = 200, .microstep = 16,
};

struct MotorState {
// NOLINTNEXTLINE(misc-non-private-member-variables-in-classes)
float steps_per_mm;
lms::LinearMotionSystemConfig lms_config;
// 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_usteps_per_mm() const -> float {
return lms_config.get_usteps_per_mm();
}
[[nodiscard]] auto get_speed() const -> float {
return speed_mm_per_sec * steps_per_mm;
return speed_mm_per_sec * get_usteps_per_mm();
}
[[nodiscard]] auto get_accel() const -> float {
return accel_mm_per_sec_sq * steps_per_mm;
return accel_mm_per_sec_sq * get_usteps_per_mm();
}
[[nodiscard]] auto get_speed_discont() const -> float {
return speed_mm_per_sec_discont * steps_per_mm;
return speed_mm_per_sec_discont * get_usteps_per_mm();
}
[[nodiscard]] auto get_distance(float mm) const -> float {
return mm * steps_per_mm;
return mm * get_usteps_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 Defaults {
struct X {
static constexpr float SPEED = 200.0;
static constexpr float ACCELERATION = 50.0;
static constexpr float 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;
static constexpr float MM_PER_REV =
lms::LeadScrewConfig::mm_per_rev(9.7536, 1.0);
static constexpr float STEPS_PER_REV = 200;
static constexpr float MICROSTEP = 16;
};

struct Z {
static constexpr float SPEED = 200.0;
static constexpr float ACCELERATION = 50.0;
static constexpr float SPEED_DISCONT = 5.0;

static constexpr float MM_PER_REV =
lms::LeadScrewConfig::mm_per_rev(9.7536, 1.0);
static constexpr float STEPS_PER_REV = 200;
static constexpr float MICROSTEP = 16;
};

struct L {
static constexpr float SPEED = 200.0;
static constexpr float ACCELERATION = 50.0;
static constexpr float SPEED_DISCONT = 5.0;

static constexpr float MM_PER_REV =
lms::GearBoxConfig::mm_per_rev(16.0, 16.0 / 30.0);
static constexpr float STEPS_PER_REV = 200;
static constexpr float MICROSTEP = 16;
};
};

template <template <class> class QueueImpl>
Expand Down Expand Up @@ -210,46 +212,40 @@ class MotorTask {
-> void {
static_cast<void>(policy);
auto direction = m.mm > 0;
MotorState& state = motor_state(m.motor_id);
if (m.mm_per_second.has_value()) {
motor_state(m.motor_id).speed_mm_per_sec = m.mm_per_second.value();
state.speed_mm_per_sec = m.mm_per_second.value();
}
if (m.mm_per_second_sq.has_value()) {
motor_state(m.motor_id).accel_mm_per_sec_sq =
m.mm_per_second_sq.value();
state.accel_mm_per_sec_sq = m.mm_per_second_sq.value();
}
if (m.mm_per_second_discont.has_value()) {
motor_state(m.motor_id).speed_mm_per_sec_discont =
m.mm_per_second_discont.value();
state.speed_mm_per_sec_discont = m.mm_per_second_discont.value();
}
controller_from_id(m.motor_id)
.start_fixed_movement(
m.id, direction,
motor_state(m.motor_id).get_distance(std::abs(m.mm)),
motor_state(m.motor_id).get_speed_discont(),
motor_state(m.motor_id).get_speed(),
motor_state(m.motor_id).get_accel());
.start_fixed_movement(m.id, direction,
state.get_distance(std::abs(m.mm)),
state.get_speed_discont(), state.get_speed(),
state.get_accel());
}

template <MotorControlPolicy Policy>
auto visit_message(const messages::MoveToLimitSwitchMessage& m,
Policy& policy) -> void {
static_cast<void>(policy);
MotorState& state = motor_state(m.motor_id);
if (m.mm_per_second.has_value()) {
motor_state(m.motor_id).speed_mm_per_sec = m.mm_per_second.value();
state.speed_mm_per_sec = m.mm_per_second.value();
}
if (m.mm_per_second_sq.has_value()) {
motor_state(m.motor_id).accel_mm_per_sec_sq =
m.mm_per_second_sq.value();
state.accel_mm_per_sec_sq = m.mm_per_second_sq.value();
}
if (m.mm_per_second_discont.has_value()) {
motor_state(m.motor_id).speed_mm_per_sec_discont =
m.mm_per_second_discont.value();
state.speed_mm_per_sec_discont = m.mm_per_second_discont.value();
}
controller_from_id(m.motor_id)
.start_movement(m.id, m.direction,
motor_state(m.motor_id).get_speed_discont(),
motor_state(m.motor_id).get_speed(),
motor_state(m.motor_id).get_accel());
.start_movement(m.id, m.direction, state.get_speed_discont(),
state.get_speed(), state.get_accel());
}

template <MotorControlPolicy Policy>
Expand Down Expand Up @@ -299,25 +295,8 @@ class MotorTask {
static_cast<void>(policy);
// sent from the driver task so we know we've written to driver
// successfully
switch (m.motor_id) {
case MotorID::MOTOR_X:
_x_mech_conf.microstep =
static_cast<float>(pow(2, m.microsteps_power));
_x_state.steps_per_mm = _x_mech_conf.get_usteps_per_mm();
break;
case MotorID::MOTOR_Z:
_z_mech_conf.microstep =
static_cast<float>(pow(2, m.microsteps_power));
_z_state.steps_per_mm = _z_mech_conf.get_usteps_per_mm();
break;
case MotorID::MOTOR_L:
_l_mech_conf.microstep =
static_cast<float>(pow(2, m.microsteps_power));
_l_state.steps_per_mm = _l_mech_conf.get_usteps_per_mm();
break;
default:
break;
}
motor_state(m.motor_id).lms_config.microstep =
static_cast<float>(pow(2, m.microsteps_power));
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think that if you make this pow(2.f, m.microsteps_power) it'll remain a float without the static cast

auto response = messages::AcknowledgePrevious{.responding_to_id = m.id};
static_cast<void>(_task_registry->send_to_address(
response, Queues::HostCommsAddress));
Expand All @@ -327,13 +306,13 @@ class MotorTask {
auto visit_message(const messages::GetMoveParamsMessage& m, Policy& policy)
-> void {
static_cast<void>(policy);
MotorState& state = motor_state(m.motor_id);
auto response = messages::GetMoveParamsResponse{
.responding_to_id = m.id,
.motor_id = m.motor_id,
.velocity = motor_state(m.motor_id).speed_mm_per_sec,
.acceleration = motor_state(m.motor_id).accel_mm_per_sec_sq,
.velocity_discont =
motor_state(m.motor_id).speed_mm_per_sec_discont,
.velocity = state.speed_mm_per_sec,
.acceleration = state.accel_mm_per_sec_sq,
.velocity_discont = state.speed_mm_per_sec_discont,
};
static_cast<void>(_task_registry->send_to_address(
response, Queues::HostCommsAddress));
Expand Down Expand Up @@ -367,29 +346,29 @@ class MotorTask {
Controller& _z_controller;
Controller& _l_controller;
bool _initialized;
lms::LinearMotionSystemConfig<lms::LeadScrewConfig> _x_mech_conf =
motor_x_config;
lms::LinearMotionSystemConfig<lms::LeadScrewConfig> _z_mech_conf =
motor_z_config;
lms::LinearMotionSystemConfig<lms::GearBoxConfig> _l_mech_conf =
motor_l_config;
MotorState _x_state{
.steps_per_mm = motor_x_config.get_usteps_per_mm(),
.speed_mm_per_sec = XState::DEFAULT_SPEED,
.accel_mm_per_sec_sq = XState::DEFAULT_SPEED,
.speed_mm_per_sec_discont = XState::DEFAULT_SPEED_DISCONT,
.lms_config = {.mm_per_rev = Defaults::X::MM_PER_REV,
.steps_per_rev = Defaults::X::STEPS_PER_REV,
.microstep = Defaults::X::MICROSTEP},
.speed_mm_per_sec = Defaults::X::SPEED,
.accel_mm_per_sec_sq = Defaults::X::ACCELERATION,
.speed_mm_per_sec_discont = Defaults::X::SPEED_DISCONT,
};
MotorState _z_state{
.steps_per_mm = motor_z_config.get_usteps_per_mm(),
.speed_mm_per_sec = XState::DEFAULT_SPEED,
.accel_mm_per_sec_sq = XState::DEFAULT_SPEED,
.speed_mm_per_sec_discont = XState::DEFAULT_SPEED_DISCONT,
.lms_config = {.mm_per_rev = Defaults::Z::MM_PER_REV,
.steps_per_rev = Defaults::Z::STEPS_PER_REV,
.microstep = Defaults::Z::MICROSTEP},
.speed_mm_per_sec = Defaults::Z::SPEED,
.accel_mm_per_sec_sq = Defaults::Z::ACCELERATION,
.speed_mm_per_sec_discont = Defaults::Z::SPEED_DISCONT,
};
MotorState _l_state{
.steps_per_mm = motor_l_config.get_usteps_per_mm(),
.speed_mm_per_sec = LState::DEFAULT_SPEED,
.accel_mm_per_sec_sq = LState::DEFAULT_SPEED,
.speed_mm_per_sec_discont = LState::DEFAULT_SPEED_DISCONT,
.lms_config = {.mm_per_rev = Defaults::L::MM_PER_REV,
.steps_per_rev = Defaults::L::STEPS_PER_REV,
.microstep = Defaults::L::MICROSTEP},
.speed_mm_per_sec = Defaults::L::SPEED,
.accel_mm_per_sec_sq = Defaults::L::ACCELERATION,
.speed_mm_per_sec_discont = Defaults::L::SPEED_DISCONT,
};
};

Expand Down
Loading