From 9f5219c5c2309a8c0a32e28ee129bd2d5b6d7b30 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Sat, 10 Sep 2016 00:00:44 -0700 Subject: [PATCH] Smooth move transitions, enforce move generation with alarm, wait a bit for planner to fill before moving. --- src/messages.def | 1 + src/motor.c | 6 +++--- src/plan/buffer.c | 28 ++++++++++++++-------------- src/plan/buffer.h | 8 ++++---- src/plan/exec.c | 39 ++++++++++++++++++++++++--------------- src/rtc.h | 4 +--- src/stepper.c | 9 +++++++-- 7 files changed, 54 insertions(+), 41 deletions(-) diff --git a/src/messages.def b/src/messages.def index d48da3b..b731d73 100644 --- a/src/messages.def +++ b/src/messages.def @@ -74,6 +74,7 @@ STAT_MSG(MACHINE_ALARMED, "Machine is alarmed - Command not processed") STAT_MSG(LIMIT_SWITCH_HIT, "Limit switch hit - Shutdown occurred") STAT_MSG(SOFT_LIMIT_EXCEEDED, "Soft limit exceeded") STAT_MSG(INVALID_AXIS, "Invalid axis") +STAT_MSG(EXPECTED_MOVE, "A move was expected but none was queued") // Homing STAT_MSG(HOMING_CYCLE_FAILED, "Homing cycle failed") diff --git a/src/motor.c b/src/motor.c index e6335d9..9209998 100644 --- a/src/motor.c +++ b/src/motor.c @@ -164,14 +164,14 @@ void motor_init() { // Enable DMA DMA.CTRL = DMA_RESET_bm; DMA.CTRL = DMA_ENABLE_bm; - DMA.INTFLAGS = 0xff; // clear all interrups + DMA.INTFLAGS = 0xff; // clear all interrupts for (int motor = 0; motor < MOTORS; motor++) { motor_t *m = &motors[motor]; // IO pins - DIRSET_PIN(m->step_pin); // Output - DIRSET_PIN(m->dir_pin); // Output + DIRSET_PIN(m->step_pin); // Output + DIRSET_PIN(m->dir_pin); // Output OUTSET_PIN(m->enable_pin); // High (disabled) DIRSET_PIN(m->enable_pin); // Output diff --git a/src/plan/buffer.c b/src/plan/buffer.c index 1192fb5..6ff9d73 100644 --- a/src/plan/buffer.c +++ b/src/plan/buffer.c @@ -51,7 +51,7 @@ #include "buffer.h" #include "state.h" -#include "report.h" +#include "rtc.h" #include @@ -68,17 +68,6 @@ typedef struct { // ring buffer for sub-moves buffer_pool_t mb; // move buffer queue -uint8_t mp_get_planner_buffer_room() { - uint16_t n = mb.buffers_available; - return n < PLANNER_BUFFER_HEADROOM ? 0 : n - PLANNER_BUFFER_HEADROOM; -} - - -void mp_wait_for_buffer() { - while (!mb.buffers_available) continue; -} - - /// buffer incr & wrap #define _bump(a) ((a < PLANNER_BUFFER_POOL_SIZE - 1) ? a + 1 : 0) @@ -105,6 +94,18 @@ void mp_init_buffers() { } +uint8_t mp_get_planner_buffer_room() { + uint16_t n = mb.buffers_available; + return n < PLANNER_BUFFER_HEADROOM ? 0 : n - PLANNER_BUFFER_HEADROOM; +} + + +uint8_t mp_get_planner_buffer_fill() { + return PLANNER_BUFFER_POOL_SIZE - mb.buffers_available; +} + + +void mp_wait_for_buffer() {while (!mb.buffers_available) continue;} bool mp_queue_empty() {return mb.w == mb.r;} @@ -123,7 +124,6 @@ mp_buffer_t *mp_get_write_buffer() { mb.w = w->nx; mb.buffers_available--; - report_request(); return w; } @@ -142,6 +142,7 @@ mp_buffer_t *mp_get_write_buffer() { void mp_commit_write_buffer(uint32_t line) { mp_state_running(); + mb.q->ts = rtc_get_time(); mb.q->line = line; mb.q->run_state = MOVE_NEW; mb.q->buffer_state = MP_BUFFER_QUEUED; @@ -174,7 +175,6 @@ void mp_free_run_buffer() { // EMPTY current run buf & adv to next mp_clear_buffer(mb.r); // clear it out (& reset replannable) mb.r = mb.r->nx; // advance to next run buffer mb.buffers_available++; - report_request(); } diff --git a/src/plan/buffer.h b/src/plan/buffer.h index 56c63bb..b19f16c 100644 --- a/src/plan/buffer.h +++ b/src/plan/buffer.h @@ -34,7 +34,7 @@ typedef enum { - MOVE_OFF, // move inactive (MUST BE ZERO) + MOVE_OFF, // move inactive (must be zero) MOVE_NEW, // initial value MOVE_INIT, // first run MOVE_RUN, // general run state (for non-acceleration moves) @@ -61,6 +61,7 @@ typedef struct mp_buffer_t { // See Planning Velocity Notes struct mp_buffer_t *pv; // pointer to previous buffer struct mp_buffer_t *nx; // pointer to next buffer + uint32_t ts; // Time stamp bf_func_t bf_func; // callback to buffer exec function mach_func_t mach_func; // callback to machine @@ -97,18 +98,17 @@ typedef struct mp_buffer_t { // See Planning Velocity Notes } mp_buffer_t; +void mp_init_buffers(); uint8_t mp_get_planner_buffer_room(); +uint8_t mp_get_planner_buffer_fill(); void mp_wait_for_buffer(); -void mp_init_buffers(); bool mp_queue_empty(); mp_buffer_t *mp_get_write_buffer(); void mp_commit_write_buffer(uint32_t line); mp_buffer_t *mp_get_run_buffer(); void mp_free_run_buffer(); mp_buffer_t *mp_get_last_buffer(); -/// Returns pointer to prev buffer in linked list #define mp_get_prev_buffer(b) (b->pv) -/// Returns pointer to next buffer in linked list #define mp_get_next_buffer(b) (b->nx) void mp_clear_buffer(mp_buffer_t *bf); void mp_copy_buffer(mp_buffer_t *bf, const mp_buffer_t *bp); diff --git a/src/plan/exec.c b/src/plan/exec.c index a7c0952..5d33bed 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -32,6 +32,7 @@ #include "util.h" #include "runtime.h" #include "state.h" +#include "rtc.h" #include "config.h" #include @@ -242,7 +243,7 @@ static float _init_forward_diffs(float Vi, float Vt, float segments) { /// Common code for head and tail sections static stat_t _exec_aline_section(float length, float vin, float vout) { if (ex.section_new) { - if (fp_ZERO(length)) return STAT_OK; // end the move + if (fp_ZERO(length)) return STAT_NOOP; // end the move // len / avg. velocity float move_time = 2 * length / (vin + vout); @@ -297,16 +298,16 @@ static stat_t _exec_aline_body() { stat_t status = _exec_aline_section(ex.body_length, ex.cruise_velocity, ex.cruise_velocity); - if (status == STAT_OK) { - if (ex.section_new) return _exec_aline_tail(); - + switch (status) { + case STAT_NOOP: return _exec_aline_tail(); + case STAT_OK: ex.section = SECTION_TAIL; ex.section_new = true; return STAT_EAGAIN; - } - return status; + default: return status; + } } @@ -316,16 +317,16 @@ static stat_t _exec_aline_head() { stat_t status = _exec_aline_section(ex.head_length, ex.entry_velocity, ex.cruise_velocity); - if (status == STAT_OK) { - if (ex.section_new) return _exec_aline_body(); - + switch (status) { + case STAT_NOOP: return _exec_aline_body(); + case STAT_OK: ex.section = SECTION_BODY; ex.section_new = true; return STAT_EAGAIN; - } - return status; + default: return status; + } } @@ -515,14 +516,18 @@ stat_t mp_exec_aline(mp_buffer_t *bf) { /// Dequeues buffer and executes move callback stat_t mp_exec_move() { - if (mp_get_state() == STATE_ESTOPPED || - mp_get_state() == STATE_HOLDING) return STAT_OK; + if (mp_get_state() == STATE_ESTOPPED || mp_get_state() == STATE_HOLDING) + return STAT_NOOP; mp_buffer_t *bf = mp_get_run_buffer(); - if (!bf) return STAT_OK; // Nothing running + if (!bf) return STAT_NOOP; // Nothing running if (!bf->bf_func) return STAT_INTERNAL_ERROR; // Should never happen if (bf->run_state == MOVE_NEW) { + // On restart wait a bit to give planner queue a chance to fill + if (!mp_runtime_is_busy() && mp_get_planner_buffer_fill() < 4 && + !rtc_expired(bf->ts + 250)) return STAT_NOOP; + // Take control of buffer bf->run_state = MOVE_INIT; bf->replannable = false; @@ -566,5 +571,9 @@ stat_t mp_exec_move() { } } - return status; + switch (status) { + case STAT_NOOP: return STAT_EAGAIN; // Tell caller to call again + case STAT_EAGAIN: return STAT_OK; // Move queued, call again later + default: return status; + } } diff --git a/src/rtc.h b/src/rtc.h index d5f422d..3d69237 100644 --- a/src/rtc.h +++ b/src/rtc.h @@ -32,9 +32,7 @@ #include #include -#define RTC_MILLISECONDS 10 // interrupt on every 10 RTC ticks (~10 ms) - -void rtc_init(); // initialize and start general timer +void rtc_init(); uint32_t rtc_get_time(); int32_t rtc_diff(uint32_t t); bool rtc_expired(uint32_t t); diff --git a/src/stepper.c b/src/stepper.c index 8e5b0c7..8b60c30 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -101,8 +101,13 @@ ISR(ADCB_CH0_vect) { stat_t status = mp_exec_move(); switch (status) { - case STAT_NOOP: continue; // No stepper command was executed try again - case STAT_OK: case STAT_EAGAIN: break; // Ok + case STAT_NOOP: break; // No command executed + case STAT_EAGAIN: continue; // No command executed, try again + + case STAT_OK: // Move executed + if (!st.move_ready) CM_ALARM(STAT_EXPECTED_MOVE); // No move was queued + break; + default: CM_ALARM(status); break; } -- 2.27.0