Skip to content

Commit

Permalink
refactored debounce mechanism, changed existing-error-present message…
Browse files Browse the repository at this point in the history 
…, cleaned up motion controller code
  • Loading branch information
@pmoegenburg
pmoegenburg committed Feb 12, 2024
1 parent 62a09c8 commit 62e5825
Show file tree
Hide file tree
Showing 6 changed files with 18 additions and 214 deletions.
1 change: 0 additions & 1 deletion include/can/core/ids.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,6 @@ enum class MessageId {
read_motor_current_response = 0x35,
read_motor_driver_error_status_request = 0x36,
read_motor_driver_error_status_response = 0x37,
motor_driver_in_error_state = 0x38,
set_brushed_motor_vref_request = 0x40,
set_brushed_motor_pwm_request = 0x41,
gripper_grip_request = 0x42,
Expand Down
5 changes: 1 addition & 4 deletions include/can/core/messages.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -198,8 +198,6 @@ using MotorPositionRequest = Empty<MessageId::motor_position_request>;
using UpdateMotorPositionEstimationRequest =
Empty<MessageId::update_motor_position_estimation_request>;

using MotorDriverInErrorState = Empty<MessageId::motor_driver_in_error_state>;

struct WriteToEEPromRequest : BaseMessage<MessageId::write_eeprom> {
uint32_t message_index;
eeprom::types::address address;
Expand Down Expand Up @@ -1614,7 +1612,6 @@ using ResponseMessageType = std::variant<
PeripheralStatusResponse, BrushedMotorConfResponse,
UpdateMotorPositionEstimationResponse, BaselineSensorResponse,
PushTipPresenceNotification, GetMotorUsageResponse, GripperJawStateResponse,
GripperJawHoldoffResponse, ReadMotorDriverErrorStatusResponse,
MotorDriverInErrorState>;
GripperJawHoldoffResponse, ReadMotorDriverErrorStatusResponse>;

} // namespace can::messages
96 changes: 1 addition & 95 deletions include/motor-control/core/tasks/messages.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,103 +6,9 @@

namespace motor_control_task_messages {

/*enum class MessageId {
route_motor_driver_interrupt = 0x0,
motor_driver_error_encountered = 0x1,
reset_motor_driver_error_handling = 0x2,
};
template <MessageId MId>
struct BaseMessage {
static const auto id = MId;
auto operator==(const BaseMessage& other) const -> bool = default;
};
template <MessageId MId>
struct Empty : BaseMessage<MId> {
uint32_t message_index;
template <bit_utils::ByteIterator Input, typename Limit>
static auto parse(Input body, Limit limit) -> Empty {
uint32_t msg_ind = 0;
body = bit_utils::bytes_to_int(body, limit, msg_ind);
return Empty{.message_index = msg_ind};
}
template <bit_utils::ByteIterator Output, typename Limit>
auto serialize(Output body, Limit limit) const -> uint8_t {
auto iter = bit_utils::int_to_bytes(message_index, body, limit);
return iter - body;
}
auto operator==(const Empty&) const -> bool = default;
};
using RouteMotorDriverInterrupt =
Empty<MessageId::route_motor_driver_interrupt>;
using MotorDriverErrorEncountered =
Empty<MessageId::motor_driver_error_encountered>;
using ResetMotorDriverErrorHandling =
Empty<MessageId::reset_motor_driver_error_handling>;
struct RouteMotorDriverInterrupt {
uint32_t message_index;
template <bit_utils::ByteIterator Input, typename Limit>
static auto parse(Input body, Limit limit) -> RouteMotorDriverInterrupt {
uint32_t msg_ind = 0;
body = bit_utils::bytes_to_int(body, limit, msg_ind);
return RouteMotorDriverInterrupt{.message_index = msg_ind};
}
template <bit_utils::ByteIterator Output, typename Limit>
auto serialize(Output body, Limit limit) const -> uint8_t {
auto iter = bit_utils::int_to_bytes(message_index, body, limit);
return iter - body;
}
auto operator==(const RouteMotorDriverInterrupt&) const -> bool = default;
};
struct MotorDriverErrorEncountered {
uint32_t message_index;
template <bit_utils::ByteIterator Input, typename Limit>
static auto parse(Input body, Limit limit) -> MotorDriverErrorEncountered {
uint32_t msg_ind = 0;
body = bit_utils::bytes_to_int(body, limit, msg_ind);
return MotorDriverErrorEncountered{.message_index = msg_ind};
}
template <bit_utils::ByteIterator Output, typename Limit>
auto serialize(Output body, Limit limit) const -> uint8_t {
auto iter = bit_utils::int_to_bytes(message_index, body, limit);
return iter - body;
}
auto operator==(const MotorDriverErrorEncountered&) const -> bool = default;
};
struct ResetMotorDriverErrorHandling {
uint32_t message_index;
template <bit_utils::ByteIterator Input, typename Limit>
static auto parse(Input body, Limit limit) -> ResetMotorDriverErrorHandling
{ uint32_t msg_ind = 0; body = bit_utils::bytes_to_int(body, limit, msg_ind);
return ResetMotorDriverErrorHandling{.message_index = msg_ind};
}
template <bit_utils::ByteIterator Output, typename Limit>
auto serialize(Output body, Limit limit) const -> uint8_t {
auto iter = bit_utils::int_to_bytes(message_index, body, limit);
return iter - body;
}
auto operator==(const ResetMotorDriverErrorHandling&) const -> bool =
default;
};*/
// needs a member 'id'. This seems to just be becoming a CAN msg!

struct RouteMotorDriverInterrupt {
uint32_t message_index;
uint8_t debounce_count;
};

using MotionControlTaskMessage = std::variant<
Expand Down
118 changes: 12 additions & 106 deletions include/motor-control/core/tasks/motion_controller_task.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,9 +70,7 @@ class MotionControllerMessageHandler {
void handle(const can::messages::EnableMotorRequest& m) {
LOG("Received enable motor request");
if (controller.read_tmc_diag0()) {
can_client.send_can_message(can::ids::NodeId::host,
can::messages::MotorDriverInErrorState{
.message_index = m.message_index});
can_client.send_can_message(can::ids::NodeId::host, can::messages::ErrorMessage{.message_index = m.message_index, .severity = can::ids::ErrorSeverity::unrecoverable, .error_code = can::ids::ErrorCode::motor_driver_error_detected});
} else {
controller.enable_motor();
can_client.send_can_message(can::ids::NodeId::host,
Expand Down Expand Up @@ -116,9 +114,7 @@ class MotionControllerMessageHandler {
m.velocity, m.acceleration, m.group_id, m.seq_id, m.duration,
m.request_stop_condition);
if (controller.read_tmc_diag0()) {
can_client.send_can_message(can::ids::NodeId::host,
can::messages::MotorDriverInErrorState{
.message_index = m.message_index});
can_client.send_can_message(can::ids::NodeId::host, can::messages::ErrorMessage{.message_index = m.message_index, .severity = can::ids::ErrorSeverity::unrecoverable, .error_code = can::ids::ErrorCode::motor_driver_error_detected});
} else {
controller.move(m);
}
Expand All @@ -129,9 +125,7 @@ class MotionControllerMessageHandler {
"groupid=%d, seqid=%d\n",
m.velocity, m.group_id, m.seq_id);
if (controller.read_tmc_diag0()) {
can_client.send_can_message(can::ids::NodeId::host,
can::messages::MotorDriverInErrorState{
.message_index = m.message_index});
can_client.send_can_message(can::ids::NodeId::host, can::messages::ErrorMessage{.message_index = m.message_index, .severity = can::ids::ErrorSeverity::unrecoverable, .error_code = can::ids::ErrorCode::motor_driver_error_detected});
} else {
controller.move(m);
}
Expand Down Expand Up @@ -171,64 +165,13 @@ class MotionControllerMessageHandler {
can_client.send_can_message(can::ids::NodeId::host, response);
}
}

void handle(const can::messages::GetMotorUsageRequest& m) {
controller.send_usage_data(m.message_index, usage_client);
}

// no debouncing needed in final product
// make messages non-CAN, should be trivial, everything takes std::monostate
/*
void handle(const motor_control_task_messages::RouteMotorDriverInterrupt& m)
{ vTaskDelay(pdMS_TO_TICKS(100)); debounce_count++; if (debounce_count > 9)
{ if (controller.read_tmc_diag0()) {
motion_client.send_motion_controller_queue(motor_control_task_messages::MotorDriverErrorEncountered{.message_index
= m.message_index}); } else {
motion_client.send_motion_controller_queue(motor_control_task_messages::ResetMotorDriverErrorHandling{.message_index
= m.message_index});
}
diag0_debounced = false;
debounce_count = 0;
} else {
motion_client.send_motion_controller_queue(motor_control_task_messages::RouteMotorDriverInterrupt{.message_index
= m.message_index});
}
}
void handle(const motor_control_task_messages::MotorDriverErrorEncountered&
m) { controller.stop(can::ids::ErrorSeverity::unrecoverable,
can::ids::ErrorCode::motor_driver_error_detected);
if (!controller.is_timer_interrupt_running()) {
can_client.send_can_message(
can::ids::NodeId::host,
can::messages::ErrorMessage{
.message_index = m.message_index,
.severity = can::ids::ErrorSeverity::unrecoverable,
.error_code =
can::ids::ErrorCode::motor_driver_error_detected});
driver_client.send_motor_driver_queue(
can::messages::ReadMotorDriverErrorStatus{.message_index =
m.message_index});
}
}
void handle(const
motor_control_task_messages::ResetMotorDriverErrorHandling& m) {
static_cast<void>(m);
controller.clear_cancel_request();
can_client.send_can_message(
can::ids::NodeId::host,
can::messages::ErrorMessage{
.message_index = m.message_index,
.severity = can::ids::ErrorSeverity::none,
.error_code =
can::ids::ErrorCode::motor_driver_error_detected}); //
delete
}
*/

void handle(
const motor_control_task_messages::RouteMotorDriverInterrupt& m) {
if (debounce_count > 9) {
void handle(const motor_control_task_messages::RouteMotorDriverInterrupt& m) {
if (m.debounce_count > 9) {
if (controller.read_tmc_diag0()) {
controller.stop(
can::ids::ErrorSeverity::unrecoverable,
Expand All @@ -249,23 +192,22 @@ class MotionControllerMessageHandler {
controller.clear_cancel_request();
}
diag0_debounced = false;
debounce_count = 0;
} else {
debounce_count++;
vTaskDelay(pdMS_TO_TICKS(100));
motion_client.send_motion_controller_queue(
motor_control_task_messages::RouteMotorDriverInterrupt{
.message_index = m.message_index});
motion_client.send_motion_controller_queue(increment_message_debounce_count(m));
}
}

auto increment_message_debounce_count(const motor_control_task_messages::RouteMotorDriverInterrupt& m) -> motor_control_task_messages::RouteMotorDriverInterrupt {
return motor_control_task_messages::RouteMotorDriverInterrupt{.message_index = m.message_index, .debounce_count = static_cast<uint8_t>(m.debounce_count + 1)};
}

MotorControllerType& controller;
CanClient& can_client;
UsageClient& usage_client;
DriverClient& driver_client;
MotionClient& motion_client;
std::atomic<bool>& diag0_debounced;
std::atomic<uint8_t> debounce_count = 0;
};

/**
Expand Down Expand Up @@ -313,54 +255,18 @@ class MotionControllerTask {

[[nodiscard]] auto get_queue() const -> QueueType& { return queue; }

/*
void run_diag0_interrupt() {
if (!diag0_debounced) {
static_cast<void>(queue.try_write_isr(motor_control_task_messages::RouteMotorDriverInterrupt{.message_index
= 0})); diag0_debounced = true;
}
}
void run_diag0_interrupt() {
if (controller->read_tmc_diag0()) {
controller->stop(can::ids::ErrorSeverity::unrecoverable,
can::ids::ErrorCode::motor_driver_error_detected);
if (!controller->is_timer_interrupt_running()) {
can_client->send_can_message(
can::ids::NodeId::host,
can::messages::ErrorMessage{
.message_index = 0,
.severity = can::ids::ErrorSeverity::unrecoverable,
.error_code =
can::ids::ErrorCode::motor_driver_error_detected});
driver_client->send_motor_driver_queue(
can::messages::ReadMotorDriverErrorStatus{.message_index =
0});
}
} else {
controller->clear_cancel_request();
}
}
*/

void run_diag0_interrupt() {
if (!diag0_debounced) {
static_cast<void>(queue.try_write_isr(
motor_control_task_messages::RouteMotorDriverInterrupt{
.message_index = 0}));
.message_index = 0, .debounce_count = 0}));
diag0_debounced = true;
}
}

private:
QueueType& queue;
std::atomic<bool> diag0_debounced = false;
// static motion_controller::MotionController<lms::MotorMechanicalConfig>*
// controller = nullptr; static can::message_writer_task::TaskClient*
// can_client = nullptr; static tmc::tasks::TaskClient* driver_client =
// nullptr; static member object for motion controller (a nullable pointer)
// Can that much calling of motion controller be done in interrupt??
// do what impacts the least
};

} // namespace motion_controller_task
8 changes: 2 additions & 6 deletions include/pipettes/core/tasks/motion_controller_task.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,9 +62,7 @@ class MotionControllerMessageHandler {
// TODO only toggle the enable pin once since all motors share
// a single enable pin line.
if (controller.read_tmc_diag0()) {
can_client.send_can_message(can::ids::NodeId::host,
can::messages::MotorDriverInErrorState{
.message_index = m.message_index});
can_client.send_can_message(can::ids::NodeId::host, can::messages::ErrorMessage{.message_index = m.message_index, .severity = can::ids::ErrorSeverity::unrecoverable, .error_code = can::ids::ErrorCode::motor_driver_error_detected});
} else {
controller.enable_motor();
can_client.send_can_message(can::ids::NodeId::host,
Expand Down Expand Up @@ -109,9 +107,7 @@ class MotionControllerMessageHandler {
"acceleration=%d, groupid=%d, seqid=%d\n",
m.velocity, m.acceleration, m.group_id, m.seq_id);
if (controller.read_tmc_diag0()) {
can_client.send_can_message(can::ids::NodeId::host,
can::messages::MotorDriverInErrorState{
.message_index = m.message_index});
can_client.send_can_message(can::ids::NodeId::host, can::messages::ErrorMessage{.message_index = m.message_index, .severity = can::ids::ErrorSeverity::unrecoverable, .error_code = can::ids::ErrorCode::motor_driver_error_detected});
} else {
controller.move(m);
}
Expand Down
4 changes: 2 additions & 2 deletions pipettes/firmware/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,8 +147,8 @@ extern "C" void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
sensors::tip_presence::TipStatusChangeDetected{}));
}
} else if (GPIO_Pin == linear_motor_hardware.get_pins().diag0.pin) {
if (call_diag0_handler != NULL) {
if (*call_diag0_handler != NULL) {
if (call_diag0_handler != nullptr) {
if (*call_diag0_handler != nullptr) {
(*call_diag0_handler)();
}
}
Expand Down

0 comments on commit 62e5825

Please sign in to comment.