From eb6223e73fdd843d976e63f50b2484e2921310ee Mon Sep 17 00:00:00 2001
From: Joseph Coffland <joseph@cauldrondevelopment.com>
Date: Thu, 4 Jan 2018 22:38:09 -0800
Subject: [PATCH] Implemented better line exec, added underflow check, other
 misc.

---
 src/avr/src/command.c  |  22 ++++---
 src/avr/src/command.h  |   2 +-
 src/avr/src/commands.c |   2 +-
 src/avr/src/config.h   |   6 +-
 src/avr/src/drv8711.c  |   2 +-
 src/avr/src/exec.c     |   5 +-
 src/avr/src/exec.h     |   4 +-
 src/avr/src/i2c.h      |   1 +
 src/avr/src/line.c     | 129 +++++++++++++++++++++++------------------
 src/avr/src/pgmspace.h |   2 +-
 src/avr/src/state.c    |  34 +++++------
 src/avr/src/state.h    |   4 +-
 src/avr/src/stepper.c  |  11 +++-
 src/avr/src/type.c     |   1 +
 src/avr/src/vars.def   |   5 +-
 15 files changed, 135 insertions(+), 95 deletions(-)

diff --git a/src/avr/src/command.c b/src/avr/src/command.c
index 565e98e..45baf5c 100644
--- a/src/avr/src/command.c
+++ b/src/avr/src/command.c
@@ -233,15 +233,22 @@ static char *_command_next() {
 }
 
 
-void command_callback() {
-  if (!_command_next()) return;
+bool command_callback() {
+  static char *block = 0;
 
+  if (!block) block = usart_readline();
+  if (!block) return false; // No command
+
+  cmd.active = true; // Disables LCD booting message
   stat_t status = STAT_OK;
 
-  switch (*_cmd) {
-  case 0: break; // Empty line
-  case '{': status = vars_parser(_cmd); break; // TODO is this necessary?
-  case '$': status = command_parser(_cmd); break;
+  // Special processing for synchronous commands
+  if (_is_synchronous(*block)) {
+    if (estop_triggered()) status = STAT_MACHINE_ALARMED;
+    else if (state_is_flushing()) status = STAT_NOOP; // Flush
+    else if (state_is_resuming() || _space() < _size(*block))
+      return false; // Wait
+  }
 
   default:
     if (estop_triggered()) {status = STAT_MACHINE_ALARMED; break;}
@@ -256,8 +263,9 @@ void command_callback() {
   _cmd = 0; // Command consumed
   _active = true;
   report_request();
+  if (status && status != STAT_NOOP) status_error(status);
 
-  if (status) status_error(status);
+  return true;
 }
 
 
diff --git a/src/avr/src/command.h b/src/avr/src/command.h
index 7d7c098..613eb1a 100644
--- a/src/avr/src/command.h
+++ b/src/avr/src/command.h
@@ -47,5 +47,5 @@ unsigned command_get_count();
 void command_print_help();
 void command_flush_queue();
 void command_push(char code, void *data);
-void command_callback();
+bool command_callback();
 bool command_exec();
diff --git a/src/avr/src/commands.c b/src/avr/src/commands.c
index 9ff56d9..d639a10 100644
--- a/src/avr/src/commands.c
+++ b/src/avr/src/commands.c
@@ -65,7 +65,7 @@ void command_out_exec(void *data) {}
 
 stat_t command_pause(char *cmd) {
   if (cmd[1] == '1') state_request_optional_pause();
-  else state_request_hold();
+  else state_request_pause();
   return STAT_OK;
 }
 
diff --git a/src/avr/src/config.h b/src/avr/src/config.h
index 8a490fe..5d8ced0 100644
--- a/src/avr/src/config.h
+++ b/src/avr/src/config.h
@@ -144,10 +144,10 @@ enum {
 
 
 // Timer setup for stepper and dwells
-#define STEP_TIMER_ENABLE      TC_CLKSEL_DIV4_gc
-#define STEP_TIMER_DIV         4
+#define STEP_TIMER_ENABLE      TC_CLKSEL_DIV8_gc
+#define STEP_TIMER_DIV         8
 #define STEP_TIMER_FREQ        (F_CPU / STEP_TIMER_DIV)
-#define STEP_TIMER_POLL        ((uint16_t)(STEP_TIMER_FREQ * 0.001))
+#define STEP_TIMER_POLL        ((uint16_t)(STEP_TIMER_FREQ * 0.001)) // 1ms
 #define STEP_TIMER_WGMODE      TC_WGMODE_NORMAL_gc // count to TOP & rollover
 #define STEP_TIMER_ISR         TCC0_OVF_vect
 #define STEP_TIMER_INTLVL      TC_OVFINTLVL_HI_gc
diff --git a/src/avr/src/drv8711.c b/src/avr/src/drv8711.c
index 18cd95d..e6760a3 100644
--- a/src/avr/src/drv8711.c
+++ b/src/avr/src/drv8711.c
@@ -62,7 +62,7 @@ typedef struct {
   drv8711_state_t state;
   current_t drive;
   current_t idle;
-  float stall_threshold;
+  float stall_threspause;
 
   uint8_t mode; // microstepping mode
   stall_callback_t stall_cb;
diff --git a/src/avr/src/exec.c b/src/avr/src/exec.c
index 02e7da8..6111d02 100644
--- a/src/avr/src/exec.c
+++ b/src/avr/src/exec.c
@@ -66,7 +66,7 @@ void exec_init() {
   ex.spindle_override = 1;
   ex.seek_switch = -1;
   // TODO implement seek
-  // TODO implement feedhold
+  // TODO implement pause
   // TODO implement move stepping
   // TODO implement overrides
   // TODO implement optional pause
@@ -83,7 +83,8 @@ void exec_set_velocity(float v) {ex.velocity = v;}
 float exec_get_velocity() {return ex.velocity;}
 void exec_set_acceleration(float a) {ex.accel = a;}
 void exec_set_jerk(float j) {ex.jerk = j;}
-void exec_set_line(int line) {ex.line = line;}
+void exec_set_line(int32_t line) {ex.line = line;}
+int32_t exec_get_line() {return ex.line;}
 void exec_set_cb(exec_cb_t cb) {ex.cb = cb;}
 
 
diff --git a/src/avr/src/exec.h b/src/avr/src/exec.h
index f2832a9..2ae2fbc 100644
--- a/src/avr/src/exec.h
+++ b/src/avr/src/exec.h
@@ -32,6 +32,7 @@
 #include "status.h"
 
 #include <stdbool.h>
+#include <stdint.h>
 
 
 typedef stat_t (*exec_cb_t)();
@@ -45,7 +46,8 @@ void exec_set_velocity(float v);
 float exec_get_velocity();
 void exec_set_acceleration(float a);
 void exec_set_jerk(float j);
-void exec_set_line(int line);
+void exec_set_line(int32_t line);
+int32_t exec_get_line();
 
 void exec_set_cb(exec_cb_t cb);
 
diff --git a/src/avr/src/i2c.h b/src/avr/src/i2c.h
index 373664f..0ef554a 100644
--- a/src/avr/src/i2c.h
+++ b/src/avr/src/i2c.h
@@ -29,6 +29,7 @@
 
 #include "config.h"
 
+#include <stdint.h>
 #include <stdbool.h>
 
 typedef void (*i2c_read_cb_t)(uint8_t *data, uint8_t length);
diff --git a/src/avr/src/line.c b/src/avr/src/line.c
index 4566308..81dd613 100644
--- a/src/avr/src/line.c
+++ b/src/avr/src/line.c
@@ -48,21 +48,17 @@ typedef struct {
 
 
 static struct {
-  float next_start[AXES];
-
   line_t line;
 
-  bool new;
   int seg;
-  uint16_t steps;
-  uint16_t step;
-  float delta;
+  bool stop_seg;
+  float current_t;
 
   float dist;
   float vel;
   float accel;
   float jerk;
-} l = {};
+} l = {.current_t = SEGMENT_TIME};
 
 
 static float _compute_distance(float t, float v, float a, float j) {
@@ -78,71 +74,89 @@ static float _compute_velocity(float t, float a, float j) {
 
 
 static bool _segment_next() {
-  while (++l.seg < 7)
-    if (l.line.times[l.seg]) return (l.new = true);
-  return false;
-}
-
-
-static void _segment_init() {
-  l.new = false;
-
-  // Jerk
-  switch (l.seg) {
-  case 0: case 6: l.jerk = l.line.max_jerk; break;
-  case 2: case 4: l.jerk = -l.line.max_jerk; break;
-  default: l.jerk = 0;
+  while (++l.seg < 7) {
+    float seg_t = l.line.times[l.seg];
+    if (!seg_t) continue;
+
+    // Check if last seg
+    bool last_seg = true;
+    for (int i = l.seg + 1; i < 7; i++)
+      if (l.line.times[i]) last_seg = false;
+
+    // Check if stop seg
+    l.stop_seg = last_seg && !l.line.target_vel;
+
+    // Jerk
+    switch (l.seg) {
+    case 0: case 6: l.jerk = l.line.max_jerk; break;
+    case 2: case 4: l.jerk = -l.line.max_jerk; break;
+    default: l.jerk = 0;
+    }
+    exec_set_jerk(l.jerk);
+
+    // Acceleration
+    switch (l.seg) {
+    case 1: case 2: l.accel = l.line.max_jerk * l.line.times[0]; break;
+    case 5: case 6: l.accel = -l.line.max_jerk * l.line.times[4]; break;
+    default: l.accel = 0;
+    }
+    exec_set_acceleration(l.accel);
+
+    // Skip segs which do not land on a SEGMENT_TIME
+    if (seg_t < l.current_t && !l.stop_seg) {
+      l.current_t -= l.line.times[l.seg];
+      l.dist += _compute_distance(seg_t, l.vel, l.accel, l.jerk);
+      l.vel += _compute_velocity(seg_t, l.accel, l.jerk);
+      continue;
+    }
+
+    return true;
   }
-  exec_set_jerk(l.jerk);
 
-  // Acceleration
-  switch (l.seg) {
-  case 1: case 2: l.accel = l.line.max_jerk * l.line.times[0]; break;
-  case 5: case 6: l.accel = -l.line.max_jerk * l.line.times[4]; break;
-  default: l.accel = 0;
-  }
-  exec_set_acceleration(l.accel);
-
-  // Compute interpolation steps
-  l.step = 0;
-  float time = l.line.times[l.seg];
-  l.steps = round(time / SEGMENT_TIME);
-  if (!l.steps) {l.steps = 1; l.delta = time;}
-  else l.delta = time / l.steps;
+  return false;
 }
 
 
 static stat_t _line_exec() {
-  if (l.new) _segment_init();
+  float seg_t = l.line.times[l.seg];
 
-  bool lastStep = l.step == l.steps - 1;
+  // Adjust time if near a stopping point
+  float delta = SEGMENT_TIME;
+  if (l.stop_seg && seg_t - l.current_t < SEGMENT_TIME) {
+    delta += seg_t - l.current_t;
+    l.current_t = seg_t;
+  }
 
-  // Compute time, distance and velocity offsets
-  float t = lastStep ? l.line.times[l.seg] : (l.delta * (l.step + 1));
-  float d = l.dist + _compute_distance(t, l.vel, l.accel, l.jerk);
-  float v = l.vel + _compute_velocity(t, l.accel, l.jerk);
+  // Compute distance and velocity offsets
+  float d = l.dist + _compute_distance(l.current_t, l.vel, l.accel, l.jerk);
+  float v = l.vel + _compute_velocity(l.current_t, l.accel, l.jerk);
 
   // Compute target position from distance
   float target[AXES];
   for (int i = 0; i < AXES; i++)
     target[i] = l.line.start[i] + l.line.unit[i] * d;
 
-  // Update dist and vel for next seg
-  if (lastStep) {
-    l.dist = d;
-    l.vel = v;
+  // Advance time
+  l.current_t += SEGMENT_TIME;
 
-  } else l.step++;
+  // Check if segment complete
+  bool lastSeg = false;
+  if (seg_t < l.current_t) {
+    l.current_t -= seg_t;
+    l.dist += _compute_distance(seg_t, l.vel, l.accel, l.jerk);
+    l.vel += _compute_velocity(seg_t, l.accel, l.jerk);
 
-  // Advance curve
-  bool lastCurve = lastStep && !_segment_next();
+    // Next segment
+    lastSeg = !_segment_next();
+  }
 
   // Do move
-  stat_t status =
-    exec_move_to_target(l.delta, lastCurve ? l.line.target : target);
+  // Stop exactly on target to correct for floating-point errors
+  stat_t status = exec_move_to_target
+    (delta, (lastSeg && l.stop_seg) ? l.line.target : target);
 
   // Check if we're done
-  if (lastCurve) {
+  if (lastSeg) {
     exec_set_velocity(l.vel = l.line.target_vel);
     exec_set_cb(0);
 
@@ -158,12 +172,13 @@ void _print_vector(const char *name, float v[AXES]) {
 
 
 stat_t command_line(char *cmd) {
+  static float next_start[AXES];
   line_t line = {};
 
   cmd++; // Skip command code
 
   // Get start position
-  copy_vector(line.start, l.next_start);
+  copy_vector(line.start, next_start);
 
   // Get target velocity
   if (!decode_float(&cmd, &line.target_vel)) return STAT_BAD_FLOAT;
@@ -178,6 +193,10 @@ stat_t command_line(char *cmd) {
   stat_t status = decode_axes(&cmd, line.target);
   if (status) return status;
 
+  // Zero disabled axes (TODO should moving a disable axis cause an error?)
+  for (int i = 0; i < AXES; i++)
+    if (!axis_is_enabled(i)) line.target[i] = 0;
+
   // Get times
   bool has_time = false;
   while (*cmd) {
@@ -199,7 +218,7 @@ stat_t command_line(char *cmd) {
   if (*cmd) return STAT_INVALID_ARGUMENTS;
 
   // Set next start position
-  copy_vector(l.next_start, line.target);
+  copy_vector(next_start, line.target);
 
   // Compute direction vector
   float length = 0;
@@ -233,7 +252,7 @@ void command_line_exec(void *data) {
 
   // Find first segment
   l.seg = -1;
-  _segment_next();
+  if (!_segment_next()) return;
 
   // Set callback
   exec_set_cb(_line_exec);
diff --git a/src/avr/src/pgmspace.h b/src/avr/src/pgmspace.h
index f0b1cd0..81cd667 100644
--- a/src/avr/src/pgmspace.h
+++ b/src/avr/src/pgmspace.h
@@ -36,7 +36,7 @@
 
 #define PRPSTR "s"
 #define PROGMEM
-#define PGM_P char *
+#define PGM_P const char *
 #define PSTR(X) X
 #define vfprintf_P vfprintf
 #define printf_P printf
diff --git a/src/avr/src/state.c b/src/avr/src/state.c
index 3d70f5f..bed09f3 100644
--- a/src/avr/src/state.c
+++ b/src/avr/src/state.c
@@ -40,7 +40,7 @@ static struct {
   state_t state;
   hold_reason_t hold_reason;
 
-  bool hold_requested;
+  bool pause_requested;
   bool flush_requested;
   bool start_requested;
   bool resume_requested;
@@ -106,14 +106,6 @@ bool state_is_quiescent() {
 }
 
 
-void state_optional_pause() {
-  if (s.optional_pause_requested) {
-    state_set_hold_reason(HOLD_REASON_USER_PAUSE);
-    state_holding();
-  }
-}
-
-
 static void _set_plan_steps(bool plan_steps) {} // TODO
 
 
@@ -123,6 +115,14 @@ void state_holding() {
 }
 
 
+void state_optional_pause() {
+  if (s.optional_pause_requested) {
+    state_set_hold_reason(HOLD_REASON_USER_PAUSE);
+    state_holding();
+  }
+}
+
+
 void state_running() {
   if (state_get() == STATE_READY) _set_state(STATE_RUNNING);
 }
@@ -130,7 +130,7 @@ void state_running() {
 
 void state_idle() {if (state_get() == STATE_RUNNING) _set_state(STATE_READY);}
 void state_estop() {_set_state(STATE_ESTOPPED);}
-void state_request_hold() {s.hold_requested = true;}
+void state_request_pause() {s.pause_requested = true;}
 void state_request_start() {s.start_requested = true;}
 void state_request_flush() {s.flush_requested = true;}
 void state_request_resume() {if (s.flush_requested) s.resume_requested = true;}
@@ -143,30 +143,30 @@ void state_request_step() {
 }
 
 
-/*** Feedholds, queue flushes and starts are all related.  Request functions
+/*** Pauses, queue flushes and starts are all related.  Request functions
  * set flags.  The callback interprets the flags according to these rules:
  *
- *   A hold request received:
+ *   A pause request received:
  *     - during motion is honored
- *     - during a feedhold is ignored and reset
+ *     - during a pause is ignored and reset
  *     - when already stopped is ignored and reset
  *
  *   A flush request received:
  *     - during motion is ignored but not reset
- *     - during a feedhold is deferred until the feedhold enters HOLDING state.
+ *     - during a pause is deferred until the feedpause enters HOLDING state.
  *       I.e. until deceleration is complete.
  *     - when stopped or holding and the exec is not busy, is honored
  *
  *   A start request received:
  *     - during motion is ignored and reset
- *     - during a feedhold is deferred until the feedhold enters HOLDING state.
+ *     - during a pause is deferred until the feedpause enters HOLDING state.
  *       I.e. until deceleration is complete.  If a queue flush request is also
  *       present the queue flush is done first
  *     - when stopped is honored and starts to run anything in the queue
  */
 void state_callback() {
-  if (s.hold_requested || s.flush_requested) {
-    s.hold_requested = false;
+  if (s.pause_requested || s.flush_requested) {
+    s.pause_requested = false;
     state_set_hold_reason(HOLD_REASON_USER_PAUSE);
 
     if (state_get() == STATE_RUNNING) _set_state(STATE_STOPPING);
diff --git a/src/avr/src/state.h b/src/avr/src/state.h
index dac8776..e6c658b 100644
--- a/src/avr/src/state.h
+++ b/src/avr/src/state.h
@@ -60,13 +60,13 @@ bool state_is_flushing();
 bool state_is_resuming();
 bool state_is_quiescent();
 
-void state_optional_pause();
 void state_holding();
+void state_optional_pause();
 void state_running();
 void state_idle();
 void state_estop();
 
-void state_request_hold();
+void state_request_pause();
 void state_request_start();
 void state_request_flush();
 void state_request_resume();
diff --git a/src/avr/src/stepper.c b/src/avr/src/stepper.c
index 48d2ec8..35464db 100644
--- a/src/avr/src/stepper.c
+++ b/src/avr/src/stepper.c
@@ -59,6 +59,8 @@ typedef struct {
   move_type_t move_type;
   float prep_dwell;
   uint16_t clock_period;
+
+  uint32_t underflow;
 } stepper_t;
 
 
@@ -166,6 +168,7 @@ static void _load_move() {
 
   // If the next move is not ready try to load it
   if (!st.move_ready) {
+    if (exec_get_velocity()) st.underflow++;
     _request_exec_move();
     _end_move();
     return;
@@ -205,6 +208,7 @@ void st_prep_line(float time, const float target[]) {
   // Trap conditions that would prevent queuing the line
   ASSERT(!st.move_ready);
   ASSERT(isfinite(time));
+  ASSERT(time * STEP_TIMER_FREQ * 60 <= 0xffff);
 
   // Setup segment parameters
   st.move_type = MOVE_TYPE_LINE;
@@ -212,8 +216,7 @@ void st_prep_line(float time, const float target[]) {
 
   // Prepare motor moves
   for (int motor = 0; motor < MOTORS; motor++)
-    //if (motor_is_enabled(motor))
-      motor_prep_move(motor, time, target[motor_get_axis(motor)]);
+    motor_prep_move(motor, time, target[motor_get_axis(motor)]);
 
   st.move_queued = true; // signal prep buffer ready (do this last)
 }
@@ -226,3 +229,7 @@ void st_prep_dwell(float seconds) {
   st.prep_dwell = seconds;
   st.move_queued = true; // signal prep buffer ready
 }
+
+
+// Var callbacks
+uint32_t get_underflow() {return st.underflow;}
diff --git a/src/avr/src/type.c b/src/avr/src/type.c
index c1d80e5..98ed175 100644
--- a/src/avr/src/type.c
+++ b/src/avr/src/type.c
@@ -33,6 +33,7 @@
 #include <math.h>
 #include <ctype.h>
 #include <stdlib.h>
+#include <inttypes.h>
 
 
 #define TYPEDEF(TYPE, DEF)                                              \
diff --git a/src/avr/src/vars.def b/src/avr/src/vars.def
index 2d16891..840848f 100644
--- a/src/avr/src/vars.def
+++ b/src/avr/src/vars.def
@@ -76,8 +76,8 @@ VAR(latch_backoff,   lb, f32,   MOTORS, 1, 1, "Homing latch backoff")
 VAR(zero_backoff,    zb, f32,   MOTORS, 1, 1, "Homing zero backoff")
 
 // Axis
-VAR(axis_position,    p,  f32,   AXES,   1, 1, "Axis position")
-VAR(axis_can_home,    ch, bool,  AXES,   0, 1, "Axis can home")
+VAR(axis_position,    p,  f32,   AXES,  1, 1, "Axis position")
+VAR(axis_can_home,    ch, bool,  AXES,  0, 1, "Axis can home")
 
 // Outputs
 VAR(output_state,    os, u8,    OUTS,   0, 1, "Output pin state")
@@ -130,3 +130,4 @@ VAR(estop,           es, bool,  0,      1, 1, "Emergency stop")
 VAR(estop_reason,    er, pstr,  0,      0, 1, "Emergency stop reason")
 VAR(state,           x,  pstr,  0,      0, 1, "Machine state")
 VAR(hold_reason,     pr, pstr,  0,      0, 1, "Machine pause reason")
+VAR(underflow,       un, u32,   0,      0, 1, "Stepper underflow count")
-- 
2.27.0