Work in progress

diff --git a/src/machine.c b/src/machine.c
index ef71fab459eb67600e9cea9cd7a73b93ca0fb789..f9bd1d9447fcd55c947df51a9ddd34740f9b8361 100644 (file)
--- a/src/machine.c
+++ b/src/machine.c
@@ -151,7 +151,7 @@ static stat_t _exec_spindle_speed(mp_buffer_t *bf) {
 void mach_set_spindle_speed(float speed) {
   mp_buffer_t *bf = mp_queue_get_tail();
   bf->value = speed * mach_get_spindle_override();
-  mp_queue_push(_exec_spindle_speed, mach_get_line());
+  mp_queue_push(_exec_spindle_speed, false, mach_get_line());
 }
 
 
@@ -166,7 +166,7 @@ static stat_t _exec_spindle_mode(mp_buffer_t *bf) {
 void mach_set_spindle_mode(spindle_mode_t mode) {
   mp_buffer_t *bf = mp_queue_get_tail();
   bf->value = mode;
-  mp_queue_push(_exec_spindle_mode, mach_get_line());
+  mp_queue_push(_exec_spindle_mode, false, mach_get_line());
 }
 
 
@@ -253,7 +253,7 @@ void mach_update_work_offsets() {
   if (!same) {
     mp_buffer_t *bf = mp_queue_get_tail();
     copy_vector(bf->target, work_offset);
-    mp_queue_push(_exec_update_work_offsets, mach_get_line());
+    mp_queue_push(_exec_update_work_offsets, false, mach_get_line());
   }
 }
 
@@ -631,7 +631,7 @@ void mach_set_absolute_origin(float origin[], bool flags[]) {
   mp_buffer_t *bf = mp_queue_get_tail();
   copy_vector(bf->target, origin);
   copy_vector(bf->unit, flags);
-  mp_queue_push(_exec_absolute_origin, mach_get_line());
+  mp_queue_push(_exec_absolute_origin, false, mach_get_line());
 }
 
 
@@ -794,7 +794,7 @@ static stat_t _exec_change_tool(mp_buffer_t *bf) {
 void mach_change_tool(bool x) {
   mp_buffer_t *bf = mp_queue_get_tail();
   bf->value = mach.gm.tool;
-  mp_queue_push(_exec_change_tool, mach_get_line());
+  mp_queue_push(_exec_change_tool, false, mach_get_line());
 }
 
 
@@ -809,7 +809,7 @@ static stat_t _exec_mist_coolant(mp_buffer_t *bf) {
 void mach_mist_coolant_control(bool mist_coolant) {
   mp_buffer_t *bf = mp_queue_get_tail();
   bf->value = mist_coolant;
-  mp_queue_push(_exec_mist_coolant, mach_get_line());
+  mp_queue_push(_exec_mist_coolant, false, mach_get_line());
 }
 
 
@@ -824,7 +824,7 @@ static stat_t _exec_flood_coolant(mp_buffer_t *bf) {
 void mach_flood_coolant_control(bool flood_coolant) {
   mp_buffer_t *bf = mp_queue_get_tail();
   bf->value = flood_coolant;
-  mp_queue_push(_exec_flood_coolant, mach_get_line());
+  mp_queue_push(_exec_flood_coolant, false, mach_get_line());
 }
 
 
@@ -893,7 +893,7 @@ static stat_t _exec_program_stop(mp_buffer_t *bf) {
 
 /// M0 Queue a program stop
 void mach_program_stop() {
-  mp_queue_push(_exec_program_stop, mach_get_line());
+  mp_queue_push(_exec_program_stop, true, mach_get_line());
 }
 
 

diff --git a/src/plan/buffer.c b/src/plan/buffer.c
index e2f76bc7087a345a60a46c64ca7cd0ec35ef7a98..a9e30ccf75819f1b2bbeae37059899afba0d10f4 100644 (file)
--- a/src/plan/buffer.c
+++ b/src/plan/buffer.c
@@ -129,10 +129,11 @@ mp_buffer_t *mp_queue_get_tail() {
  * buffer once it has been queued.  Action may start on the buffer immediately,
  * invalidating its contents
  */
-void mp_queue_push(buffer_cb_t cb, uint32_t line) {
+void mp_queue_push(buffer_cb_t cb, bool plan, uint32_t line) {
   mp_state_running();
 
   mb.tail->ts = rtc_get_time();
+  mb.tail->plan = plan;
   mb.tail->cb = cb;
   mb.tail->line = line;
   mb.tail->run_state = MOVE_NEW;
@@ -151,3 +152,25 @@ void mp_queue_pop() {
   _clear_buffer(mb.head);
   _pop();
 }
+
+
+mp_buffer_t *mp_buffer_prev_plan(mp_buffer_t *bp) {
+  for (int i = 0; i < PLANNER_BUFFER_POOL_SIZE; i++) {
+    bp = mp_buffer_prev(bp);
+    if (bp->run_state == MOVE_OFF) break;
+    if (bp->plan) return bp;
+  }
+
+  return 0;
+}
+
+
+mp_buffer_t *mp_buffer_next_plan(mp_buffer_t *bp) {
+  for (int i = 0; i < PLANNER_BUFFER_POOL_SIZE; i++) {
+    bp = mp_buffer_next(bp);
+    if (bp->run_state == MOVE_OFF) break;
+    if (bp->plan) return bp;
+  }
+
+  return 0;
+}

diff --git a/src/plan/buffer.h b/src/plan/buffer.h
index 90e1f9971777d26791a3623ce9cd1240eb86a75b..cf1bd10033447f4c87990cb1c83b4917a37091c1 100644 (file)
--- a/src/plan/buffer.h
+++ b/src/plan/buffer.h
@@ -56,6 +56,7 @@ typedef struct mp_buffer_t {      // See Planning Velocity Notes
   buffer_cb_t cb;                 // callback to buffer exec function
 
   run_state_t run_state;          // run state machine sequence
+  bool plan;                      // if false, ignored by the planner
   bool replannable;               // true if move can be re-planned
 
   float value;                    // used in dwell and other callbacks
@@ -93,10 +94,12 @@ uint8_t mp_queue_get_fill();
 bool mp_queue_is_empty();
 
 mp_buffer_t *mp_queue_get_tail();
-void mp_queue_push(buffer_cb_t func, uint32_t line);
+void mp_queue_push(buffer_cb_t func, bool plan, uint32_t line);
 
 mp_buffer_t *mp_queue_get_head();
 void mp_queue_pop();
 
 static inline mp_buffer_t *mp_buffer_prev(mp_buffer_t *bp) {return bp->prev;}
 static inline mp_buffer_t *mp_buffer_next(mp_buffer_t *bp) {return bp->next;}
+mp_buffer_t *mp_buffer_prev_plan(mp_buffer_t *bp);
+mp_buffer_t *mp_buffer_next_plan(mp_buffer_t *bp);

diff --git a/src/plan/calibrate.c b/src/plan/calibrate.c
index 3e4919aae761e7369a581b0dfaddbec38a3fe9d3..d616836c20fe582251eef560c9ee1ad8b607f6ef 100644 (file)
--- a/src/plan/calibrate.c
+++ b/src/plan/calibrate.c
@@ -169,7 +169,7 @@ uint8_t command_calibrate(int argc, char *argv[]) {
   mp_set_cycle(CYCLE_CALIBRATING);
   cal.motor = 1;
 
-  mp_queue_push(_exec_calibrate, -1);
+  mp_queue_push(_exec_calibrate, false, -1);
 
   return 0;
 }

diff --git a/src/plan/dwell.c b/src/plan/dwell.c
index 07bba61c86d5d6edde81be18fa335b14d86eb9f1..485acb9e2c3ec2a617b390abcb89844386ff707f 100644 (file)
--- a/src/plan/dwell.c
+++ b/src/plan/dwell.c
@@ -48,7 +48,7 @@ static stat_t _exec_dwell(mp_buffer_t *bf) {
 stat_t mp_dwell(float seconds, int32_t line) {
   mp_buffer_t *bf = mp_queue_get_tail();
   bf->value = seconds; // in seconds, not minutes
-  mp_queue_push(_exec_dwell, line);
+  mp_queue_push(_exec_dwell, true, line);
 
   return STAT_OK;
 }

diff --git a/src/plan/exec.c b/src/plan/exec.c
index 612eb4d5d10487b805a89194dab7beb7c1ca6fde..6854ec44c1078a3c494d4961ce6968b8d03ee8e1 100644 (file)
--- a/src/plan/exec.c
+++ b/src/plan/exec.c
@@ -247,7 +247,7 @@ static stat_t _exec_aline_section(float length, float vin, float vout) {
 
     // len / avg. velocity
     float move_time = 2 * length / (vin + vout);
-    ex.segments = ceil(usec(move_time) / NOM_SEGMENT_USEC);
+    ex.segments = ceil(move_time * MICROSECONDS_PER_MINUTE / NOM_SEGMENT_USEC);
     ex.segment_time = move_time / ex.segments;
     ex.segment_count = (uint32_t)ex.segments;
 
@@ -551,7 +551,8 @@ stat_t mp_exec_move() {
       // Solves a potential race condition where the current move ends but
       // the new move has not started because the current move is still
       // being run by the steppers.  Planning can overwrite the new move.
-      mp_buffer_next(bf)->replannable = false;
+      mp_buffer_t *bp = mp_buffer_next_plan(bf);
+      if (bp) bp->replannable = false;
 
       mp_queue_pop(); // Free buffer
 

diff --git a/src/plan/jog.c b/src/plan/jog.c
index 79b5f494b2f60911e7002fe14732f682eceaa44c..f0b6068bc2fb490a82442e545a2f136a2217d71c 100644 (file)
--- a/src/plan/jog.c
+++ b/src/plan/jog.c
@@ -127,7 +127,7 @@ uint8_t command_jog(int argc, char *argv[]) {
 
   if (!mp_jog_busy()) {
     mp_set_cycle(CYCLE_JOGGING);
-    mp_queue_push(_exec_jog, -1);
+    mp_queue_push(_exec_jog, false, -1);
   }
 
   return STAT_OK;

diff --git a/src/plan/line.c b/src/plan/line.c
index c3a2003e3d79af36360a04bff32231f839d1106b..6ab556182aa28c6568f10724d14c161fef99d469 100644 (file)
--- a/src/plan/line.c
+++ b/src/plan/line.c
@@ -240,8 +240,8 @@ static void _calc_and_cache_jerk_values(mp_buffer_t *bf) {
 
 
 static void _calc_max_velocities(mp_buffer_t *bf, float move_time) {
-  float junction_velocity =
-    _get_junction_vmax(mp_buffer_prev(bf)->unit, bf->unit);
+  mp_buffer_t *bp = mp_buffer_prev_plan(bf);
+  float junction_velocity = bp ? _get_junction_vmax(bp->unit, bf->unit) : 0;
 
   bf->cruise_vmax = bf->length / move_time; // target velocity requested
   bf->entry_vmax = min(bf->cruise_vmax, junction_velocity);
@@ -307,9 +307,10 @@ stat_t mp_aline(const float target[], int32_t line) {
   _calc_max_velocities(bf, time);
 
   // Note, the following lines must remain in order.
+  bf->plan = true;
   mp_plan_block_list(bf);       // Plan block list
   mp_set_position(target);      // Set planner position before committing buffer
-  mp_queue_push(mp_exec_aline, line); // After position update
+  mp_queue_push(mp_exec_aline, true, line); // After position update
 
   return STAT_OK;
 }

diff --git a/src/plan/planner.c b/src/plan/planner.c
index 18d3545c967740788c53b773ad168e142c408a98..d43ec2acad80a90d6f251cfe45fb500eccdab7f9 100644 (file)
--- a/src/plan/planner.c
+++ b/src/plan/planner.c
@@ -261,7 +261,8 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) {
 
   // B" case: Block is short, but fits into a single body segment
   if (naive_move_time <= NOM_SEGMENT_TIME) {
-    bf->entry_velocity = mp_buffer_prev(bf)->exit_velocity;
+    mp_buffer_t *bp = mp_buffer_prev_plan(bf);
+    bf->entry_velocity = bp ? bp->exit_velocity : 0;
 
     if (fp_NOT_ZERO(bf->entry_velocity)) {
       bf->cruise_velocity = bf->entry_velocity;
@@ -495,8 +496,7 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) {
  *
  * [1] Whether or not a block is planned is controlled by the bf->replannable
  *     setting.  Replan flags are checked during the backwards pass.  They prune
- *     the replan list to include only the the latest blocks that require
- *     planning.
+ *     the replan list to include only the latest blocks that require planning.
  *
  *     In normal operation, the first block (currently running block) is not
  *     replanned, but may be for feedholds and feed overrides.  In these cases,
@@ -507,40 +507,52 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) {
  *     optimizations.
  */
 void mp_plan_block_list(mp_buffer_t *bf) {
+  ASSERT(bf->plan); // Must start with a plannable buffer
+
   mp_buffer_t *bp = bf;
 
   // Backward planning pass.  Find first block and update braking velocities.
   // By the end bp points to the buffer before the first block.
+  mp_buffer_t *next = bp;
   while ((bp = mp_buffer_prev(bp)) != bf) {
+    if (!bp->plan && bp->run_state != MOVE_OFF) continue;
     if (!bp->replannable) break;
     bp->braking_velocity =
-      min(mp_buffer_next(bp)->entry_vmax,
-          mp_buffer_next(bp)->braking_velocity) + bp->delta_vmax;
+      min(next->entry_vmax, next->braking_velocity) + bp->delta_vmax;
+    next = bp;
   }
 
   // Forward planning pass.  Recompute trapezoids from the first block to bf.
+  mp_buffer_t *prev = bp;
   while ((bp = mp_buffer_next(bp)) != bf) {
-    if (mp_buffer_prev(bp) == bf)
-      bp->entry_velocity = bp->entry_vmax; // first block
-    else bp->entry_velocity = mp_buffer_prev(bp)->exit_velocity; // other blocks
+    if (!bp->plan) continue;
+
+    if (prev == bf) bp->entry_velocity = bp->entry_vmax; // first block
+    else bp->entry_velocity = prev->exit_velocity;       // other blocks
+
+    // Note, next cannot be null.  Since bp != bf, bf is yet to come.
+    mp_buffer_t *next = mp_buffer_next_plan(bp);
+    ASSERT(next);
 
     bp->cruise_velocity = bp->cruise_vmax;
-    bp->exit_velocity = min4(bp->exit_vmax, mp_buffer_next(bp)->entry_vmax,
-                             mp_buffer_next(bp)->braking_velocity,
+    bp->exit_velocity = min4(bp->exit_vmax, next->entry_vmax,
+                             next->braking_velocity,
                              bp->entry_velocity + bp->delta_vmax);
 
     mp_calculate_trapezoid(bp);
 
     // Test for optimally planned trapezoids by checking exit conditions
     if  ((fp_EQ(bp->exit_velocity, bp->exit_vmax) ||
-          fp_EQ(bp->exit_velocity, mp_buffer_next(bp)->entry_vmax)) ||
-         (!mp_buffer_prev(bp)->replannable &&
+          fp_EQ(bp->exit_velocity, next->entry_vmax)) ||
+         (!prev->replannable &&
           fp_EQ(bp->exit_velocity, (bp->entry_velocity + bp->delta_vmax))))
       bp->replannable = false;
+
+    prev = bp;
   }
 
   // Finish last block
-  bp->entry_velocity = mp_buffer_prev(bp)->exit_velocity;
+  bp->entry_velocity = prev->exit_velocity;
   bp->cruise_velocity = bp->cruise_vmax;
   bp->exit_velocity = 0;
 
@@ -554,12 +566,18 @@ void mp_replan_blocks() {
 
   mp_buffer_t *bp = bf;
 
+  // Skip leading non-plannable blocks
+  while (!bp->plan) {
+    bp = mp_buffer_next(bp);
+    if (bp->run_state == MOVE_OFF || bp == bf) return; // Nothing to plan
+  }
+
   // Mark all blocks replanable
   while (true) {
     bp->replannable = true;
-    if (mp_buffer_next(bp)->run_state == MOVE_OFF || mp_buffer_next(bp) == bf)
-      break;
-    bp = mp_buffer_next(bp);
+    mp_buffer_t *next = mp_buffer_next(bp);
+    if (next->run_state == MOVE_OFF || next == bf) break;
+    bp = next;
   }
 
   // Plan blocks