From 077649c17d9a8bd8af465096d3181c5b3c59e6a4 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Wed, 7 Mar 2018 22:13:30 -0800 Subject: [PATCH] Improved state handling, work on jogging, optional pause --- src/avr/Makefile | 1 - src/avr/src/jog.c | 75 ++++++++++++++--------------- src/avr/src/state.c | 109 +++++++++++++------------------------------ src/avr/src/state.h | 1 - src/avr/src/vars.def | 1 + 5 files changed, 68 insertions(+), 119 deletions(-) diff --git a/src/avr/Makefile b/src/avr/Makefile index 304ef1c..d232216 100644 --- 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 801dec8..a678820 100644 --- 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 ebdeae5..f7940f1 100644 --- 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 fab452f..e746994 100644 --- 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 b681d0c..f44fcc8 100644 --- 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") -- 2.27.0