Skip to content
Permalink
Browse files

Add support to the PRU code to check the EStop input before each step

pulse. If it's detected, update the queue status and change the queue
header state to STATE_ABORT then abort the step generation for the
segment. If there are more segments in the queue they will also get
aborted due to the EStop input being active.

Modify the PRUMotionQueue::Enqueue() to detect the STATE_ABORT and
return false. This indicates that the motion segment could not be
added to the queue due to the abort condition caused by the EStop.

Modify all the applicable classes so that the Enqueue failure is
propogated back to the Planner so that CGodeMachineControl can
detect the abort condition and then check_for_estop(). This will
then turn all the aux outputs off, disable the motors, and set the
homing_state to NEVER_HOMED.

Signed-off-by: H Hartley Sweeten <hsweeten@visionengravers.com>
  • Loading branch information...
bigguiness authored and hzeller committed Oct 16, 2018
1 parent 4e3f93c commit 9e947d2bb40480101f655c1b959f74ef375b6210
@@ -3,3 +3,7 @@

;; For now, we just use the generic ones.
#include "../../src/pru-generic-io-routines.hp"

// Dummy function
CheckForEStop:
RET
@@ -3,3 +3,7 @@

;; Just use the generic ones.
#include "../../src/pru-generic-io-routines.hp"

// Dummy function
CheckForEStop:
RET
@@ -2,3 +2,7 @@

;; For now, we just use the generic ones.
#include "../../src/pru-generic-io-routines.hp"

// Dummy function
CheckForEStop:
RET
@@ -3,3 +3,30 @@

;; Just use the generic ones.
#include "../../src/pru-generic-io-routines.hp"

// HARDCODED function to check the E-Stop input on the VGEN5 board
// IN_7_GPIO (GPIO_0_BASE | 27) is the dedicated E-Stop input
//
// NOTE: two reads are done for a minimal debouce
//
// Available registers:
// r0 = used to return the E-Stop status (0=ok, 1=E-Stop active)
// r4 = GPIO_DATAIN register for the EStop input
// r5 = EStop input bit
// r6 = GPIO_DATAIN register value
CheckForEStop:
MOV r4, (IN_7_GPIO & 0xfffff000)
QBEQ estop_ok, r4.w2, GPIO_NOT_MAPPED
MOV r5, GPIO_DATAIN
ADD r4, r4, r5
MOV r5, (IN_7_GPIO & 0x1f)
debounce:
LBBO r6, r4, 0, 4
QBBC estop_ok, r6, r5
QBEQ estop_active, r0, 1
MOV r0, 1
JMP debounce
estop_ok:
MOV r0, 0
estop_active:
RET
@@ -100,7 +100,7 @@ class StatsMotorOperations : public MotorOperations {
public:
StatsMotorOperations(BeagleGPrintStats *stats) : print_stats_(stats) {}

void Enqueue(const LinearSegmentSteps &param) final {
bool Enqueue(const LinearSegmentSteps &param) final {
int max_steps = 0;
for (int i = 0; i < BEAGLEG_NUM_MOTORS; ++i) {
int steps = abs(param.steps[i]);
@@ -111,6 +111,7 @@ class StatsMotorOperations : public MotorOperations {
// max_steps = a/2*t^2 + v0*t; a = (v1-v0)/t
print_stats_->total_time_seconds += 2 * max_steps / (param.v0 + param.v1);
//printf("HZ:v0=%7.1f v1=%7.1f steps=%d\n", param.v0, param.v1, max_steps);
return true;
}

void MotorEnable(bool on) final {}
@@ -802,7 +802,9 @@ bool GCodeMachineControl::Impl::coordinated_move(float feed,
}

float feedrate = prog_speed_factor_ * current_feedrate_mm_per_sec_;
planner_->Enqueue(axis, feedrate);
if (!planner_->Enqueue(axis, feedrate)) {
if (check_for_estop()) return false;
}
return true;
}

@@ -817,7 +819,9 @@ bool GCodeMachineControl::Impl::rapid_move(float feed,
if (given > 0 && current_feedrate_mm_per_sec_ <= 0) {
current_feedrate_mm_per_sec_ = given; // At least something for G1.
}
planner_->Enqueue(axis, given > 0 ? given : rapid_feed);
if (!planner_->Enqueue(axis, given > 0 ? given : rapid_feed)) {
if (check_for_estop()) return false;
}
return true;
}

@@ -708,15 +708,15 @@ class MotorOperationsPrinter : public MotorOperations {
if (v < min_v_) min_v_ = v;
}

void Enqueue(const LinearSegmentSteps &param) final {
bool Enqueue(const LinearSegmentSteps &param) final {
GCodeParserAxis dominant_axis = AXIS_X;
for (int i = 1; i < BEAGLEG_NUM_MOTORS; ++i) {
if (abs(param.steps[i]) > abs(param.steps[dominant_axis]))
dominant_axis = (GCodeParserAxis) i; // here, we have motor=axis
}
if (config_.steps_per_mm[dominant_axis] == 0 ||
param.steps[dominant_axis] == 0) {
return; // Nothing really to do.
return true; // Nothing really to do.
}

switch (pass_) {
@@ -748,6 +748,7 @@ class MotorOperationsPrinter : public MotorOperations {
PrintSegment(param, dominant_axis);
break;
}
return true;
}

// Set the length of how long the smallest colored range should be.
@@ -97,7 +97,8 @@ class MotionQueue {
// Enqueue a motion segment into queue. Blocks until capacity of queue allows
// to receive more elements.
// Might change values in MotionSegment.
virtual void Enqueue(MotionSegment *segment) = 0;
// Returns true if segment was added, false if PRU abort was detected
virtual bool Enqueue(MotionSegment *segment) = 0;

// Block and wait for queue to be empty.
virtual void WaitQueueEmpty() = 0;
@@ -129,7 +130,7 @@ class PRUMotionQueue : public MotionQueue {
PRUMotionQueue(HardwareMapping *hw, PruHardwareInterface *pru);
~PRUMotionQueue();

void Enqueue(MotionSegment *segment);
bool Enqueue(MotionSegment *segment);
void WaitQueueEmpty();
void MotorEnable(bool on);
void Shutdown(bool flush_queue);
@@ -138,6 +139,8 @@ class PRUMotionQueue : public MotionQueue {
private:
bool Init();

void ClearPRUAbort(unsigned int idx);

HardwareMapping *const hardware_mapping_;
PruHardwareInterface *const pru_interface_;

@@ -149,7 +152,7 @@ class PRUMotionQueue : public MotionQueue {
// Queue that does nothing. For testing purposes.
class DummyMotionQueue : public MotionQueue {
public:
void Enqueue(MotionSegment *segment) {}
bool Enqueue(MotionSegment *segment) { return true; }
void WaitQueueEmpty() {}
void MotorEnable(bool on) {}
void Shutdown(bool flush_queue) {}
@@ -30,6 +30,7 @@
#define STATE_EMPTY 0 // Queue element empty, ready to be filled by host
#define STATE_FILLED 1 // Queue element filled by host, to be picked up by PRU
#define STATE_EXIT 2 // Filled by host, no parameters; tells PRU to exit.
#define STATE_ABORT 3 // Filled by PRU when Estop is detected

#define QUEUE_LEN 16

@@ -198,6 +198,7 @@ QUEUE_READ:
.assign QueueHeader, r1.w0, r1.w0, queue_header
LBCO queue_header, CONST_PRUDRAM, r2, SIZE(queue_header)
QBEQ QUEUE_READ, queue_header.state, STATE_EMPTY ; wait until got data.
QBEQ QUEUE_READ, queue_header.state, STATE_ABORT

QBEQ FINISH, queue_header.state, STATE_EXIT

@@ -240,6 +241,17 @@ QUEUE_READ:
;; status-variable: r28
;; call/ret: r30
STEP_GEN:
MOV r0, 0
CALL CheckForEStop
QBNE DO_STEP_GEN, r0, 1
// Estop detected, update the queue status and abort
ZERO &r28, 3 // Estop detected, zero the loop counter
ADD r28, r28, 1 // status_loops++ (removed by UpdateQueueStatus)
UpdateQueueStatus
MOV queue_header.state, STATE_ABORT
JMP STEP_GEN_ABORTED

DO_STEP_GEN:
;;
;; Generate motion profile configured by TravelParameters
;;
@@ -261,6 +273,7 @@ STEP_GEN:
CalculateDelay r1, travel_params, r3, r5, r6
QBEQ DONE_STEP_GEN, r1, 0 ; special value 0: all steps consumed.
UpdateQueueStatus

STEP_DELAY: ; Create time delay between steps.
SUB r1, r1, 1 ; two cycles per loop.
QBNE STEP_DELAY, r1, 0
@@ -270,6 +283,7 @@ STEP_DELAY: ; Create time delay between steps.
DONE_STEP_GEN:
;; We are done with instruction. Mark slot as empty...
MOV queue_header.state, STATE_EMPTY
STEP_GEN_ABORTED:
SBCO queue_header.state, CONST_PRUDRAM, r2, 1
MOV R31.b0, PRU0_ARM_INTERRUPT+16 ; signal host program free slot.

@@ -115,7 +115,7 @@ MotionQueueMotorOperations::~MotionQueueMotorOperations() {
delete shadow_queue_;
}

void MotionQueueMotorOperations::EnqueueInternal(const LinearSegmentSteps &param,
bool MotionQueueMotorOperations::EnqueueInternal(const LinearSegmentSteps &param,
int defining_axis_steps) {
struct MotionSegment new_element = {};
new_element.direction_bits = 0;
@@ -194,7 +194,7 @@ void MotionQueueMotorOperations::EnqueueInternal(const LinearSegmentSteps &param
new_element.aux = param.aux_bits;
new_element.state = STATE_FILLED;
backend_->MotorEnable(true);
backend_->Enqueue(&new_element);
return backend_->Enqueue(&new_element);
}

bool MotionQueueMotorOperations::GetPhysicalStatus(PhysicalStatus *status) {
@@ -252,8 +252,9 @@ static int get_defining_axis_steps(const LinearSegmentSteps &param) {
return defining_axis_steps;
}

void MotionQueueMotorOperations::Enqueue(const LinearSegmentSteps &param) {
bool MotionQueueMotorOperations::Enqueue(const LinearSegmentSteps &param) {
const int defining_axis_steps = get_defining_axis_steps(param);
bool ret;

if (defining_axis_steps == 0) {
// The new segment is based on the previous position.
@@ -267,7 +268,7 @@ void MotionQueueMotorOperations::Enqueue(const LinearSegmentSteps &param) {
history_segment.aux_bits = param.aux_bits;
shadow_queue_->push_front(history_segment);

backend_->Enqueue(&empty_element);
ret = backend_->Enqueue(&empty_element);
}
else if (defining_axis_steps > MAX_STEPS_PER_SEGMENT) {
// We have more steps that we can enqueue in one chunk, so let's cut
@@ -303,12 +304,13 @@ void MotionQueueMotorOperations::Enqueue(const LinearSegmentSteps &param) {
const double v1 = v1squared > 0.0 ? sqrt(v1squared) : 0;
output.v0 = previous_speed;
output.v1 = v1;
EnqueueInternal(output, division_steps);
ret = EnqueueInternal(output, division_steps);
if (!ret) break;
previous = accumulator;
previous_speed = v1;
}
} else {
EnqueueInternal(param, defining_axis_steps);
ret = EnqueueInternal(param, defining_axis_steps);
}
// Shrink the queue and remove the elements that we are not interested
// in anymore.
@@ -317,6 +319,7 @@ void MotionQueueMotorOperations::Enqueue(const LinearSegmentSteps &param) {
const int buffer_size = backend_->GetPendingElements(NULL);
const int new_size = buffer_size > 0 ? buffer_size : 1;
shadow_queue_->resize(new_size);
return ret;
}

void MotionQueueMotorOperations::MotorEnable(bool on) {
@@ -59,10 +59,11 @@ class MotorOperations { // Rename SegmentQueue ?

// Enqueue a coordinated move command.
// If there is space in the ringbuffer, this function returns immediately,
// otherwise it waits until a slot frees up.
// otherwise it waits until a slot frees up or an abort happens.
// If "err_stream" is non-NULL, prints error message there.
// Automatically enables motors if not already.
virtual void Enqueue(const LinearSegmentSteps &segment) = 0;
// Returns true if the move was added, false if aborted
virtual bool Enqueue(const LinearSegmentSteps &segment) = 0;

// Waits for the queue to be empty and Enables/disables motors according to the
// given boolean value (Right now, motors cannot be individually addressed).
@@ -86,14 +87,14 @@ class MotionQueueMotorOperations : public MotorOperations {
MotionQueueMotorOperations(HardwareMapping *hw, MotionQueue *backend);
~MotionQueueMotorOperations() override;

void Enqueue(const LinearSegmentSteps &segment) final;
bool Enqueue(const LinearSegmentSteps &segment) final;
void MotorEnable(bool on) final;
void WaitQueueEmpty() final;
bool GetPhysicalStatus(PhysicalStatus *status) final;
void SetExternalPosition(int axis, int pos) final;

private:
void EnqueueInternal(const LinearSegmentSteps &param,
bool EnqueueInternal(const LinearSegmentSteps &param,
int defining_axis_steps);

HardwareMapping *const hardware_mapping_;
Oops, something went wrong.

0 comments on commit 9e947d2

Please sign in to comment.
You can’t perform that action at this time.