Skip to content

Commit

Permalink
feat(flex-stacker): add motor interrupt handlers (#458)
Browse files Browse the repository at this point in the history 
* PLAT-469 add linear motion system for stacker axes

* pass motor policy to interrupt controller

* add movement profile to interrupt controller
  • Loading branch information
@ahiuchingau
ahiuchingau authored Aug 30, 2024
1 parent 50420fe commit eb20ccb
Show file tree
Hide file tree
Showing 12 changed files with 305 additions and 135 deletions.
22 changes: 15 additions & 7 deletions stm32-modules/flex-stacker/firmware/motor_control/freertos_motor_task.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,13 +12,16 @@ namespace motor_control_task {

// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static auto x_motor_interrupt =
motor_interrupt_controller::MotorInterruptController(MotorID::MOTOR_X);
motor_interrupt_controller::MotorInterruptController(MotorID::MOTOR_X,
nullptr);
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static auto z_motor_interrupt =
motor_interrupt_controller::MotorInterruptController(MotorID::MOTOR_Z);
motor_interrupt_controller::MotorInterruptController(MotorID::MOTOR_Z,
nullptr);
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static auto l_motor_interrupt =
motor_interrupt_controller::MotorInterruptController(MotorID::MOTOR_L);
motor_interrupt_controller::MotorInterruptController(MotorID::MOTOR_L,
nullptr);

enum class Notifications : uint8_t {
INCOMING_MESSAGE = 1,
Expand All @@ -34,20 +37,25 @@ static tasks::FirmwareTasks::MotorQueue
static auto _top_task = motor_task::MotorTask(
_queue, nullptr, x_motor_interrupt, z_motor_interrupt, l_motor_interrupt);

static auto callback_glue(MotorID motor_id) {
[[nodiscard]] static auto callback_glue(MotorID motor_id) {
bool done = false;
switch (motor_id) {
case MotorID::MOTOR_L:
l_motor_interrupt.tick();
done = l_motor_interrupt.tick();
break;
case MotorID::MOTOR_X:
x_motor_interrupt.tick();
done = x_motor_interrupt.tick();
break;
case MotorID::MOTOR_Z:
z_motor_interrupt.tick();
done = z_motor_interrupt.tick();
break;
default:
break;
}
if (done) {
static_cast<void>(_queue.try_send_from_isr(
messages::MoveCompleteMessage{.motor_id = motor_id}));
}
}

auto run(tasks::FirmwareTasks::QueueAggregator* aggregator) -> void {
Expand Down
176 changes: 75 additions & 101 deletions stm32-modules/flex-stacker/firmware/motor_control/motor_hardware.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,13 +87,20 @@ TIM_HandleTypeDef htim17;
TIM_HandleTypeDef htim20;
TIM_HandleTypeDef htim3;

typedef struct PinConfig {
void* port;
uint16_t pin;
uint8_t active_setting;
} PinConfig;

typedef struct stepper_hardware_struct {
bool enabled;
bool moving;
bool direction;
int32_t step_count;
int32_t step_target;
TIM_HandleTypeDef timer;
PinConfig enable;
PinConfig direction;
PinConfig step;
PinConfig limit_switch_minus;
PinConfig limit_switch_plus;
PinConfig ebrake;
} stepper_hardware_t;

typedef struct motor_hardware_struct {
Expand All @@ -107,22 +114,31 @@ typedef struct motor_hardware_struct {
static motor_hardware_t _motor_hardware = {
.initialized = false,
.motor_x = {
.enabled = false,
.moving = false,
.direction = false,
.timer = {0},
.enable = {X_EN_PORT, X_EN_PIN, GPIO_PIN_SET},
.direction = {X_DIR_PORT, X_DIR_PIN, GPIO_PIN_SET},
.step = {X_STEP_PORT, X_STEP_PIN, GPIO_PIN_SET},
.limit_switch_minus = {X_MINUS_LIMIT_PORT, X_MINUS_LIMIT_PIN, GPIO_PIN_SET},
.limit_switch_plus = {X_PLUS_LIMIT_PORT, X_PLUS_LIMIT_PIN, GPIO_PIN_SET},
.ebrake = {0},
},
.motor_z = {
.enabled = false,
.moving = false,
.direction = false,
.timer = {0}
.timer = {0},
.enable = {Z_EN_PORT, Z_EN_PIN, GPIO_PIN_SET},
.direction = {Z_DIR_PORT, Z_DIR_PIN, GPIO_PIN_SET},
.step = {Z_STEP_PORT, Z_STEP_PIN, GPIO_PIN_SET},
.limit_switch_minus = {Z_MINUS_LIMIT_PORT, Z_MINUS_LIMIT_PIN, GPIO_PIN_SET},
.limit_switch_plus = {Z_PLUS_LIMIT_PORT, Z_PLUS_LIMIT_PIN, GPIO_PIN_SET},
.ebrake = {Z_N_BRAKE_PORT, Z_N_BRAKE_PIN, GPIO_PIN_RESET},
},
.motor_l = {
.enabled = false,
.moving = false,
.direction = false,
.timer = {0}
.timer = {0},
.enable = {L_EN_PORT, L_EN_PIN, GPIO_PIN_SET},
.direction = {L_DIR_PORT, L_DIR_PIN, GPIO_PIN_SET},
.step = {L_STEP_PORT, L_STEP_PIN, GPIO_PIN_SET},
.limit_switch_minus = {L_N_HELD_PORT, L_N_HELD_PIN, GPIO_PIN_RESET},
.limit_switch_plus = {L_N_RELEASED_PORT, L_N_RELEASED_PIN, GPIO_PIN_RESET},
.ebrake = {0},
}
};

Expand Down Expand Up @@ -326,109 +342,67 @@ void hw_enable_ebrake(MotorID motor_id, bool enable) {
return;
}

bool hw_enable_motor(MotorID motor_id) {
hw_enable_ebrake(motor_id, false);
void* port;
uint16_t pin;
HAL_StatusTypeDef status = HAL_OK;
stepper_hardware_t get_motor(MotorID motor_id) {
switch (motor_id) {
case MOTOR_Z:
port = Z_EN_PORT;
pin = Z_EN_PIN;
status = HAL_TIM_Base_Start_IT(&_motor_hardware.motor_z.timer);
break;
return _motor_hardware.motor_z;
case MOTOR_X:
port = X_EN_PORT;
pin = X_EN_PIN;
status = HAL_TIM_Base_Start_IT(&_motor_hardware.motor_x.timer);
break;
return _motor_hardware.motor_x;
case MOTOR_L:
port = L_EN_PORT;
pin = L_EN_PIN;
status = HAL_TIM_Base_Start_IT(&_motor_hardware.motor_l.timer);
break;
return _motor_hardware.motor_l;
default:
return false;
return (stepper_hardware_t){0};
}
HAL_GPIO_WritePin(port, pin, GPIO_PIN_SET);
}

static uint8_t invert_gpio_value(uint8_t setting) {
if (setting == GPIO_PIN_SET) {
return GPIO_PIN_RESET;
} else {
return GPIO_PIN_SET;
}
}

bool hw_enable_motor(MotorID motor_id) {
stepper_hardware_t motor = get_motor(motor_id);
hw_enable_ebrake(motor_id, false);
HAL_StatusTypeDef status = HAL_OK;
status = HAL_TIM_Base_Start_IT(&motor.timer);
HAL_GPIO_WritePin(motor.enable.port, motor.enable.pin, motor.enable.active_setting);
return status == HAL_OK;
}

bool hw_disable_motor(MotorID motor_id) {
void* port;
uint16_t pin;
HAL_StatusTypeDef status = HAL_OK;
switch (motor_id) {
case MOTOR_Z:
__HAL_TIM_DISABLE_IT(&_motor_hardware.motor_z.timer, TIM_IT_UPDATE);
port = Z_EN_PORT;
pin = Z_EN_PIN;
status = HAL_TIM_Base_Stop_IT(&_motor_hardware.motor_z.timer);
break;
case MOTOR_X:
__HAL_TIM_DISABLE_IT(&_motor_hardware.motor_x.timer, TIM_IT_UPDATE);
port = X_EN_PORT;
pin = X_EN_PIN;
status = HAL_TIM_Base_Stop_IT(&_motor_hardware.motor_x.timer);
break;
case MOTOR_L:
__HAL_TIM_DISABLE_IT(&_motor_hardware.motor_l.timer, TIM_IT_UPDATE);
port = L_EN_PORT;
pin = L_EN_PIN;
status = HAL_TIM_Base_Stop_IT(&_motor_hardware.motor_l.timer);
break;
default:
return status == HAL_OK;
}
HAL_GPIO_WritePin(port, pin, GPIO_PIN_RESET);
stepper_hardware_t motor = get_motor(motor_id);
HAL_StatusTypeDef status = HAL_OK;
// _HAL_TIM_DISABLE_IT(&motor.timer, TIM_IT_UPDATE);
status = HAL_TIM_Base_Stop_IT(&motor.timer);
HAL_GPIO_WritePin(motor.enable.port, motor.enable.pin, invert_gpio_value(motor.enable.active_setting));
hw_enable_ebrake(motor_id, true);
return status == HAL_OK;
}

void step_motor(MotorID motor_id) {
void* port;
uint16_t pin;
switch (motor_id) {
case MOTOR_Z:
port = Z_STEP_PORT;
pin = Z_STEP_PIN;
break;
case MOTOR_X:
port = X_STEP_PORT;
pin = X_STEP_PIN;
break;
case MOTOR_L:
port = L_STEP_PORT;
pin = L_STEP_PIN;
break;
default:
return;
}
HAL_GPIO_WritePin(port, pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(port, pin, GPIO_PIN_RESET);

void hw_step_motor(MotorID motor_id) {
stepper_hardware_t motor = get_motor(motor_id);
HAL_GPIO_WritePin(motor.step.port, motor.step.pin, motor.step.active_setting);
HAL_GPIO_WritePin(motor.step.port, motor.step.pin, invert_gpio_value(motor.step.active_setting));
}

void hw_set_direction(MotorID motor_id, bool direction) {
void* port;
uint16_t pin;
switch (motor_id) {
case MOTOR_Z:
port = Z_DIR_PORT;
pin = Z_DIR_PIN;
break;
case MOTOR_X:
port = X_DIR_PORT;
pin = X_DIR_PIN;
break;
case MOTOR_L:
port = L_DIR_PORT;
pin = L_DIR_PIN;
break;
default:
return;
stepper_hardware_t motor = get_motor(motor_id);
HAL_GPIO_WritePin(motor.direction.port, motor.direction.pin, direction ? GPIO_PIN_SET : GPIO_PIN_RESET);
}

bool hw_read_limit_switch(MotorID motor_id, bool direction) {
stepper_hardware_t motor = get_motor(motor_id);
if (direction) {
return HAL_GPIO_ReadPin(motor.limit_switch_plus.port,
motor.limit_switch_plus.pin) ==
motor.limit_switch_plus.active_setting;
}
HAL_GPIO_WritePin(port, pin, direction ? GPIO_PIN_SET : GPIO_PIN_RESET);
return HAL_GPIO_ReadPin(motor.limit_switch_minus.port,
motor.limit_switch_minus.pin) ==
motor.limit_switch_minus.active_setting;
}

void TIM3_IRQHandler(void)
Expand Down
12 changes: 11 additions & 1 deletion stm32-modules/flex-stacker/firmware/motor_control/motor_policy.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,4 +15,14 @@ auto MotorPolicy::disable_motor(MotorID motor_id) -> bool {
}

// NOLINTNEXTLINE(readability-convert-member-functions-to-static)
auto MotorPolicy::step(MotorID motor_id) -> void { step_motor(motor_id); }
auto MotorPolicy::step(MotorID motor_id) -> void { hw_step_motor(motor_id); }

// NOLINTNEXTLINE(readability-convert-member-functions-to-static)
auto MotorPolicy::set_direction(MotorID motor_id, bool direction) -> void {
hw_set_direction(motor_id, direction);
}

// NOLINTNEXTLINE(readability-convert-member-functions-to-static)
auto MotorPolicy::check_limit_switch(MotorID motor_id, bool direction) -> bool {
return hw_read_limit_switch(motor_id, direction);
}
1 change: 1 addition & 0 deletions stm32-modules/flex-stacker/src/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@ endif()
# Configure lintable/nonlintable sources here
set(CORE_LINTABLE_SOURCES
${CMAKE_CURRENT_SOURCE_DIR}/errors.cpp
${CMAKE_CURRENT_SOURCE_DIR}/motor_utils.cpp
)
set(CORE_LINTABLE_SOURCES
${CORE_LINTABLE_SOURCES}
Expand Down
5 changes: 5 additions & 0 deletions stm32-modules/flex-stacker/src/motor_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,3 +75,8 @@ auto MovementProfile::tick() -> TickReturn {
[[nodiscard]] auto MovementProfile::current_distance() const -> ticks {
return _current_distance;
}

/** Returns the movement type.*/
[[nodiscard]] auto MovementProfile::movement_type() const -> MovementType {
return _type;
}
66 changes: 66 additions & 0 deletions stm32-modules/include/common/core/linear_motion_system.hpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,66 @@
#pragma once

#include <concepts>
#include <numbers>

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 {
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 {
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 {
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{};
// NOLINTNEXTLINE(misc-non-private-member-variables-in-classes)
float steps_per_rev{};
// NOLINTNEXTLINE(misc-non-private-member-variables-in-classes)
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;
}
};

} // namespace lms
3 changes: 2 additions & 1 deletion stm32-modules/include/flex-stacker/firmware/motor_hardware.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,10 +15,11 @@ void spi_hardware_init(void);
bool motor_spi_sendreceive(MotorID motor_id, uint8_t *tx_data, uint8_t *rx_data,
uint16_t len);

void step_motor(MotorID motor_id);
void hw_step_motor(MotorID motor_id);
bool hw_enable_motor(MotorID motor_id);
bool hw_disable_motor(MotorID motor_id);
void hw_set_direction(MotorID, bool direction);
bool hw_read_limit_switch(MotorID motor_id, bool direction);

#ifdef __cplusplus
} // extern "C"
Expand Down
Loading

0 comments on commit eb20ccb

Please sign in to comment.