Skip to content

Commit

Permalink
Added clear to motion queues
Browse files Browse the repository at this point in the history
  • Loading branch information
lromor committed Apr 29, 2023
1 parent 355a1a0 commit e602c77
Show file tree
Hide file tree
Showing 13 changed files with 73 additions and 21 deletions.
1 change: 1 addition & 0 deletions src/determine-print-stats.cc
Original file line number Diff line number Diff line change
Expand Up @@ -117,6 +117,7 @@ class StatsSegmentQueue : public SegmentQueue {
void WaitQueueEmpty() final {}
bool GetPhysicalStatus(PhysicalStatus *status) final { return false; }
void SetExternalPosition(int axis, int pos) final {}
bool Clear() final { return false; }

private:
BeagleGPrintStats *const print_stats_;
Expand Down
1 change: 1 addition & 0 deletions src/gcode-machine-control_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,7 @@ class MockMotorOps final : public SegmentQueue {
void MotorEnable(bool on) final {}
bool GetPhysicalStatus(PhysicalStatus *status) final { return false; }
void SetExternalPosition(int axis, int steps) final {}
bool Clear() final { return false; }

int call_count_wait_queue_empty = 0;

Expand Down
1 change: 1 addition & 0 deletions src/gcode2ps.cc
Original file line number Diff line number Diff line change
Expand Up @@ -1037,6 +1037,7 @@ class SegmentQueuePrinter final : public SegmentQueue {
void MotorEnable(bool on) final {}
void WaitQueueEmpty() final {}
bool GetPhysicalStatus(PhysicalStatus *status) final { return false; }
bool Clear() final { return false; }
void SetExternalPosition(int motor, int pos) final {
current_pos_[motor] = pos;
if (pass_ == ProcessingStep::GenerateOutput) {
Expand Down
7 changes: 7 additions & 0 deletions src/motion-queue-motor-operations.cc
Original file line number Diff line number Diff line change
Expand Up @@ -337,6 +337,13 @@ bool MotionQueueMotorOperations::Enqueue(const LinearSegmentSteps &segment) {
return ret;
}

bool MotionQueueMotorOperations::Clear() {
const bool ret = backend_->Clear();
shadow_queue_->clear();
shadow_queue_->push_front({});
return ret;
}

void MotionQueueMotorOperations::MotorEnable(bool on) {
backend_->WaitQueueEmpty();
backend_->MotorEnable(on);
Expand Down
1 change: 1 addition & 0 deletions src/motion-queue-motor-operations.h
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@ class MotionQueueMotorOperations : public SegmentQueue {
void WaitQueueEmpty() final;
bool GetPhysicalStatus(PhysicalStatus *status) final;
void SetExternalPosition(int axis, int position_steps) final;
bool Clear() final;

private:
bool EnqueueInternal(const LinearSegmentSteps &param,
Expand Down
39 changes: 38 additions & 1 deletion src/motion-queue-motor-operations_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,15 @@ class MockMotionQueue final : public MotionQueue {
if (head_item_progress) *head_item_progress = remaining_loops_;
return queue_size_;
}
bool EmergencyReset() final { return true; }

bool Clear() final {
clear_calls_count++;
remaining_loops_ = 0;
queue_size_ = 0;
return true;
}

int clear_calls_count = 0;

void SimRun(const uint32_t executed_loops, const unsigned int buffer_size) {
assert(buffer_size <= queue_size_);
Expand Down Expand Up @@ -203,6 +211,35 @@ TEST(RealtimePosition, zero_loops_edge) {
EXPECT_THAT(expected, ::testing::ContainerEq(status.pos_steps));
}

// Clear motion queue motor operations.
// The physical status should be reset and motion_backend.Clear() called.
TEST(RealtimePosition, clear_queue) {
HardwareMapping hw;
MockMotionQueue motion_backend = MockMotionQueue();
MotionQueueMotorOperations motor_operations(&hw, &motion_backend);

// Enqueue a segment
LinearSegmentSteps segment = {
0 /* v0 */,
0 /* v1 */,
0 /* aux */,
{10, 20, 30, 40, 50, 60, 70, 80} /* steps */
};
int expected[BEAGLEG_NUM_MOTORS] = {10, 20, 30, 40, 50, 60, 70, 80};

motor_operations.Enqueue(segment);
motion_backend.SimRun(0, 1);

PhysicalStatus status;
motor_operations.GetPhysicalStatus(&status);
EXPECT_THAT(expected, ::testing::ContainerEq(status.pos_steps));
EXPECT_TRUE(motor_operations.Clear());
EXPECT_EQ(motion_backend.clear_calls_count, 1);
motor_operations.GetPhysicalStatus(&status);
memset(expected, 0, sizeof(expected));
EXPECT_THAT(expected, ::testing::ContainerEq(status.pos_steps));
}

int main(int argc, char *argv[]) {
Log_init("/dev/stderr");
::testing::InitGoogleTest(&argc, argv);
Expand Down
10 changes: 5 additions & 5 deletions src/motion-queue.h
Original file line number Diff line number Diff line change
Expand Up @@ -119,9 +119,9 @@ class MotionQueue {
// of not yet executed loops in the item currenly being executed.
virtual int GetPendingElements(uint32_t *head_item_progress) = 0;

// Perform an immediate shutdown, even if motors are still moving, and
// reset the queue to its initial state.
virtual bool EmergencyReset() = 0;
// Perform an immediate reset of the queue,
// even if motors are still moving.
virtual bool Clear() = 0;
};

// Standard implementation.
Expand All @@ -141,7 +141,7 @@ class PRUMotionQueue final : public MotionQueue {
void MotorEnable(bool on) final;
void Shutdown(bool flush_queue) final;
int GetPendingElements(uint32_t *head_item_progress) final;
bool EmergencyReset() final;
bool Clear() final;

private:
bool Init();
Expand All @@ -166,7 +166,7 @@ class DummyMotionQueue final : public MotionQueue {
if (head_item_progress) *head_item_progress = 0;
return 1;
}
bool EmergencyReset() final { return true; }
bool Clear() final { return true; }
};

#endif // _BEAGLEG_MOTION_QUEUE_H_
1 change: 1 addition & 0 deletions src/planner_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -124,6 +124,7 @@ class FakeMotorOperations : public SegmentQueue {
void WaitQueueEmpty() final {}
bool GetPhysicalStatus(PhysicalStatus *status) final { return false; }
void SetExternalPosition(int axis, int steps) final {}
bool Clear() final { return false; }

int SegmentsCount() const { return collected_.size(); }
const std::vector<LinearSegmentSteps> &segments() { return collected_; }
Expand Down
4 changes: 2 additions & 2 deletions src/pru-motion-queue.cc
Original file line number Diff line number Diff line change
Expand Up @@ -177,8 +177,8 @@ void PRUMotionQueue::Shutdown(bool flush_queue) {
MotorEnable(false);
}

bool PRUMotionQueue::EmergencyReset() {
Shutdown(false);
bool PRUMotionQueue::Clear() {
pru_interface_->Shutdown();
return Init();
}

Expand Down
21 changes: 8 additions & 13 deletions src/pru-motion-queue_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -30,12 +30,8 @@ class MockPRUInterface : public PruHardwareInterface {
public:
MockPRUInterface() : execution_index_(QUEUE_LEN - 1) {
mmap = NULL;
ON_CALL(*this, Init).WillByDefault([]() {
return true;
});
ON_CALL(*this, Shutdown).WillByDefault([]() {
return true;
});
ON_CALL(*this, Init).WillByDefault([]() { return true; });
ON_CALL(*this, Shutdown).WillByDefault([]() { return true; });
}
~MockPRUInterface() override { free(mmap); }

Expand All @@ -45,6 +41,8 @@ class MockPRUInterface : public PruHardwareInterface {
MOCK_METHOD(bool, Shutdown, (), ());

bool AllocateSharedMem(void **pru_mmap, const size_t size) final {
if (mmap != NULL)
return true;
mmap = (struct MockPRUCommunication *)malloc(size);
*pru_mmap = (void *)mmap;
memset(*pru_mmap, 0x00, size);
Expand Down Expand Up @@ -141,9 +139,8 @@ TEST(PruMotionQueue, one_round_queue) {
EXPECT_EQ(motion_backend.GetPendingElements(NULL), QUEUE_LEN);
}

// Check emergency reset shutsdowns the motors and
// the PRU and sets to zero the whole queue.
TEST(PruMotionQueue, emergency_stop) {
// Check the PRU is reset and no elements are pending.
TEST(PruMotionQueue, clear_queue) {
MotorsRegister absolute_pos_loops;
NiceMock<MockPRUInterface> pru_interface;
HardwareMapping hmap = HardwareMapping();
Expand All @@ -164,11 +161,9 @@ TEST(PruMotionQueue, emergency_stop) {
EXPECT_CALL(pru_interface, Shutdown())
.Times(1)
.WillRepeatedly(testing::Return(true));
EXPECT_CALL(pru_interface, Init())
.Times(1)
.WillOnce(testing::Return(true));
EXPECT_CALL(pru_interface, Init()).Times(1).WillOnce(testing::Return(true));
}
EXPECT_TRUE(motion_backend.EmergencyReset());
EXPECT_TRUE(motion_backend.Clear());
EXPECT_EQ(motion_backend.GetPendingElements(NULL), 0);
}

Expand Down
6 changes: 6 additions & 0 deletions src/segment-queue.h
Original file line number Diff line number Diff line change
Expand Up @@ -77,6 +77,12 @@ class SegmentQueue {
// source (e.g. homing). This will allow accurate reporting of the
// PhysicalStatus.
virtual void SetExternalPosition(int axis, int position_steps) = 0;

// Clear the queue and reset it to its initial state.
// This action is immediate and will discard any running or
// enqueued and not yet executed segment.
// Current physical status will be lost.
virtual bool Clear() = 0;
};

#endif // _BEAGLEG_MOTOR_OPERATIONS_H_
1 change: 1 addition & 0 deletions src/sim-audio-out.h
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@ class SimFirmwareAudioQueue : public MotionQueue {
void WaitQueueEmpty() final {}
void MotorEnable(bool on) final {}
void Shutdown(bool flush_queue) final {}
bool Clear() final { return false; }
int GetPendingElements(uint32_t *head_item_progress) final {
if (head_item_progress) *head_item_progress = 0;
return 1;
Expand Down
1 change: 1 addition & 0 deletions src/sim-firmware.h
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@ class SimFirmwareQueue : public MotionQueue {
void WaitQueueEmpty() final {}
void MotorEnable(bool on) final {}
void Shutdown(bool flush_queue) final {}
bool Clear() final { return false; }
int GetPendingElements(uint32_t *head_item_progress) final {
if (head_item_progress) *head_item_progress = 0;
return 1;
Expand Down

0 comments on commit e602c77

Please sign in to comment.