Smooth move transitions, enforce move generation with alarm, wait a bit for planner...

diff --git a/src/messages.def b/src/messages.def
index d48da3bc58aa47174b8b80e47b735f5aac99a65f..b731d73619a9c5def26102c3f2100b883e8aaceb 100644 (file)
--- 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 e6335d9016d77cc19976c3079b4ec94ac1623c12..92099986b6ff93d305fbde117cc841ec161890d7 100644 (file)
--- 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 1192fb55000e926f486fdb5342c236c3b9378385..6ff9d73f7cc20f43fcf33a2d64e88de702028c1f 100644 (file)
--- 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 <string.h>
 
@@ -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 56c63bb5b5a218c16228239d67939c33629c3fcc..b19f16ca99b1aa1c3353935ed490894162309d6d 100644 (file)
--- 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 a7c0952dc32e49f06202e4f2accbe0e382d21ac7..5d33bed73b47236baf5f871b00726126175bbdc7 100644 (file)
--- 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 <string.h>
@@ -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 d5f422d7ac57709eebf47adb33c3d3ce5422775f..3d692379e1eb04b750051059539e32c966163b2b 100644 (file)
--- a/src/rtc.h
+++ b/src/rtc.h
@@ -32,9 +32,7 @@
 #include <stdint.h>
 #include <stdbool.h>
 
-#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 8e5b0c7d2010c8904b27f25a023492949aea72ab..8b60c3021ef5de82004ede5b30e5c7752dd33be8 100644 (file)
--- 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;
     }