Skip to content

Commit

Permalink
fix(motor-control): Reimplement motor driver chip errors as poller (#811
Browse files Browse the repository at this point in the history
)

* Revert "Revert "fix(motor-control): Revert motor driver chip errors (#777)" (#796)"

This reverts commit 4169f60.

* add the pin definitions and driver configs

* add driver definitions

* add the can messages

* init the diag pin as input

* implement the pin polling paradime

* return errors if trying to move while there is a motor driver error

* fix struct init

* fix some funtion declerations

* hookup reading the status register

* format

* re-add timeout increase

* brushed motor isn't hooked up to diag
  • Loading branch information
ryanthecoder authored Oct 25, 2024
1 parent 2ffc6ce commit 744fb80
Show file tree
Hide file tree
Showing 90 changed files with 374 additions and 1,019 deletions.
24 changes: 4 additions & 20 deletions gantry/core/tasks_proto.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -55,20 +55,18 @@ static auto eeprom_data_rev_update_builder =
/**
* Start gantry tasks.
*/
auto gantry::tasks::start_tasks(
void gantry::tasks::start_tasks(
can::bus::CanBus& can_bus,
motion_controller::MotionController<lms::BeltConfig>& motion_controller,
spi::hardware::SpiDeviceBase& spi_device,
tmc2130::configs::TMC2130DriverConfig& driver_configs,
motor_hardware_task::MotorHardwareTask& mh_tsk,
i2c::hardware::I2CBase& i2c2,
eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface)
-> interfaces::diag0_handler {
eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) {
auto& can_writer = can_task::start_writer(can_bus);
can_task::start_reader(can_bus);
auto& motion =
mc_task_builder.start(5, "motion controller", motion_controller,
::queues, ::queues, ::queues, ::queues);
auto& motion = mc_task_builder.start(5, "motion controller",
motion_controller, ::queues, ::queues);
auto& tmc2130_driver = motor_driver_task_builder.start(
5, "tmc2130 driver", driver_configs, ::queues, spi_task_client);
auto& move_group =
Expand Down Expand Up @@ -118,15 +116,6 @@ auto gantry::tasks::start_tasks(
::queues.usage_storage_queue = &usage_storage_task.get_queue();

mh_tsk.start_task();

return gantry::tasks::call_run_diag0_interrupt;
}

void gantry::tasks::call_run_diag0_interrupt() {
if (gantry::tasks::get_tasks().motion_controller) {
return gantry::tasks::get_tasks()
.motion_controller->run_diag0_interrupt();
}
}

gantry::queues::QueueClient::QueueClient(can::ids::NodeId this_fw)
Expand All @@ -142,11 +131,6 @@ void gantry::queues::QueueClient::send_motor_driver_queue(
motor_driver_queue->try_write(m);
}

void gantry::queues::QueueClient::send_motor_driver_queue_isr(
const tmc2130::tasks::TaskMessage& m) {
static_cast<void>(motor_driver_queue->try_write_isr(m));
}

void gantry::queues::QueueClient::send_move_group_queue(
const move_group_task::TaskMessage& m) {
move_group_queue->try_write(m);
Expand Down
24 changes: 4 additions & 20 deletions gantry/core/tasks_rev1.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -54,20 +54,18 @@ static auto tail_accessor = eeprom::dev_data::DevDataTailAccessor{queues};
/**
* Start gantry ::tasks.
*/
auto gantry::tasks::start_tasks(
void gantry::tasks::start_tasks(
can::bus::CanBus& can_bus,
motion_controller::MotionController<lms::BeltConfig>& motion_controller,
spi::hardware::SpiDeviceBase& spi_device,
tmc2160::configs::TMC2160DriverConfig& driver_configs,
motor_hardware_task::MotorHardwareTask& mh_tsk,
i2c::hardware::I2CBase& i2c2,
eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface)
-> interfaces::diag0_handler {
eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) {
auto& can_writer = can_task::start_writer(can_bus);
can_task::start_reader(can_bus);
auto& motion =
mc_task_builder.start(5, "motion controller", motion_controller,
::queues, ::queues, ::queues, ::queues);
auto& motion = mc_task_builder.start(5, "motion controller",
motion_controller, ::queues, ::queues);
auto& tmc2160_driver = motor_driver_task_builder.start(
5, "tmc2160 driver", driver_configs, ::queues, spi_task_client);
auto& move_group =
Expand Down Expand Up @@ -117,15 +115,6 @@ auto gantry::tasks::start_tasks(
::queues.usage_storage_queue = &usage_storage_task.get_queue();

mh_tsk.start_task();

return gantry::tasks::call_run_diag0_interrupt;
}

void gantry::tasks::call_run_diag0_interrupt() {
if (gantry::tasks::get_tasks().motion_controller) {
return gantry::tasks::get_tasks()
.motion_controller->run_diag0_interrupt();
}
}

gantry::queues::QueueClient::QueueClient(can::ids::NodeId this_fw)
Expand All @@ -141,11 +130,6 @@ void gantry::queues::QueueClient::send_motor_driver_queue(
motor_driver_queue->try_write(m);
}

void gantry::queues::QueueClient::send_motor_driver_queue_isr(
const tmc2160::tasks::TaskMessage& m) {
static_cast<void>(motor_driver_queue->try_write_isr(m));
}

void gantry::queues::QueueClient::send_move_group_queue(
const move_group_task::TaskMessage& m) {
move_group_queue->try_write(m);
Expand Down
9 changes: 4 additions & 5 deletions gantry/firmware/interfaces_proto.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -244,8 +244,8 @@ static stall_check::StallCheck stallcheck(
* Handler of motor interrupts.
*/
static motor_handler::MotorInterruptHandler motor_interrupt(
motor_queue, gantry::queues::get_queues(), gantry::queues::get_queues(),
motor_hardware_iface, stallcheck, update_position_queue);
motor_queue, gantry::queues::get_queues(), motor_hardware_iface, stallcheck,
update_position_queue);

static auto encoder_background_timer =
motor_encoder::BackgroundTimer(motor_interrupt, motor_hardware_iface);
Expand Down Expand Up @@ -281,14 +281,13 @@ static constexpr auto can_bit_timings =
can::bit_timings::BitTimings<170 * can::bit_timings::MHZ, 100,
500 * can::bit_timings::KHZ, 800>{};

void interfaces::initialize(diag0_handler* call_diag0_handler) {
void interfaces::initialize() {
// Initialize SPI
if (initialize_spi(get_axis_type()) != HAL_OK) {
Error_Handler();
}

initialize_timer(call_motor_handler, call_diag0_handler,
enc_overflow_callback);
initialize_timer(call_motor_handler, enc_overflow_callback);

// Start the can bus
canbus.start(can_bit_timings);
Expand Down
9 changes: 4 additions & 5 deletions gantry/firmware/interfaces_rev1.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -271,8 +271,8 @@ static stall_check::StallCheck stallcheck(
* Handler of motor interrupts.
*/
static motor_handler::MotorInterruptHandler motor_interrupt(
motor_queue, gantry::queues::get_queues(), gantry::queues::get_queues(),
motor_hardware_iface, stallcheck, update_position_queue);
motor_queue, gantry::queues::get_queues(), motor_hardware_iface, stallcheck,
update_position_queue);

static auto encoder_background_timer =
motor_encoder::BackgroundTimer(motor_interrupt, motor_hardware_iface);
Expand Down Expand Up @@ -309,14 +309,13 @@ static constexpr auto can_bit_timings =
can::bit_timings::BitTimings<170 * can::bit_timings::MHZ, 100,
500 * can::bit_timings::KHZ, 800>{};

void interfaces::initialize(diag0_handler* call_diag0_handler) {
void interfaces::initialize() {
// Initialize SPI
if (initialize_spi(get_axis_type()) != HAL_OK) {
Error_Handler();
}

initialize_timer(call_motor_handler, call_diag0_handler,
enc_overflow_callback);
initialize_timer(call_motor_handler, enc_overflow_callback);

// Start the can bus
canbus.start(can_bit_timings);
Expand Down
6 changes: 2 additions & 4 deletions gantry/firmware/main_proto.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,6 @@
#include "gantry/core/tasks_proto.hpp"
#include "i2c/firmware/i2c_comms.hpp"

static interfaces::diag0_handler call_diag0_handler = nullptr;

static auto i2c_comms2 = i2c::hardware::I2C();
static auto i2c_handles = I2CHandlerStruct{};

Expand Down Expand Up @@ -46,11 +44,11 @@ auto main() -> int {

app_update_clear_flags();

interfaces::initialize(&call_diag0_handler);
interfaces::initialize();
i2c_setup(&i2c_handles);
i2c_comms2.set_handle(i2c_handles.i2c2);

call_diag0_handler = gantry::tasks::start_tasks(
gantry::tasks::start_tasks(
interfaces::get_can_bus(), interfaces::get_motor().motion_controller,
interfaces::get_spi(), interfaces::get_driver_config(),
interfaces::get_motor_hardware_task(), i2c_comms2, eeprom_hw_iface);
Expand Down
6 changes: 2 additions & 4 deletions gantry/firmware/main_rev1.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,6 @@
#include "gantry/core/tasks_rev1.hpp"
#include "i2c/firmware/i2c_comms.hpp"

static interfaces::diag0_handler call_diag0_handler = nullptr;

static auto i2c_comms2 = i2c::hardware::I2C();
static auto i2c_handles = I2CHandlerStruct{};

Expand Down Expand Up @@ -46,11 +44,11 @@ auto main() -> int {

app_update_clear_flags();

interfaces::initialize(&call_diag0_handler);
interfaces::initialize();
i2c_setup(&i2c_handles);
i2c_comms2.set_handle(i2c_handles.i2c2);

call_diag0_handler = gantry::tasks::start_tasks(
gantry::tasks::start_tasks(
interfaces::get_can_bus(), interfaces::get_motor().motion_controller,
interfaces::get_spi(), interfaces::get_driver_config(),
interfaces::get_motor_hardware_task(), i2c_comms2, eeprom_hw_iface);
Expand Down
19 changes: 2 additions & 17 deletions gantry/firmware/motor_hardware.c
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,7 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* hspi) {

// Diag0
GPIO_InitStruct.Pin = GPIO_PIN_5;
GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
}
Expand Down Expand Up @@ -127,7 +127,6 @@ HAL_StatusTypeDef initialize_spi(enum GantryAxisType gantry_type) {

static motor_interrupt_callback timer_callback = NULL;
static encoder_overflow_callback enc_overflow_callback = NULL;
static diag0_interrupt_callback* diag0_callback = NULL;


/**
Expand All @@ -149,9 +148,6 @@ void MX_GPIO_Init(void) {
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

HAL_NVIC_SetPriority(EXTI9_5_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);
}

// motor timer: 200kHz from
Expand Down Expand Up @@ -248,16 +244,6 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
}
}

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
if (GPIO_Pin == GPIO_PIN_5) {
if (diag0_callback != NULL) {
if (*diag0_callback != NULL) {
(*diag0_callback)();
}
}
}
}

void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef *htim) {
if (htim == &htim2) {
/* Peripheral clock enable */
Expand All @@ -277,10 +263,9 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef *htim) {
}
}

void initialize_timer(motor_interrupt_callback callback, diag0_interrupt_callback* diag0_int_callback, encoder_overflow_callback enc_callback) {
void initialize_timer(motor_interrupt_callback callback, encoder_overflow_callback enc_callback) {
timer_callback = callback;
enc_overflow_callback = enc_callback;
diag0_callback = diag0_int_callback;
MX_GPIO_Init();
MX_TIM7_Init();
Encoder_GPIO_Init();
Expand Down
3 changes: 1 addition & 2 deletions gantry/firmware/motor_hardware.h
Original file line number Diff line number Diff line change
Expand Up @@ -14,12 +14,11 @@ extern TIM_HandleTypeDef htim2;

typedef void (*motor_interrupt_callback)();
typedef void (*encoder_overflow_callback)(int32_t);
typedef void (*diag0_interrupt_callback)();

HAL_StatusTypeDef initialize_spi(enum GantryAxisType);
void gantry_driver_CLK_init(enum GantryAxisType);

void initialize_timer(motor_interrupt_callback callback, diag0_interrupt_callback* diag0_int_callback, encoder_overflow_callback enc_callback);
void initialize_timer(motor_interrupt_callback callback, encoder_overflow_callback enc_callback);

#ifdef __cplusplus
} // extern "C"
Expand Down
1 change: 0 additions & 1 deletion gantry/firmware/stm32g4xx_it.c
Original file line number Diff line number Diff line change
Expand Up @@ -147,7 +147,6 @@ void FDCAN1_IT0_IRQHandler(void) {
*/
void TIM7_IRQHandler(void) { HAL_TIM_IRQHandler(&htim7); }
void TIM2_IRQHandler(void) { HAL_TIM_IRQHandler(&htim2); }
void EXTI9_5_IRQHandler(void) { HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_5); }


extern void xPortSysTickHandler(void);
Expand Down
8 changes: 3 additions & 5 deletions gantry/simulator/interfaces.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -89,16 +89,14 @@ static stall_check::StallCheck stallcheck(
* Handler of motor interrupts.
*/
static motor_handler::MotorInterruptHandler motor_interrupt(
motor_queue, gantry::queues::get_queues(), gantry::queues::get_queues(),
motor_interface, stallcheck, update_position_queue);
motor_queue, gantry::queues::get_queues(), motor_interface, stallcheck,
update_position_queue);

static motor_interrupt_driver::MotorInterruptDriver A(motor_queue,
motor_interrupt,
motor_interface,
update_position_queue);
void interfaces::initialize(diag0_handler* call_diag0_handler) {
static_cast<void>(call_diag0_handler);
}
void interfaces::initialize() {}

static po::variables_map options{};

Expand Down
6 changes: 2 additions & 4 deletions gantry/simulator/main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,6 @@ void signal_handler(int signum) {
exit(signum);
}

static interfaces::diag0_handler call_diag0_handler = NULL;

int main(int argc, char** argv) {
signal(SIGINT, signal_handler);

Expand All @@ -23,10 +21,10 @@ int main(int argc, char** argv) {
return pcTaskGetName(xTaskGetCurrentTaskHandle());
});

interfaces::initialize(&call_diag0_handler);
interfaces::initialize();
interfaces::initialize_sim(argc, argv);

call_diag0_handler = gantry::tasks::start_tasks(
gantry::tasks::start_tasks(
interfaces::get_can_bus(), interfaces::get_motor().motion_controller,
interfaces::get_spi(), interfaces::get_driver_config(),
interfaces::get_motor_hardware_task(), *interfaces::get_sim_i2c2(),
Expand Down
11 changes: 4 additions & 7 deletions gripper/core/tasks.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,7 @@ static auto eeprom_data_rev_update_builder =
/**
* Start gripper tasks.
*/
auto gripper_tasks::start_tasks(
void gripper_tasks::start_tasks(
can::bus::CanBus& can_bus,
motor_class::Motor<lms::LeadScrewConfig>& z_motor,
brushed_motor::BrushedMotor<lms::GearBoxConfig>& grip_motor,
Expand All @@ -72,8 +72,7 @@ auto gripper_tasks::start_tasks(
sensors::hardware::SensorHardwareBase& sensor_hardware,
eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface,
motor_hardware_task::MotorHardwareTask& zmh_tsk,
motor_hardware_task::MotorHardwareTask& gmh_tsk)
-> z_motor_iface::diag0_handler {
motor_hardware_task::MotorHardwareTask& gmh_tsk) {
auto& can_writer = can_task::start_writer(can_bus);
can_task::start_reader(can_bus);
tasks.can_writer = &can_writer;
Expand Down Expand Up @@ -132,8 +131,8 @@ auto gripper_tasks::start_tasks(
queues.capacitive_sensor_queue_rear =
&capacitive_sensor_task_rear.get_queue();

auto diag0_handler = z_tasks::start_task(
z_motor, spi_device, driver_configs, tasks, queues, tail_accessor);
z_tasks::start_task(z_motor, spi_device, driver_configs, tasks, queues,
tail_accessor);

g_tasks::start_task(grip_motor, tasks, queues, tail_accessor);

Expand All @@ -142,8 +141,6 @@ auto gripper_tasks::start_tasks(

zmh_tsk.start_task();
gmh_tsk.start_task();

return diag0_handler;
}

gripper_tasks::QueueClient::QueueClient(can::ids::NodeId this_fw)
Expand Down
Loading

0 comments on commit 744fb80

Please sign in to comment.