Skip to content

Commit 744fb80

Browse files
ryanthecoderryanthecoder
authored
fix(motor-control): Reimplement motor driver chip errors as poller (#811)
* 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
1 parent 2ffc6ce commit 744fb80

File tree

90 files changed

+374
-1019
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

90 files changed

+374
-1019
lines changed

‎gantry/core/tasks_proto.cpp

+4-20
Original file line numberDiff line numberDiff line change
@@ -55,20 +55,18 @@ static auto eeprom_data_rev_update_builder =
5555
/**
5656
* Start gantry tasks.
5757
*/
58-
auto gantry::tasks::start_tasks(
58+
void gantry::tasks::start_tasks(
5959
can::bus::CanBus& can_bus,
6060
motion_controller::MotionController<lms::BeltConfig>& motion_controller,
6161
spi::hardware::SpiDeviceBase& spi_device,
6262
tmc2130::configs::TMC2130DriverConfig& driver_configs,
6363
motor_hardware_task::MotorHardwareTask& mh_tsk,
6464
i2c::hardware::I2CBase& i2c2,
65-
eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface)
66-
-> interfaces::diag0_handler {
65+
eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) {
6766
auto& can_writer = can_task::start_writer(can_bus);
6867
can_task::start_reader(can_bus);
69-
auto& motion =
70-
mc_task_builder.start(5, "motion controller", motion_controller,
71-
::queues, ::queues, ::queues, ::queues);
68+
auto& motion = mc_task_builder.start(5, "motion controller",
69+
motion_controller, ::queues, ::queues);
7270
auto& tmc2130_driver = motor_driver_task_builder.start(
7371
5, "tmc2130 driver", driver_configs, ::queues, spi_task_client);
7472
auto& move_group =
@@ -118,15 +116,6 @@ auto gantry::tasks::start_tasks(
118116
::queues.usage_storage_queue = &usage_storage_task.get_queue();
119117

120118
mh_tsk.start_task();
121-
122-
return gantry::tasks::call_run_diag0_interrupt;
123-
}
124-
125-
void gantry::tasks::call_run_diag0_interrupt() {
126-
if (gantry::tasks::get_tasks().motion_controller) {
127-
return gantry::tasks::get_tasks()
128-
.motion_controller->run_diag0_interrupt();
129-
}
130119
}
131120

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

145-
void gantry::queues::QueueClient::send_motor_driver_queue_isr(
146-
const tmc2130::tasks::TaskMessage& m) {
147-
static_cast<void>(motor_driver_queue->try_write_isr(m));
148-
}
149-
150134
void gantry::queues::QueueClient::send_move_group_queue(
151135
const move_group_task::TaskMessage& m) {
152136
move_group_queue->try_write(m);

‎gantry/core/tasks_rev1.cpp

+4-20
Original file line numberDiff line numberDiff line change
@@ -54,20 +54,18 @@ static auto tail_accessor = eeprom::dev_data::DevDataTailAccessor{queues};
5454
/**
5555
* Start gantry ::tasks.
5656
*/
57-
auto gantry::tasks::start_tasks(
57+
void gantry::tasks::start_tasks(
5858
can::bus::CanBus& can_bus,
5959
motion_controller::MotionController<lms::BeltConfig>& motion_controller,
6060
spi::hardware::SpiDeviceBase& spi_device,
6161
tmc2160::configs::TMC2160DriverConfig& driver_configs,
6262
motor_hardware_task::MotorHardwareTask& mh_tsk,
6363
i2c::hardware::I2CBase& i2c2,
64-
eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface)
65-
-> interfaces::diag0_handler {
64+
eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface) {
6665
auto& can_writer = can_task::start_writer(can_bus);
6766
can_task::start_reader(can_bus);
68-
auto& motion =
69-
mc_task_builder.start(5, "motion controller", motion_controller,
70-
::queues, ::queues, ::queues, ::queues);
67+
auto& motion = mc_task_builder.start(5, "motion controller",
68+
motion_controller, ::queues, ::queues);
7169
auto& tmc2160_driver = motor_driver_task_builder.start(
7270
5, "tmc2160 driver", driver_configs, ::queues, spi_task_client);
7371
auto& move_group =
@@ -117,15 +115,6 @@ auto gantry::tasks::start_tasks(
117115
::queues.usage_storage_queue = &usage_storage_task.get_queue();
118116

119117
mh_tsk.start_task();
120-
121-
return gantry::tasks::call_run_diag0_interrupt;
122-
}
123-
124-
void gantry::tasks::call_run_diag0_interrupt() {
125-
if (gantry::tasks::get_tasks().motion_controller) {
126-
return gantry::tasks::get_tasks()
127-
.motion_controller->run_diag0_interrupt();
128-
}
129118
}
130119

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

144-
void gantry::queues::QueueClient::send_motor_driver_queue_isr(
145-
const tmc2160::tasks::TaskMessage& m) {
146-
static_cast<void>(motor_driver_queue->try_write_isr(m));
147-
}
148-
149133
void gantry::queues::QueueClient::send_move_group_queue(
150134
const move_group_task::TaskMessage& m) {
151135
move_group_queue->try_write(m);

‎gantry/firmware/interfaces_proto.cpp

+4-5
Original file line numberDiff line numberDiff line change
@@ -244,8 +244,8 @@ static stall_check::StallCheck stallcheck(
244244
* Handler of motor interrupts.
245245
*/
246246
static motor_handler::MotorInterruptHandler motor_interrupt(
247-
motor_queue, gantry::queues::get_queues(), gantry::queues::get_queues(),
248-
motor_hardware_iface, stallcheck, update_position_queue);
247+
motor_queue, gantry::queues::get_queues(), motor_hardware_iface, stallcheck,
248+
update_position_queue);
249249

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

284-
void interfaces::initialize(diag0_handler* call_diag0_handler) {
284+
void interfaces::initialize() {
285285
// Initialize SPI
286286
if (initialize_spi(get_axis_type()) != HAL_OK) {
287287
Error_Handler();
288288
}
289289

290-
initialize_timer(call_motor_handler, call_diag0_handler,
291-
enc_overflow_callback);
290+
initialize_timer(call_motor_handler, enc_overflow_callback);
292291

293292
// Start the can bus
294293
canbus.start(can_bit_timings);

‎gantry/firmware/interfaces_rev1.cpp

+4-5
Original file line numberDiff line numberDiff line change
@@ -271,8 +271,8 @@ static stall_check::StallCheck stallcheck(
271271
* Handler of motor interrupts.
272272
*/
273273
static motor_handler::MotorInterruptHandler motor_interrupt(
274-
motor_queue, gantry::queues::get_queues(), gantry::queues::get_queues(),
275-
motor_hardware_iface, stallcheck, update_position_queue);
274+
motor_queue, gantry::queues::get_queues(), motor_hardware_iface, stallcheck,
275+
update_position_queue);
276276

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

312-
void interfaces::initialize(diag0_handler* call_diag0_handler) {
312+
void interfaces::initialize() {
313313
// Initialize SPI
314314
if (initialize_spi(get_axis_type()) != HAL_OK) {
315315
Error_Handler();
316316
}
317317

318-
initialize_timer(call_motor_handler, call_diag0_handler,
319-
enc_overflow_callback);
318+
initialize_timer(call_motor_handler, enc_overflow_callback);
320319

321320
// Start the can bus
322321
canbus.start(can_bit_timings);

‎gantry/firmware/main_proto.cpp

+2-4
Original file line numberDiff line numberDiff line change
@@ -16,8 +16,6 @@
1616
#include "gantry/core/tasks_proto.hpp"
1717
#include "i2c/firmware/i2c_comms.hpp"
1818

19-
static interfaces::diag0_handler call_diag0_handler = nullptr;
20-
2119
static auto i2c_comms2 = i2c::hardware::I2C();
2220
static auto i2c_handles = I2CHandlerStruct{};
2321

@@ -46,11 +44,11 @@ auto main() -> int {
4644

4745
app_update_clear_flags();
4846

49-
interfaces::initialize(&call_diag0_handler);
47+
interfaces::initialize();
5048
i2c_setup(&i2c_handles);
5149
i2c_comms2.set_handle(i2c_handles.i2c2);
5250

53-
call_diag0_handler = gantry::tasks::start_tasks(
51+
gantry::tasks::start_tasks(
5452
interfaces::get_can_bus(), interfaces::get_motor().motion_controller,
5553
interfaces::get_spi(), interfaces::get_driver_config(),
5654
interfaces::get_motor_hardware_task(), i2c_comms2, eeprom_hw_iface);

‎gantry/firmware/main_rev1.cpp

+2-4
Original file line numberDiff line numberDiff line change
@@ -16,8 +16,6 @@
1616
#include "gantry/core/tasks_rev1.hpp"
1717
#include "i2c/firmware/i2c_comms.hpp"
1818

19-
static interfaces::diag0_handler call_diag0_handler = nullptr;
20-
2119
static auto i2c_comms2 = i2c::hardware::I2C();
2220
static auto i2c_handles = I2CHandlerStruct{};
2321

@@ -46,11 +44,11 @@ auto main() -> int {
4644

4745
app_update_clear_flags();
4846

49-
interfaces::initialize(&call_diag0_handler);
47+
interfaces::initialize();
5048
i2c_setup(&i2c_handles);
5149
i2c_comms2.set_handle(i2c_handles.i2c2);
5250

53-
call_diag0_handler = gantry::tasks::start_tasks(
51+
gantry::tasks::start_tasks(
5452
interfaces::get_can_bus(), interfaces::get_motor().motion_controller,
5553
interfaces::get_spi(), interfaces::get_driver_config(),
5654
interfaces::get_motor_hardware_task(), i2c_comms2, eeprom_hw_iface);

‎gantry/firmware/motor_hardware.c

+2-17
Original file line numberDiff line numberDiff line change
@@ -59,7 +59,7 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* hspi) {
5959

6060
// Diag0
6161
GPIO_InitStruct.Pin = GPIO_PIN_5;
62-
GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;
62+
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
6363
GPIO_InitStruct.Pull = GPIO_NOPULL;
6464
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
6565
}
@@ -127,7 +127,6 @@ HAL_StatusTypeDef initialize_spi(enum GantryAxisType gantry_type) {
127127

128128
static motor_interrupt_callback timer_callback = NULL;
129129
static encoder_overflow_callback enc_overflow_callback = NULL;
130-
static diag0_interrupt_callback* diag0_callback = NULL;
131130

132131

133132
/**
@@ -149,9 +148,6 @@ void MX_GPIO_Init(void) {
149148
GPIO_InitStruct.Pull = GPIO_NOPULL;
150149
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
151150
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
152-
153-
HAL_NVIC_SetPriority(EXTI9_5_IRQn, 5, 0);
154-
HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);
155151
}
156152

157153
// motor timer: 200kHz from
@@ -248,16 +244,6 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
248244
}
249245
}
250246

251-
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
252-
if (GPIO_Pin == GPIO_PIN_5) {
253-
if (diag0_callback != NULL) {
254-
if (*diag0_callback != NULL) {
255-
(*diag0_callback)();
256-
}
257-
}
258-
}
259-
}
260-
261247
void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef *htim) {
262248
if (htim == &htim2) {
263249
/* Peripheral clock enable */
@@ -277,10 +263,9 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef *htim) {
277263
}
278264
}
279265

280-
void initialize_timer(motor_interrupt_callback callback, diag0_interrupt_callback* diag0_int_callback, encoder_overflow_callback enc_callback) {
266+
void initialize_timer(motor_interrupt_callback callback, encoder_overflow_callback enc_callback) {
281267
timer_callback = callback;
282268
enc_overflow_callback = enc_callback;
283-
diag0_callback = diag0_int_callback;
284269
MX_GPIO_Init();
285270
MX_TIM7_Init();
286271
Encoder_GPIO_Init();

‎gantry/firmware/motor_hardware.h

+1-2
Original file line numberDiff line numberDiff line change
@@ -14,12 +14,11 @@ extern TIM_HandleTypeDef htim2;
1414

1515
typedef void (*motor_interrupt_callback)();
1616
typedef void (*encoder_overflow_callback)(int32_t);
17-
typedef void (*diag0_interrupt_callback)();
1817

1918
HAL_StatusTypeDef initialize_spi(enum GantryAxisType);
2019
void gantry_driver_CLK_init(enum GantryAxisType);
2120

22-
void initialize_timer(motor_interrupt_callback callback, diag0_interrupt_callback* diag0_int_callback, encoder_overflow_callback enc_callback);
21+
void initialize_timer(motor_interrupt_callback callback, encoder_overflow_callback enc_callback);
2322

2423
#ifdef __cplusplus
2524
} // extern "C"

‎gantry/firmware/stm32g4xx_it.c

-1
Original file line numberDiff line numberDiff line change
@@ -147,7 +147,6 @@ void FDCAN1_IT0_IRQHandler(void) {
147147
*/
148148
void TIM7_IRQHandler(void) { HAL_TIM_IRQHandler(&htim7); }
149149
void TIM2_IRQHandler(void) { HAL_TIM_IRQHandler(&htim2); }
150-
void EXTI9_5_IRQHandler(void) { HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_5); }
151150

152151

153152
extern void xPortSysTickHandler(void);

‎gantry/simulator/interfaces.cpp

+3-5
Original file line numberDiff line numberDiff line change
@@ -89,16 +89,14 @@ static stall_check::StallCheck stallcheck(
8989
* Handler of motor interrupts.
9090
*/
9191
static motor_handler::MotorInterruptHandler motor_interrupt(
92-
motor_queue, gantry::queues::get_queues(), gantry::queues::get_queues(),
93-
motor_interface, stallcheck, update_position_queue);
92+
motor_queue, gantry::queues::get_queues(), motor_interface, stallcheck,
93+
update_position_queue);
9494

9595
static motor_interrupt_driver::MotorInterruptDriver A(motor_queue,
9696
motor_interrupt,
9797
motor_interface,
9898
update_position_queue);
99-
void interfaces::initialize(diag0_handler* call_diag0_handler) {
100-
static_cast<void>(call_diag0_handler);
101-
}
99+
void interfaces::initialize() {}
102100

103101
static po::variables_map options{};
104102

‎gantry/simulator/main.cpp

+2-4
Original file line numberDiff line numberDiff line change
@@ -12,8 +12,6 @@ void signal_handler(int signum) {
1212
exit(signum);
1313
}
1414

15-
static interfaces::diag0_handler call_diag0_handler = NULL;
16-
1715
int main(int argc, char** argv) {
1816
signal(SIGINT, signal_handler);
1917

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

26-
interfaces::initialize(&call_diag0_handler);
24+
interfaces::initialize();
2725
interfaces::initialize_sim(argc, argv);
2826

29-
call_diag0_handler = gantry::tasks::start_tasks(
27+
gantry::tasks::start_tasks(
3028
interfaces::get_can_bus(), interfaces::get_motor().motion_controller,
3129
interfaces::get_spi(), interfaces::get_driver_config(),
3230
interfaces::get_motor_hardware_task(), *interfaces::get_sim_i2c2(),

‎gripper/core/tasks.cpp

+4-7
Original file line numberDiff line numberDiff line change
@@ -62,7 +62,7 @@ static auto eeprom_data_rev_update_builder =
6262
/**
6363
* Start gripper tasks.
6464
*/
65-
auto gripper_tasks::start_tasks(
65+
void gripper_tasks::start_tasks(
6666
can::bus::CanBus& can_bus,
6767
motor_class::Motor<lms::LeadScrewConfig>& z_motor,
6868
brushed_motor::BrushedMotor<lms::GearBoxConfig>& grip_motor,
@@ -72,8 +72,7 @@ auto gripper_tasks::start_tasks(
7272
sensors::hardware::SensorHardwareBase& sensor_hardware,
7373
eeprom::hardware_iface::EEPromHardwareIface& eeprom_hw_iface,
7474
motor_hardware_task::MotorHardwareTask& zmh_tsk,
75-
motor_hardware_task::MotorHardwareTask& gmh_tsk)
76-
-> z_motor_iface::diag0_handler {
75+
motor_hardware_task::MotorHardwareTask& gmh_tsk) {
7776
auto& can_writer = can_task::start_writer(can_bus);
7877
can_task::start_reader(can_bus);
7978
tasks.can_writer = &can_writer;
@@ -132,8 +131,8 @@ auto gripper_tasks::start_tasks(
132131
queues.capacitive_sensor_queue_rear =
133132
&capacitive_sensor_task_rear.get_queue();
134133

135-
auto diag0_handler = z_tasks::start_task(
136-
z_motor, spi_device, driver_configs, tasks, queues, tail_accessor);
134+
z_tasks::start_task(z_motor, spi_device, driver_configs, tasks, queues,
135+
tail_accessor);
137136

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

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

143142
zmh_tsk.start_task();
144143
gmh_tsk.start_task();
145-
146-
return diag0_handler;
147144
}
148145

149146
gripper_tasks::QueueClient::QueueClient(can::ids::NodeId this_fw)

0 commit comments

Comments
 (0)