From bebea7ae21e6ca3ae61e0bd2c5dd082f3c373a86 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Mon, 26 Nov 2018 17:54:11 -0800 Subject: [PATCH] Fixes for speed / sync speed handling --- src/avr/src/huanyang.c | 2 + src/avr/src/pwm.c | 4 +- src/avr/src/spindle.c | 189 +++++++++++++++++++++----------------- src/avr/src/spindle.h | 2 - src/avr/src/stepper.c | 12 ++- src/avr/src/vfd_spindle.c | 2 + 6 files changed, 119 insertions(+), 92 deletions(-) diff --git a/src/avr/src/huanyang.c b/src/avr/src/huanyang.c index 4ad7ca5..3c02539 100644 --- a/src/avr/src/huanyang.c +++ b/src/avr/src/huanyang.c @@ -279,10 +279,12 @@ void huanyang_deinit(deinit_cb_t cb) { void huanyang_set(float power) { + cli(); if (hy.power != power && !hy.shutdown) { hy.power = power; hy.changed = true; } + sei(); } diff --git a/src/avr/src/pwm.c b/src/avr/src/pwm.c index a19722a..524a4a3 100644 --- a/src/avr/src/pwm.c +++ b/src/avr/src/pwm.c @@ -186,7 +186,7 @@ float get_pwm_min_duty() {return pwm.min_duty * 100;} void set_pwm_min_duty(float value) { - pwm.min_duty = value / 100; + pwm.min_duty = value * 0.01; _update_pwm(); } @@ -195,7 +195,7 @@ float get_pwm_max_duty() {return pwm.max_duty * 100;} void set_pwm_max_duty(float value) { - pwm.max_duty = value / 100; + pwm.max_duty = value * 0.01; _update_pwm(); } diff --git a/src/avr/src/spindle.c b/src/avr/src/spindle.c index 0e5a2f1..dacf806 100644 --- a/src/avr/src/spindle.c +++ b/src/avr/src/spindle.c @@ -52,11 +52,11 @@ static struct { bool reversed; float min_rpm; float max_rpm; + float inv_max_rpm; bool dynamic_power; float inv_feed; - bool dirty; spindle_type_t next_type; } spindle = { @@ -66,59 +66,64 @@ static struct { }; -static float _speed_to_power(float speed) { - speed *= spindle.override; +static float _get_power() { + switch (spindle.type) { + case SPINDLE_TYPE_DISABLED: return 0; + case SPINDLE_TYPE_PWM: return pwm_get(); + case SPINDLE_TYPE_HUANYANG: return huanyang_get(); + default: return vfd_spindle_get(); + } +} - bool negative = speed < 0; - if (fabs(speed) < spindle.min_rpm) speed = 0; - else if (spindle.max_rpm <= fabs(speed)) speed = negative ? -1 : 1; - else speed /= spindle.max_rpm; - return spindle.reversed ? -speed : speed; -} +static float _speed_to_power(float speed) { + bool negative = speed < 0; + float power = fabs(speed * spindle.override); + if (power < spindle.min_rpm) power = 0; + else if (spindle.max_rpm <= power) power = 1; + else power *= spindle.inv_max_rpm; -static power_update_t _get_update() { - float power = _speed_to_power(spindle.speed); - if (spindle.dynamic_power) - power *= spindle.inv_feed ? spindle.inv_feed * exec_get_velocity() : 1; - return pwm_get_update(power); + return (spindle.reversed ^ negative) ? -power : power; } static void _set_speed(float speed) { spindle.speed = speed; - spindle.dirty = false; - - if (spindle.type == SPINDLE_TYPE_PWM) { - // PWM speed updates must be synchronized with stepper movement - power_update_t update = _get_update(); - spindle_update(update); - return; - } float power = _speed_to_power(speed); switch (spindle.type) { case SPINDLE_TYPE_DISABLED: break; + + case SPINDLE_TYPE_PWM: { + // PWM speed updates must be synchronized with stepper movement + spindle.sync_speed.dist = 0; + spindle.sync_speed.speed = speed; + break; + } + case SPINDLE_TYPE_HUANYANG: huanyang_set(power); break; default: vfd_spindle_set(power); break; } } +static void _update_speed() {_set_speed(spindle.speed);} + + static void _deinit_cb() { spindle.type = spindle.next_type; spindle.next_type = SPINDLE_TYPE_DISABLED; switch (spindle.type) { - case SPINDLE_TYPE_DISABLED: break; - case SPINDLE_TYPE_PWM: pwm_init(); break; - case SPINDLE_TYPE_HUANYANG: huanyang_init(); break; - default: vfd_spindle_init(); break; + case SPINDLE_TYPE_DISABLED: break; + case SPINDLE_TYPE_PWM: pwm_init(); break; + case SPINDLE_TYPE_HUANYANG: huanyang_init(); break; + default: vfd_spindle_init(); break; } - spindle.dirty = true; + _update_speed(); } @@ -130,10 +135,10 @@ static void _set_type(spindle_type_t type) { spindle.type = SPINDLE_TYPE_DISABLED; switch (old_type) { - case SPINDLE_TYPE_DISABLED: _deinit_cb(); break; - case SPINDLE_TYPE_PWM: pwm_deinit(_deinit_cb); break; - case SPINDLE_TYPE_HUANYANG: huanyang_deinit(_deinit_cb); break; - default: vfd_spindle_deinit(_deinit_cb); break; + case SPINDLE_TYPE_DISABLED: _deinit_cb(); break; + case SPINDLE_TYPE_PWM: pwm_deinit(_deinit_cb); break; + case SPINDLE_TYPE_HUANYANG: huanyang_deinit(_deinit_cb); break; + default: vfd_spindle_deinit(_deinit_cb); break; } } @@ -141,16 +146,27 @@ static void _set_type(spindle_type_t type) { spindle_type_t spindle_get_type() {return spindle.type;} +static power_update_t _get_power_update() { + float power = _speed_to_power(spindle.speed); + + // Handle dynamic power + if (spindle.dynamic_power && spindle.inv_feed) { + float scale = spindle.inv_feed * exec_get_velocity(); + if (scale < 1) power *= scale; + } + + return pwm_get_update(power); +} + + void spindle_load_power_updates(power_update_t updates[], float minD, float maxD) { float stepD = (maxD - minD) * (1.0 / POWER_MAX_UPDATES); float d = minD + 1e-3; // Starting distance for (unsigned i = 0; i < POWER_MAX_UPDATES; i++) { - bool set = false; - + bool changed = false; d += stepD; // Ending distance for this power step - updates[i].state = POWER_IGNORE; while (true) { // Load new sync speed if needed and available @@ -158,85 +174,91 @@ void spindle_load_power_updates(power_update_t updates[], float minD, spindle.sync_speed = *(sync_speed_t *)(command_next() + 1); // Exit if we don't have a speed or it's not ready to be set - if (spindle.sync_speed.dist < 0 || d < spindle.sync_speed.dist) break; + if (spindle.sync_speed.dist == -1 || d < spindle.sync_speed.dist) break; - set = true; - spindle.speed = spindle.sync_speed.speed; + // Load sync speed spindle.sync_speed.dist = -1; // Mark done + spindle.speed = spindle.sync_speed.speed; + changed = true; } - // Prep power update - if (spindle.type == SPINDLE_TYPE_PWM) updates[i] = _get_update(); - else if (set) _set_speed(spindle.speed); // Set speed now for non-PWM + if (spindle.type == SPINDLE_TYPE_PWM) updates[i] = _get_power_update(); + else { + updates[i].state = POWER_IGNORE; + if (changed) _update_speed(); + } } } -void spindle_update(power_update_t update) {return pwm_update(update);} - - -static void _flush_sync_speeds() { - spindle.sync_speed.dist = -1; - while (command_peek() == COMMAND_sync_speed) command_next(); -} +// Called from hi-level stepper interrupt +void spindle_update(power_update_t update) {pwm_update(update);} // Called from lo-level stepper interrupt void spindle_idle() { - if (spindle.dirty) _set_speed(spindle.speed); - _flush_sync_speeds(); // Flush speeds in case we are holding there are more -} - - -float spindle_get_speed() { - float speed = 0; + if (spindle.sync_speed.dist != -1) { + spindle.sync_speed.dist = -1; // Mark done + spindle.speed = spindle.sync_speed.speed; - switch (spindle.type) { - case SPINDLE_TYPE_DISABLED: break; - case SPINDLE_TYPE_PWM: speed = pwm_get(); break; - case SPINDLE_TYPE_HUANYANG: speed = huanyang_get(); break; - default: speed = vfd_spindle_get(); break; + if (spindle.type == SPINDLE_TYPE_PWM) spindle_update(_get_power_update()); + else _update_speed(); } - - return speed * spindle.max_rpm; -} - - -void spindle_stop() { - _flush_sync_speeds(); - _set_speed(0); } +void spindle_stop() {_set_speed(0);} // Only called when steppers have halted void spindle_estop() {_set_type(SPINDLE_TYPE_DISABLED);} -bool spindle_is_reversed() {return spindle.reversed;} // Var callbacks uint8_t get_tool_type() {return spindle.type;} void set_tool_type(uint8_t value) {_set_type((spindle_type_t)value);} -float get_speed() {return spindle_get_speed();} +float get_speed() {return _get_power() * spindle.max_rpm;} bool get_tool_reversed() {return spindle.reversed;} void set_tool_reversed(bool reversed) { if (spindle.reversed != reversed) { spindle.reversed = reversed; - spindle.dirty = true; + _update_speed(); } } float get_max_spin() {return spindle.max_rpm;} -void set_max_spin(float value) {spindle.max_rpm = value; spindle.dirty = true;} + + +void set_max_spin(float value) { + if (spindle.max_rpm != value) { + spindle.max_rpm = value; + spindle.inv_max_rpm = 1 / value; + _update_speed(); + } +} + + float get_min_spin() {return spindle.min_rpm;} -void set_min_spin(float value) {spindle.min_rpm = value; spindle.dirty = true;} + + +void set_min_spin(float value) { + if (spindle.min_rpm != value) { + spindle.min_rpm = value; + _update_speed(); + } +} + + uint16_t get_speed_override() {return spindle.override * 1000;} void set_speed_override(uint16_t value) { - spindle.override = value / 1000.0; - spindle.dirty = true; + value *= 0.001; + + if (spindle.override != value) { + spindle.override = value; + _update_speed(); + } } @@ -244,9 +266,10 @@ bool get_dynamic_power() {return spindle.dynamic_power;} void set_dynamic_power(bool enable) { - if (spindle.dynamic_power == enable) return; - spindle.dynamic_power = enable; - spindle.dirty = true; + if (spindle.dynamic_power != enable) { + spindle.dynamic_power = enable; + _update_speed(); + } } @@ -254,9 +277,10 @@ float get_inverse_feed() {return spindle.inv_feed;} void set_inverse_feed(float iF) { - if (spindle.inv_feed == iF) return; - spindle.inv_feed = iF; - spindle.dirty = true; + if (spindle.inv_feed != iF) { + spindle.inv_feed = iF; + _update_speed(); + } } @@ -267,8 +291,8 @@ stat_t command_sync_speed(char *cmd) { cmd++; // Skip command code // Get distance and speed - if (!decode_float(&cmd, &s.dist)) return STAT_BAD_FLOAT; - if (!decode_float(&cmd, &s.speed)) return STAT_BAD_FLOAT; + if (!decode_float(&cmd, &s.dist) || s.dist < 0) return STAT_BAD_FLOAT; + if (!decode_float(&cmd, &s.speed)) return STAT_BAD_FLOAT; // Queue command_push(COMMAND_sync_speed, &s); @@ -281,7 +305,6 @@ unsigned command_sync_speed_size() {return sizeof(sync_speed_t);} void command_sync_speed_exec(void *data) { - spindle.sync_speed.dist = -1; // Flush any left over _set_speed(((sync_speed_t *)data)->speed); } diff --git a/src/avr/src/spindle.h b/src/avr/src/spindle.h index 94de062..f7cb570 100644 --- a/src/avr/src/spindle.h +++ b/src/avr/src/spindle.h @@ -61,10 +61,8 @@ typedef void (*deinit_cb_t)(); spindle_type_t spindle_get_type(); -float spindle_get_speed(); void spindle_stop(); void spindle_estop(); -bool spindle_is_reversed(); void spindle_load_power_updates(power_update_t updates[], float minD, float maxD); void spindle_update(power_update_t update); diff --git a/src/avr/src/stepper.c b/src/avr/src/stepper.c index dc540c3..f81b5f1 100644 --- a/src/avr/src/stepper.c +++ b/src/avr/src/stepper.c @@ -189,16 +189,17 @@ static void _next_move() { // Start move _load_move(); - // Handle power updates - st.power_index = 0; - memcpy(st.powers, st.prep_powers, sizeof(st.powers)); - _update_power(); - // Request next move when not in a dwell. Requesting the next move may // power up motors which should not be powered up during a dwell. _request_exec_move(); } + // Handle power updates + st.power_index = 0; + memcpy(st.powers, st.prep_powers, sizeof(st.powers)); + memset(st.prep_powers, 0, sizeof(st.prep_powers)); + _update_power(); + st.busy = true; // Executing move so mark busy st.move_ready = false; // We are done with this move, flip the flag back } @@ -229,6 +230,7 @@ void st_prep_line(const float target[]) { /// Add a dwell to the move buffer void st_prep_dwell(float seconds) { ESTOP_ASSERT(!st.move_ready, STAT_STEPPER_NOT_READY); + spindle_load_power_updates(st.prep_powers, 0, 0); st.prep_dwell = seconds; st.move_queued = true; // signal prep buffer ready } diff --git a/src/avr/src/vfd_spindle.c b/src/avr/src/vfd_spindle.c index 74d55d7..6fa25b7 100644 --- a/src/avr/src/vfd_spindle.c +++ b/src/avr/src/vfd_spindle.c @@ -347,10 +347,12 @@ void vfd_spindle_deinit(deinit_cb_t cb) { void vfd_spindle_set(float power) { + cli(); if (vfd.power != power) { vfd.power = power; vfd.changed = true; } + sei(); } -- 2.27.0