Improved state handling, work on jogging, optional pause

diff --git a/src/avr/Makefile b/src/avr/Makefile
index 304ef1c54337d64dfe11e5c2119d6e0c4c17a5ec..d23221650901029284161b86e6444665bfbcc498 100644 (file)
--- a/src/avr/Makefile
+++ b/src/avr/Makefile
@@ -7,7 +7,6 @@ TARGET  = $(PROJECT).elf
 
 # Compile flags
 CC = avr-gcc
-CPP = avr-g++
 
 COMMON = -mmcu=$(MCU) -flto -fwhole-program
 

diff --git a/src/avr/src/jog.c b/src/avr/src/jog.c
index 801dec8f21a7032169976508fbd61c6ddaf15f7d..a6788203476ffb1ae3ddbaa9aae429525b52dc3a 100644 (file)
--- a/src/avr/src/jog.c
+++ b/src/avr/src/jog.c
@@ -102,32 +102,26 @@ static float _compute_deccel_dist(float vel, float accel, float maxA,
   // Compute distance to decrease accel to zero
   if (0 < accel) {
     float t = accel / jerk;
-
-    // s(t) = v * t + 1/3 * a * t^2
-    dist += vel * t + 1.0 / 3.0 * accel * t * t;
-
-    // v(t) = a * t / 2 + v
-    vel += accel * t / 2;
+    dist += scurve_distance(t, vel, accel, -jerk);
+    vel += scurve_velocity(t, accel, -jerk);
     accel = 0;
-    t = 0;
   }
 
-  // At this point accel <= 0, aka a deccelleration
+  // At this point accel <= 0, aka a decceleration
 
   // Compute max deccel by applying the quadratic formula.
-  //   (1 / j) * Am^2 + ((1 - a) / j) * Am + (a^2 - 0.5 * a) / j + v = 0
   float t = accel / jerk;
-  float a = 1 / jerk;
-  float b = a - t;
-  float c = t * (accel - 0.5) + vel;
-  float maxDeccel = (-b - sqrt(b * b - 4 * a * c)) / a * 0.5;
+  float a = -1 / jerk;
+  float b = 2 * t;
+  float c = vel - 1.5 * t * accel;
+  float maxDeccel = (-b + sqrt(b * b - 4 * a * c)) / a * 0.5;
 
   // Limit decceleration
   if (maxDeccel < -maxA) maxDeccel = -maxA;
 
   // Compute distance and velocity change to max deccel
   if (maxDeccel < accel) {
-    float t = (accel - maxDeccel) / jerk;
+    float t = (maxDeccel - accel) / jerk;
     dist += scurve_distance(t, vel, accel, -jerk);
     vel += scurve_velocity(t, accel, -jerk);
     accel = maxDeccel;
@@ -174,10 +168,8 @@ static float _soft_limit(int axis, float V, float Vt, float A) {
   if (max <= position) return !positive ? Vt : 0;
 
   // Min velocity near limits
-  if (positive && max < position + 5) return MIN_VELOCITY;
-  if (!positive && position - 5 < min) return MIN_VELOCITY;
-
-  return Vt; // TODO compute deccel dist
+  if (positive && max < position + 1) return MIN_VELOCITY;
+  if (!positive && position - 1 < min) return MIN_VELOCITY;
 
   // Compute dist to deccel
   float jerk = axis_get_jerk_max(axis);
@@ -192,29 +184,6 @@ static float _soft_limit(int axis, float V, float Vt, float A) {
 }
 
 
-static bool _axis_velocity_target(int axis) {
-  jog_axis_t *a = &jr.axes[axis];
-
-  float Vn = a->next * axis_get_velocity_max(axis);
-  float Vi = a->velocity;
-  float Vt = a->target;
-
-  if (MIN_VELOCITY < fabs(Vn)) jr.done = false; // Still jogging
-
-  if (!fp_ZERO(Vi) && (Vn < 0) != (Vi < 0))
-    Vn = 0; // Plan to zero on sign change
-
-  if (fabs(Vn) < MIN_VELOCITY) Vn = 0;
-
-  if (Vt == Vn) return false; // No change
-
-  a->target = Vn;
-  if (Vn) a->sign = Vn < 0 ? -1 : 1;
-
-  return true; // Velocity changed
-}
-
-
 static float _compute_axis_velocity(int axis) {
   jog_axis_t *a = &jr.axes[axis];
 
@@ -241,6 +210,29 @@ static float _compute_axis_velocity(int axis) {
 }
 
 
+static bool _axis_velocity_target(int axis) {
+  jog_axis_t *a = &jr.axes[axis];
+
+  float Vn = a->next * axis_get_velocity_max(axis);
+  float Vi = a->velocity;
+  float Vt = a->target;
+
+  if (MIN_VELOCITY < fabs(Vn)) jr.done = false; // Still jogging
+
+  if (!fp_ZERO(Vi) && (Vn < 0) != (Vi < 0))
+    Vn = 0; // Plan to zero on sign change
+
+  if (fabs(Vn) < MIN_VELOCITY) Vn = 0;
+
+  if (Vt == Vn) return false; // No change
+
+  a->target = Vn;
+  if (Vn) a->sign = Vn < 0 ? -1 : 1;
+
+  return true; // Velocity changed
+}
+
+
 stat_t jog_exec() {
   // Load next velocity
   jr.done = true;
@@ -288,6 +280,7 @@ stat_t jog_exec() {
 
 
 void jog_stop() {
+  if (state_get() != STATE_JOGGING) return;
   jr.writing = true;
   for (int axis = 0; axis < AXES; axis++)
     jr.axes[axis].next = 0;

diff --git a/src/avr/src/state.c b/src/avr/src/state.c
index ebdeae54d2d24f74b9d9c0ebaae737469d3c0143..f7940f128df6cdaf43729694ea8cfffc27c124f5 100644 (file)
--- a/src/avr/src/state.c
+++ b/src/avr/src/state.c
@@ -48,11 +48,11 @@ static struct {
   bool pause_requested;
   bool optional_pause_requested;
   bool unpause_requested;
-  bool flush_requested;
-  bool resume_requested;
+  bool flushing;
+  bool resuming;
 
 } s = {
-  .flush_requested = true, // Start out flushing
+  .flushing = true, // Start out flushing
 };
 
 
@@ -72,9 +72,9 @@ PGM_P state_get_pgmstr(state_t state) {
 
 PGM_P state_get_hold_reason_pgmstr(hold_reason_t reason) {
   switch (reason) {
-  case HOLD_REASON_USER_PAUSE:    return PSTR("User paused");
+  case HOLD_REASON_USER_PAUSE:    return PSTR("User pause");
   case HOLD_REASON_USER_STOP:     return PSTR("User stop");
-  case HOLD_REASON_PROGRAM_PAUSE: return PSTR("Program paused");
+  case HOLD_REASON_PROGRAM_PAUSE: return PSTR("Program pause");
   case HOLD_REASON_SWITCH_FOUND:  return PSTR("Switch found");
   }
 
@@ -100,8 +100,8 @@ static void _set_hold_reason(hold_reason_t reason) {
 }
 
 
-bool state_is_flushing() {return s.flush_requested && !s.resume_requested;}
-bool state_is_resuming() {return s.resume_requested;}
+bool state_is_flushing() {return s.flushing && !s.resuming;}
+bool state_is_resuming() {return s.resuming;}
 
 
 bool state_is_quiescent() {
@@ -119,32 +119,23 @@ void state_seek_hold() {
 
 
 static void _stop() {
-  switch (state_get()) {
-  case STATE_STOPPING:
-    if (!exec_get_velocity()) {
-      _set_state(STATE_READY);
-      _stop();
-      return;
-    }
+  _set_hold_reason(HOLD_REASON_USER_STOP);
 
+  switch (state_get()) {
   case STATE_RUNNING:
-    _set_hold_reason(HOLD_REASON_USER_STOP);
     _set_state(STATE_STOPPING);
     break;
 
   case STATE_JOGGING:
-    jog_stop();
-    // Fall through
-
   case STATE_READY:
   case STATE_HOLDING:
-    s.flush_requested = true;
+    jog_stop();
     spindle_stop();
     outputs_stop();
     seek_cancel();
-    _set_state(STATE_READY);
     break;
 
+  case STATE_STOPPING:
   case STATE_ESTOPPED:
     break; // Ignore
   }
@@ -153,27 +144,13 @@ static void _stop() {
 
 void state_holding() {
   _set_state(STATE_HOLDING);
-
-  switch (s.hold_reason) {
-  case HOLD_REASON_PROGRAM_PAUSE: break;
-
-  case HOLD_REASON_USER_PAUSE:
-  case HOLD_REASON_SWITCH_FOUND:
-    s.flush_requested = true;
-    break;
-
-  case HOLD_REASON_USER_STOP:
-    _stop();
-    break;
-  }
+  if (s.hold_reason == HOLD_REASON_USER_STOP) _stop();
 }
 
 
-void state_pause(bool optional) {
-  if (!optional || s.optional_pause_requested) {
-    _set_hold_reason(HOLD_REASON_PROGRAM_PAUSE);
-    state_holding();
-  }
+void state_pause() {
+  _set_hold_reason(HOLD_REASON_PROGRAM_PAUSE);
+  state_holding();
 }
 
 
@@ -196,32 +173,11 @@ void state_idle() {
 void state_estop() {_set_state(STATE_ESTOPPED);}
 
 
-/*** Pauses, queue flushes and starts are all related.  Request functions
- * set flags.  The callback interprets the flags according to these rules:
- *
- *   A pause request received:
- *     - during motion is honored
- *     - 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 hold is deferred until HOLDING state is entered.
- *       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 hold is deferred until HOLDING state is entered.
- *       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 (estop_triggered()) return;
 
   // Pause
-  if (s.pause_requested || s.flush_requested) {
+  if (s.pause_requested || s.flushing) {
     if (state_get() == STATE_RUNNING) {
       if (s.pause_requested) _set_hold_reason(HOLD_REASON_USER_PAUSE);
       _set_state(STATE_STOPPING);
@@ -237,21 +193,19 @@ void state_callback() {
   }
 
   // Only flush queue when idle or holding
-  if (s.flush_requested && state_is_quiescent()) {
+  if (s.flushing && state_is_quiescent()) {
     command_flush_queue();
 
     // Resume
-    if (s.resume_requested) {
-      s.flush_requested = s.resume_requested = false;
+    if (s.resuming) {
+      s.flushing = s.resuming = false;
       _set_state(STATE_READY);
     }
   }
 
-  // Don't start while flushing or stopping
-  if (s.unpause_requested && !s.flush_requested &&
-      state_get() != STATE_STOPPING) {
+  // Unpause
+  if (s.unpause_requested && !s.flushing && state_get() != STATE_STOPPING) {
     s.unpause_requested = false;
-    s.optional_pause_requested = false;
 
     if (state_get() == STATE_HOLDING) {
       // Check if any moves are buffered
@@ -265,17 +219,16 @@ void state_callback() {
 // Var callbacks
 PGM_P get_state() {return state_get_pgmstr(state_get());}
 PGM_P get_hold_reason() {return state_get_hold_reason_pgmstr(s.hold_reason);}
+bool get_optional_pause() {return s.optional_pause_requested;}
+void set_optional_pause(bool x) {s.optional_pause_requested = x;}
 
 
 // Command callbacks
 stat_t command_pause(char *cmd) {
   pause_t type = (pause_t)(cmd[1] - '0');
 
-  switch (type) {
-  case PAUSE_USER: s.pause_requested = true; break;
-  case PAUSE_OPTIONAL: s.optional_pause_requested = true; break;
-  default: command_push(cmd[0], &type); break;
-  }
+  if (type == PAUSE_USER) s.pause_requested = true;
+  else command_push(cmd[0], &type);
 
   return STAT_OK;
 }
@@ -286,8 +239,12 @@ unsigned command_pause_size() {return sizeof(pause_t);}
 
 void command_pause_exec(void *data) {
   switch (*(pause_t *)data) {
-  case PAUSE_PROGRAM: state_pause(false); break;
-  case PAUSE_PROGRAM_OPTIONAL: state_pause(true); break;
+  case PAUSE_PROGRAM_OPTIONAL:
+    if (!s.optional_pause_requested) return;
+    s.optional_pause_requested = false;
+    // Fall through
+
+  case PAUSE_PROGRAM: state_pause(); break;
   default: break;
   }
 }
@@ -306,12 +263,12 @@ stat_t command_unpause(char *cmd) {
 
 
 stat_t command_resume(char *cmd) {
-  if (s.flush_requested) s.resume_requested = true;
+  if (s.flushing) s.resuming = true;
   return STAT_OK;
 }
 
 
 stat_t command_flush(char *cmd) {
-  s.flush_requested = true;
+  s.flushing = true;
   return STAT_OK;
 }

diff --git a/src/avr/src/state.h b/src/avr/src/state.h
index fab452f52b512cf17d6e9757b0bced9eaabb7be3..e7469943d9a28f683c65be0cc1dbe4c6ab74fb02 100644 (file)
--- a/src/avr/src/state.h
+++ b/src/avr/src/state.h
@@ -52,7 +52,6 @@ typedef enum {
 
 typedef enum {
   PAUSE_USER,
-  PAUSE_OPTIONAL,
   PAUSE_PROGRAM,
   PAUSE_PROGRAM_OPTIONAL,
 } pause_t;

diff --git a/src/avr/src/vars.def b/src/avr/src/vars.def
index b681d0c29c80700fa83b51365f763bc651190049..f44fcc816261b72a8aa91640d89f1f63b9bbd27b 100644 (file)
--- a/src/avr/src/vars.def
+++ b/src/avr/src/vars.def
@@ -112,6 +112,7 @@ VAR(id,              id, u32,   0,      1, 1, "Last executed command ID")
 VAR(speed,           s,  f32,   0,      1, 1, "Current spindle speed")
 VAR(feed_override,   fo, f32,   0,      1, 1, "Feed rate override")
 VAR(speed_override,  so, f32,   0,      1, 1, "Spindle speed override")
+VAR(optional_pause,  op, bool,  0,      1, 1, "Optional pause state")
 
 // System
 VAR(velocity,        v,  f32,   0,      0, 1, "Current velocity")