Skip to content

Commit

Permalink
send report message on start sensor move
Browse files Browse the repository at this point in the history
  • Loading branch information
@caila-marashaj
caila-marashaj committed Jan 29, 2024
1 parent d8cc38d commit ac09f4e
Show file tree
Hide file tree
Showing 4 changed files with 143 additions and 48 deletions.
20 changes: 20 additions & 0 deletions include/motor-control/core/motor_messages.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,6 +79,26 @@ struct Move { // NOLINT(cppcoreguidelines-pro-type-member-init)
}
};

struct SensorSyncMove // NOLINT(cppcoreguidelines-pro-type-member-init)
: public Move {
can::ids::SensorId sensor_id;

auto build_ack(int32_t pulses, uint8_t flags, AckMessageId _id) -> Ack {
// not sure if we need to include sensor id in the ack
return Ack{
.message_index = message_index,
.group_id = group_id,
.seq_id = seq_id,
.current_position_steps = 0,
.encoder_position = pulses,
.position_flags = flags,
.ack_id = _id,
.start_encoder_position = start_encoder_position,
.usage_key = usage_key,
};
}
};

struct GearMotorMove // NOLINT(cppcoreguidelines-pro-type-member-init)
: public Move {
uint32_t start_step_position;
Expand Down
20 changes: 20 additions & 0 deletions include/motor-control/core/stepper_motor/motion_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,26 @@ class MotionController {
return linear_motion_sys_config;
}

void move(const can::messages::AddSensorMoveRequest& can_msg) {
steps_per_tick velocity_steps =
fixed_point_multiply(steps_per_mm, can_msg.velocity);
steps_per_tick_sq acceleration_steps =
fixed_point_multiply(steps_per_um, can_msg.acceleration);
SensorSyncMove msg{
.message_index = can_msg.message_index,
.duration = can_msg.duration,
.velocity = velocity_steps,
.acceleration = acceleration_steps,
.group_id = can_msg.group_id,
.seq_id = can_msg.seq_id,
.stop_condition = can_msg.request_stop_condition,
.usage_key = hardware.get_usage_eeprom_config().get_distance_key()};
if (!enabled) {
enable_motor();
}
queue.try_write(msg);
}

void move(const can::messages::AddLinearMoveRequest& can_msg) {
steps_per_tick velocity_steps =
fixed_point_multiply(steps_per_mm, can_msg.velocity);
Expand Down
43 changes: 22 additions & 21 deletions include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -594,26 +594,27 @@ class MotorInterruptHandler {
return has_active_move() && buffered_move.velocity != 0;
}

private:
void update_hardware_step_tracker() {
hardware.set_step_tracker(
static_cast<uint32_t>(position_tracker >> 31));
}

uint64_t tick_count = 0x0;
static constexpr const q31_31 tick_flag = 0x80000000;
static constexpr const uint64_t overflow_flag = 0x8000000000000000;
// Tracks position with sub-microstep accuracy
q31_31 position_tracker{0};
MoveQueue& move_queue;
StatusClient& status_queue_client;
MotorHardware& hardware;
stall_check::StallCheck& stall_checker;
UpdatePositionQueue& update_position_queue;
MotorMoveMessage buffered_move = MotorMoveMessage{};
bool clear_queue_until_empty = false;
bool stall_handled = false;
bool in_estop = false;
std::atomic_bool _has_active_move = false;
// private:
protected:
void update_hardware_step_tracker() {
hardware.set_step_tracker(
static_cast<uint32_t>(position_tracker >> 31));
}

uint64_t tick_count = 0x0;
static constexpr const q31_31 tick_flag = 0x80000000;
static constexpr const uint64_t overflow_flag = 0x8000000000000000;
// Tracks position with sub-microstep accuracy
q31_31 position_tracker{0};
MoveQueue& move_queue;
StatusClient& status_queue_client;
MotorHardware& hardware;
stall_check::StallCheck& stall_checker;
UpdatePositionQueue& update_position_queue;
MotorMoveMessage buffered_move = MotorMoveMessage{};
bool clear_queue_until_empty = false;
bool stall_handled = false;
bool in_estop = false;
std::atomic_bool _has_active_move = false;
};
} // namespace motor_handler
108 changes: 81 additions & 27 deletions include/pipettes/core/motor_interrupt_handler.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,19 +5,20 @@

namespace pipettes {
using namespace motor_messages;
//using InterruptHandler = motor_handler::MotorInterruptHandler<
// freertos_message_queue::FreeRTOSMessageQueue, StatusClient,
// MotorMoveMessage, MotorHardware>;
// might need to change MotorMoveMessage to include sensormove
template <template <class> class QueueImpl, class StatusClient,
typename MotorMoveMessage, typename MotorHardware, class SensorClient>
requires MessageQueue<QueueImpl<MotorMoveMessage>, MotorMoveMessage> &&
std::is_base_of_v<motor_hardware::MotorHardwareIface, MotorHardware>
class PipetteMotorInterruptHandler
: public motor_handler::MotorInterruptHandler<
freertos_message_queue::FreeRTOSMessageQueue, StatusClient,
MotorMoveMessage, MotorHardware> {
: public motor_handler::MotorInterruptHandler<
freertos_message_queue::FreeRTOSMessageQueue, StatusClient,
MotorMoveMessage, MotorHardware> {
public:
using MoveQueue = QueueImpl<MotorMoveMessage>;
using InterruptHandler = motor_handler::MotorInterruptHandler<
freertos_message_queue::FreeRTOSMessageQueue, StatusClient,
MotorMoveMessage, MotorHardware>;
using UpdatePositionQueue =
QueueImpl<can::messages::UpdateMotorPositionEstimationRequest>;
PipetteMotorInterruptHandler() = delete;
Expand All @@ -26,7 +27,9 @@ class PipetteMotorInterruptHandler
MotorHardware& hardware_iface, stall_check::StallCheck& stall,
UpdatePositionQueue& incoming_update_position_queue,
SensorClient& sensor_queue_client) :
InterruptHandler(incoming_move_queue, outgoing_queue, hardware_iface,
motor_handler::MotorInterruptHandler<
freertos_message_queue::FreeRTOSMessageQueue, StatusClient,
MotorMoveMessage, MotorHardware>(incoming_move_queue, outgoing_queue, hardware_iface,
stall, incoming_update_position_queue),
sensor_client(sensor_queue_client) {
// sensor_client = sensor_tasks::QueueClient{};
Expand All @@ -49,44 +52,95 @@ class PipetteMotorInterruptHandler

// need to write a new kind of move that contains the stuff thats gonna be sent in the new can message

void run_interrupt() {
// handle various error states
if (this->clear_queue_until_empty) {
// If we were executing a move when estop asserted, and
// what's in the queue is the remaining enqueued moves from
// that group, then we will have called
// cancel_and_clear_moves and clear_queue_until_empty will
// be true. That means we should pop out the queue.
// clear_queue_until_empty will also be true if we were in
// the steady-state estop asserted; got new messages; and
// the other branch of this if asserted. In either case, an
// error message has been sent, so we need to just keep
// clearing the queue.
this->clear_queue_until_empty = this->pop_and_discard_move();
} else if (this->in_estop) {
this->handle_update_position_queue();
this->in_estop = this->estop_update();
} else if (this->estop_triggered()) {
this->cancel_and_clear_moves(can::ids::ErrorCode::estop_detected);
this->in_estop = true;
} else if (this->hardware.has_cancel_request()) {
this->cancel_and_clear_moves(can::ids::ErrorCode::stop_requested,
can::ids::ErrorSeverity::warning);
} else {
// Normal Move logic
this->run_normal_interrupt();
}
}

void handle_move_type(motor_messages::Move m) {
}

void handle_move_type(motor_messages::SensorSyncMove m) {
auto msg = can::messages::BindSensorOutputRequest{
.message_index = m.message_index,
.sensor = can::ids::SensorType::pressure,
.sensor_id = m.sensor_id,
.binding = can::ids::SensorOutputBinding::report
};
}

void update_move() {
// do stuff in here
_has_active_move = move_queue.try_read_isr(&buffered_move);
if (_has_active_move) {
hardware.enable_encoder();
buffered_move.start_encoder_position =
hardware.get_encoder_pulses();
this->_has_active_move = this->move_queue.try_read_isr(&this->buffered_move);
if (this->_has_active_move) {
handle_move_type(this->buffered_move);
this->hardware.enable_encoder();
this->buffered_move.start_encoder_position =
this->hardware.get_encoder_pulses();
}
if (set_direction_pin()) {
hardware.positive_direction();
if (this->set_direction_pin()) {
this->hardware.positive_direction();
} else {
hardware.negative_direction();
this->hardware.negative_direction();
}
if (_has_active_move && buffered_move.check_stop_condition(
if (this->_has_active_move && this->buffered_move.check_stop_condition(
MoveStopCondition::limit_switch)) {
position_tracker = 0x7FFFFFFFFFFFFFFF;
update_hardware_step_tracker();
hardware.position_flags.clear_flag(
this->position_tracker = 0x7FFFFFFFFFFFFFFF;
this->update_hardware_step_tracker();
this->hardware.position_flags.clear_flag(
can::ids::MotorPositionFlags::stepper_position_ok);
hardware.position_flags.clear_flag(
this->hardware.position_flags.clear_flag(
can::ids::MotorPositionFlags::encoder_position_ok);
}
}

void finish_current_move(
AckMessageId ack_msg_id = AckMessageId::complete_without_condition) {
_has_active_move = false;
tick_count = 0x0;
stall_handled = false;
build_and_send_ack(ack_msg_id);
set_buffered_move(MotorMoveMessage{});
this->_has_active_move = false;
this->tick_count = 0x0;
this->stall_handled = false;
this->build_and_send_ack(ack_msg_id); // might want to overwrite this func
(MotorMoveMessage{});
// update the stall check ideal encoder counts based on
// last known location

// check if queue is empty in here and send bindsensoroutputrequest

if (!has_move_messages()) {
stall_checker.reset_itr_counts(hardware.get_step_tracker());
if (!this->has_move_messages()) {
this->stall_checker.reset_itr_counts(this->hardware.get_step_tracker());
}
}

void send_to_pressure_sensor_queue(can::messages::SensorOutputBinding& m) {
if(this->buffered_move.sensor_id == can::ids::SensorId::S1) {
sensor_client.send_pressure_sensor_queue_front(m);
}
else {
sensor_client.send_pressure_sensor_queue_rear(m);
}
}

Expand Down

0 comments on commit ac09f4e

Please sign in to comment.