From efa29fb9b3f3ed8ec9c50985b2057275255959eb Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Wed, 9 Mar 2016 14:08:57 -0800 Subject: [PATCH] Mostly style updates --- src/canonical_machine.c | 20 +- src/canonical_machine.h | 3 +- src/config.h | 17 +- src/controller.c | 93 +++--- src/cycle_jogging.c | 169 +++++----- src/plan/arc.c | 3 +- src/plan/arc.h | 2 - src/plan/exec.c | 99 +++--- src/plan/kinematics.c | 5 +- src/plan/line.c | 543 +++++++++++++++++-------------- src/plan/planner.c | 2 +- src/plan/planner.h | 171 +++++----- src/plan/zoid.c | 76 ++--- src/stepper.c | 683 +++++++++++++++----------------------- src/stepper.h | 701 +++++++++++++++++++++------------------- src/vars.def | 2 +- 16 files changed, 1283 insertions(+), 1306 deletions(-) diff --git a/src/canonical_machine.c b/src/canonical_machine.c index 2091543..96faa28 100644 --- a/src/canonical_machine.c +++ b/src/canonical_machine.c @@ -414,19 +414,21 @@ void cm_set_model_target(float target[], float flag[]) { float tmp = 0; // process XYZABC for lower modes - for (axis=AXIS_X; axis<=AXIS_Z; axis++) { - if ((fp_FALSE(flag[axis])) || (cm.a[axis].axis_mode == AXIS_DISABLED)) - continue; // skip axis if not flagged for update or its disabled - else if ((cm.a[axis].axis_mode == AXIS_STANDARD) || (cm.a[axis].axis_mode == AXIS_INHIBITED)) { + for (axis = AXIS_X; axis <= AXIS_Z; axis++) { + if (fp_FALSE(flag[axis]) || cm.a[axis].axis_mode == AXIS_DISABLED) + continue; // skip axis if not flagged for update or its disabled + + else if (cm.a[axis].axis_mode == AXIS_STANDARD || cm.a[axis].axis_mode == AXIS_INHIBITED) { if (cm.gm.distance_mode == ABSOLUTE_MODE) cm.gm.target[axis] = cm_get_active_coord_offset(axis) + _to_millimeters(target[axis]); else cm.gm.target[axis] += _to_millimeters(target[axis]); } } + // FYI: The ABC loop below relies on the XYZ loop having been run first - for (axis=AXIS_A; axis<=AXIS_C; axis++) { - if ((fp_FALSE(flag[axis])) || (cm.a[axis].axis_mode == AXIS_DISABLED)) - continue; // skip axis if not flagged for update or its disabled + for (axis = AXIS_A; axis <= AXIS_C; axis++) { + if (fp_FALSE(flag[axis]) || cm.a[axis].axis_mode == AXIS_DISABLED) + continue; // skip axis if not flagged for update or its disabled else tmp = _calc_ABC(axis, target, flag); if (cm.gm.distance_mode == ABSOLUTE_MODE) @@ -435,6 +437,7 @@ void cm_set_model_target(float target[], float flag[]) { } } + /* * cm_test_soft_limits() - return error code if soft limit is exceeded * @@ -898,6 +901,7 @@ stat_t cm_straight_traverse(float target[], float flags[]) { cm_cycle_start(); // required for homing & other cycles mp_aline(&cm.gm); // send the move to the planner cm_finalize_move(); + return STAT_OK; } @@ -1408,7 +1412,7 @@ float cm_get_axis_jerk(uint8_t axis) { void cm_set_axis_jerk(uint8_t axis, float jerk) { cm.a[axis].jerk_max = jerk; - cm.a[axis].recip_jerk = 1/(jerk * JERK_MULTIPLIER); + cm.a[axis].recip_jerk = 1 / (jerk * JERK_MULTIPLIER); } diff --git a/src/canonical_machine.h b/src/canonical_machine.h index 48de721..0cdba9f 100644 --- a/src/canonical_machine.h +++ b/src/canonical_machine.h @@ -220,7 +220,7 @@ typedef struct cmSingleton { // struct to manage cm globals and cycles uint8_t distance_mode; // G90,G91 reset default // coordinate systems and offsets - float offset[COORDS+1][AXES]; // persistent coordinate offsets: absolute (G53) + G54,G55,G56,G57,G58,G59 + float offset[COORDS + 1][AXES]; // persistent coordinate offsets: absolute (G53) + G54,G55,G56,G57,G58,G59 // settings for axes X,Y,Z,A B,C cfgAxis_t a[AXES]; @@ -520,7 +520,6 @@ uint8_t cm_get_cycle_state(); uint8_t cm_get_motion_state(); uint8_t cm_get_hold_state(); uint8_t cm_get_homing_state(); -uint8_t cm_get_jogging_state(); void cm_set_motion_state(uint8_t motion_state); float cm_get_axis_jerk(uint8_t axis); void cm_set_axis_jerk(uint8_t axis, float jerk); diff --git a/src/config.h b/src/config.h index 4fdce29..362345b 100644 --- a/src/config.h +++ b/src/config.h @@ -3,7 +3,7 @@ // Compile-time settings #define __STEP_CORRECTION -//#define __JERK_EXEC // Use computed jerk (versus forward difference based exec) +//#define __JERK_EXEC // Use computed jerk (vs. forward difference) //#define __KAHAN // Use Kahan summation in aline exec functions #define INPUT_BUFFER_LEN 255 // text buffer size (255 max) @@ -25,11 +25,16 @@ // Motor settings #define MOTOR_MICROSTEPS 8 -#define MOTOR_POWER_MODE MOTOR_POWERED_IN_CYCLE // one of: MOTOR_DISABLED (0) - // MOTOR_ALWAYS_POWERED (1) - // MOTOR_POWERED_IN_CYCLE (2) - // MOTOR_POWERED_ONLY_WHEN_MOVING (3) -#define MOTOR_IDLE_TIMEOUT 2.00 // seconds to maintain motor at full power before idling + +/// One of: +/// MOTOR_DISABLED +/// MOTOR_ALWAYS_POWERED +/// MOTOR_POWERED_IN_CYCLE +/// MOTOR_POWERED_ONLY_WHEN_MOVING +#define MOTOR_POWER_MODE MOTOR_ALWAYS_POWERED + +/// Seconds to maintain motor at full power before idling +#define MOTOR_IDLE_TIMEOUT 2.00 #define M1_MOTOR_MAP AXIS_X // 1ma diff --git a/src/controller.c b/src/controller.c index c45a32b..b8628ad 100644 --- a/src/controller.c +++ b/src/controller.c @@ -50,19 +50,56 @@ controller_t cs; // controller state structure -static stat_t _shutdown_idler(); -static stat_t _limit_switch_handler(); -static stat_t _sync_to_planner(); -static stat_t _sync_to_tx_buffer(); +/// Blink rapidly and prevent further activity from occurring +/// Shutdown idler flashes indicator LED rapidly to show everything is not OK. +/// Shutdown idler returns EAGAIN causing the control loop to never advance beyond +/// this point. It's important that the reset handler is still called so a SW reset +/// (ctrl-x) or bootloader request can be processed. +static stat_t _shutdown_idler() { + if (cm_get_machine_state() != MACHINE_SHUTDOWN) return STAT_OK; + + if (rtc_get_time() > cs.led_timer) { + cs.led_timer = rtc_get_time() + LED_ALARM_TIMER; + indicator_led_toggle(); + } + + return STAT_EAGAIN; // EAGAIN prevents any lower-priority actions from running +} + + +/// Return eagain if TX queue is backed up +static stat_t _sync_to_tx_buffer() { + if (usart_tx_full()) return STAT_EAGAIN; + + return STAT_OK; +} + + +/// Return eagain if planner is not ready for a new command +static stat_t _sync_to_planner() { + // allow up to N planner buffers for this line + if (mp_get_planner_buffers_available() < PLANNER_BUFFER_HEADROOM) + return STAT_EAGAIN; + + return STAT_OK; +} + + +/// Shut down system if limit switch fired +static stat_t _limit_switch_handler() { + if (cm_get_machine_state() == MACHINE_ALARM) return STAT_NOOP; + if (!get_limit_switch_thrown()) return STAT_NOOP; + + return cm_hard_alarm(STAT_LIMIT_SWITCH_HIT); +} void controller_init() { - memset(&cs, 0, sizeof(controller_t)); // clear all values, job_id's, pointers and status + memset(&cs, 0, sizeof(controller_t)); // clear all values, job_id's, pointers and status } -/* - * Main loop - top-level controller +/* Main loop - top-level controller * * The order of the dispatched tasks is very important. * Tasks are ordered by increasing dependency (blocking hierarchy). @@ -110,45 +147,3 @@ void controller_run() { -/// Blink rapidly and prevent further activity from occurring -/// Shutdown idler flashes indicator LED rapidly to show everything is not OK. -/// Shutdown idler returns EAGAIN causing the control loop to never advance beyond -/// this point. It's important that the reset handler is still called so a SW reset -/// (ctrl-x) or bootloader request can be processed. -static stat_t _shutdown_idler() { - if (cm_get_machine_state() != MACHINE_SHUTDOWN) return STAT_OK; - - if (rtc_get_time() > cs.led_timer) { - cs.led_timer = rtc_get_time() + LED_ALARM_TIMER; - indicator_led_toggle(); - } - - return STAT_EAGAIN; // EAGAIN prevents any lower-priority actions from running -} - - -/// Return eagain if TX queue is backed up -static stat_t _sync_to_tx_buffer() { - if (usart_tx_full()) return STAT_EAGAIN; - - return STAT_OK; -} - - -/// Return eagain if planner is not ready for a new command -static stat_t _sync_to_planner() { - // allow up to N planner buffers for this line - if (mp_get_planner_buffers_available() < PLANNER_BUFFER_HEADROOM) - return STAT_EAGAIN; - - return STAT_OK; -} - - -/// _limit_switch_handler() - shut down system if limit switch fired -static stat_t _limit_switch_handler() { - if (cm_get_machine_state() == MACHINE_ALARM) return STAT_NOOP; - if (!get_limit_switch_thrown()) return STAT_NOOP; - - return cm_hard_alarm(STAT_LIMIT_SWITCH_HIT); -} diff --git a/src/cycle_jogging.c b/src/cycle_jogging.c index f67312b..18ae8f6 100644 --- a/src/cycle_jogging.c +++ b/src/cycle_jogging.c @@ -3,25 +3,31 @@ * * by Mike Estee - Other Machine Company * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #include "canonical_machine.h" @@ -33,61 +39,53 @@ #include #include -/**** Jogging singleton structure ****/ -struct jmJoggingSingleton { // persistent jogging runtime variables +struct jmJoggingSingleton { // persistent jogging runtime variables // controls for jogging cycle - int8_t axis; // axis currently being jogged - float dest_pos; // distance relative to start position to travel + int8_t axis; // axis currently being jogged + float dest_pos; // travel relative to start position float start_pos; float velocity_start; // initial jog feed float velocity_max; - uint8_t (*func)(int8_t axis); // binding for callback function state machine + uint8_t (*func)(int8_t axis); // binding for callback function // state saved from gcode model - float saved_feed_rate; // F setting - uint8_t saved_units_mode; // G20,G21 global setting - uint8_t saved_coord_system; // G54 - G59 setting - uint8_t saved_distance_mode; // G90,G91 global setting - uint8_t saved_feed_rate_mode; // G93,G94 global setting + float saved_feed_rate; // F setting + uint8_t saved_units_mode; // G20, G21 global setting + uint8_t saved_coord_system; // G54 - G59 setting + uint8_t saved_distance_mode; // G90, G91 global setting + uint8_t saved_feed_rate_mode; // G93, G94 global setting float saved_jerk; // saved and restored for each axis homed }; -static struct jmJoggingSingleton jog; +static struct jmJoggingSingleton jog; -/**** NOTE: global prototypes and other .h info is located in canonical_machine.h ****/ +// NOTE: global prototypes and other .h info is located in canonical_machine.h static stat_t _set_jogging_func(uint8_t (*func)(int8_t axis)); static stat_t _jogging_axis_start(int8_t axis); static stat_t _jogging_axis_jog(int8_t axis); static stat_t _jogging_finalize_exit(int8_t axis); + /***************************************************************************** - * cm_jogging_cycle_start() - jogging cycle using soft limits + * jogging cycle using soft limits * - */ -/* --- Some further details --- + * --- Some further details --- * * Note: When coding a cycle (like this one) you get to perform one queued * move per entry into the continuation, then you must exit. * - * Another Note: When coding a cycle (like this one) you must wait until + * Another Note: When coding a cycle you must wait until * the last move has actually been queued (or has finished) before declaring * the cycle to be done. Otherwise there is a nasty race condition in the * tg_controller() that will accept the next command before the position of * the final move has been recorded in the Gcode model. That's what the call * to cm_isbusy() is about. */ - -static stat_t _set_jogging_func(uint8_t (*func)(int8_t axis)); -static stat_t _jogging_axis_start(int8_t axis); -static stat_t _jogging_axis_jog(int8_t axis); -static stat_t _jogging_finalize_exit(int8_t axis); - -stat_t cm_jogging_cycle_start(uint8_t axis) -{ - // save relevant non-axis parameters from Gcode model +stat_t cm_jogging_cycle_start(uint8_t axis) { + // Save relevant non-axis parameters from Gcode model jog.saved_units_mode = cm_get_units_mode(ACTIVE_MODEL); jog.saved_coord_system = cm_get_coord_system(ACTIVE_MODEL); jog.saved_distance_mode = cm_get_distance_mode(ACTIVE_MODEL); @@ -95,96 +93,98 @@ stat_t cm_jogging_cycle_start(uint8_t axis) jog.saved_feed_rate = cm_get_feed_rate(ACTIVE_MODEL); jog.saved_jerk = cm_get_axis_jerk(axis); - // set working values + // Set working values cm_set_units_mode(MILLIMETERS); cm_set_distance_mode(ABSOLUTE_MODE); - cm_set_coord_system(ABSOLUTE_COORDS); // jogging is done in machine coordinates + cm_set_coord_system(ABSOLUTE_COORDS); // jog in machine coordinates cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE); - jog.velocity_start = JOGGING_START_VELOCITY; // see canonical_machine.h for #define + jog.velocity_start = JOGGING_START_VELOCITY; // see canonical_machine.h jog.velocity_max = cm.a[axis].velocity_max; jog.start_pos = cm_get_absolute_position(RUNTIME, axis); jog.dest_pos = cm_get_jogging_dest(); jog.axis = axis; - jog.func = _jogging_axis_start; // bind initial processing function + jog.func = _jogging_axis_start; // initial processing function cm.cycle_state = CYCLE_JOG; + return STAT_OK; } -/* Jogging axis moves - these execute in sequence for each axis - * cm_jogging_callback() - main loop callback for running the jogging cycle - * _set_jogging_func() - a convenience for setting the next dispatch vector and exiting - * _jogging_axis_start() - setup and start - * _jogging_axis_jog() - ramp the jog - * _jogging_axis_move() - move - * _jogging_finalize_exit() - back off the cleared limit switch - */ +// Jogging axis moves execute in sequence for each axis + +/// main loop callback for running the jogging cycle +stat_t cm_jogging_callback() { + if (cm.cycle_state != CYCLE_JOG) return STAT_NOOP; // not in a jogging cycle + if (cm_get_runtime_busy()) return STAT_EAGAIN; // sync to planner -stat_t cm_jogging_callback() -{ - if (cm.cycle_state != CYCLE_JOG) { return STAT_NOOP; } // exit if not in a jogging cycle - if (cm_get_runtime_busy() == true) { return STAT_EAGAIN; } // sync to planner move ends - return jog.func(jog.axis); // execute the current homing move + return jog.func(jog.axis); // execute current move } -static stat_t _set_jogging_func(stat_t (*func)(int8_t axis)) -{ + +/// a convenience for setting the next dispatch vector and exiting +static stat_t _set_jogging_func(stat_t (*func)(int8_t axis)) { jog.func = func; return STAT_EAGAIN; } -static stat_t _jogging_axis_start(int8_t axis) -{ - return _set_jogging_func(_jogging_axis_jog); // register the callback for the jog move + +/// setup and start +static stat_t _jogging_axis_start(int8_t axis) { + return _set_jogging_func(_jogging_axis_jog); // register jog move callback } -static stat_t _jogging_axis_jog(int8_t axis) // run the jog move -{ - float vect[] = {0,0,0,0,0,0}; + +/// ramp the jog +static stat_t _jogging_axis_jog(int8_t axis) { // run jog move + float vect[] = {0, 0, 0, 0, 0, 0}; float flags[] = {false, false, false, false, false, false}; flags[axis] = true; float velocity = jog.velocity_start; - float direction = jog.start_pos <= jog.dest_pos ? 1. : -1.; + float direction = jog.start_pos <= jog.dest_pos ? 1 : -1; float delta = abs(jog.dest_pos - jog.start_pos); cm.gm.feed_rate = velocity; - mp_flush_planner(); // don't use cm_request_queue_flush() here + mp_flush_planner(); // don't use cm_request_queue_flush() here cm_request_cycle_start(); float ramp_dist = 2.0; float steps = 0.0; float max_steps = 25; float offset = 0.01; - while( delta>ramp_dist && offset < delta && steps < max_steps ) - { - vect[axis] = jog.start_pos + offset * direction; - cm.gm.feed_rate = velocity; - ritorno(cm_straight_feed(vect, flags)); - - steps++; - float scale = pow(10.0, steps/max_steps) / 10.0; - velocity = jog.velocity_start + (jog.velocity_max - jog.velocity_start) * scale; - offset += ramp_dist * steps/max_steps; - } + + while (delta > ramp_dist && offset < delta && steps < max_steps) { + vect[axis] = jog.start_pos + offset * direction; + cm.gm.feed_rate = velocity; + ritorno(cm_straight_feed(vect, flags)); + + steps++; + float scale = pow(10.0, steps / max_steps) / 10.0; + velocity = + jog.velocity_start + (jog.velocity_max - jog.velocity_start) * scale; + offset += ramp_dist * steps / max_steps; + } // final move cm.gm.feed_rate = jog.velocity_max; vect[axis] = jog.dest_pos; ritorno(cm_straight_feed(vect, flags)); + return _set_jogging_func(_jogging_finalize_exit); } -static stat_t _jogging_finalize_exit(int8_t axis) // finish a jog -{ - mp_flush_planner(); // FIXME: not sure what to do on exit +/// back off the cleared limit switch +static stat_t _jogging_finalize_exit(int8_t axis) { + mp_flush_planner(); // FIXME: not sure what to do + + // Restore saved settings cm_set_axis_jerk(axis, jog.saved_jerk); - cm_set_coord_system(jog.saved_coord_system); // restore to work coordinate system + cm_set_coord_system(jog.saved_coord_system); cm_set_units_mode(jog.saved_units_mode); cm_set_distance_mode(jog.saved_distance_mode); cm_set_feed_rate_mode(jog.saved_feed_rate_mode); @@ -193,6 +193,5 @@ static stat_t _jogging_finalize_exit(int8_t axis) // finish a jog cm_cycle_end(); cm.cycle_state = CYCLE_OFF; - printf("{\"jog\":0}\n"); return STAT_OK; } diff --git a/src/plan/arc.c b/src/plan/arc.c index 9ce7925..5e893b2 100644 --- a/src/plan/arc.c +++ b/src/plan/arc.c @@ -60,7 +60,7 @@ stat_t cm_arc_feed(float target[], float flags[], // arc endpoints // set radius mode flag and do simple test(s) bool radius_f = fp_NOT_ZERO(cm.gf.arc_radius); // set true if radius arc - if (radius_f && (cm.gn.arc_radius < MIN_ARC_RADIUS)) // radius value must be + and > minimum radius + if (radius_f && (cm.gn.arc_radius < MIN_ARC_RADIUS)) // radius value must be + and > minimum radius return STAT_ARC_RADIUS_OUT_OF_TOLERANCE; // setup some flags @@ -156,7 +156,6 @@ stat_t cm_arc_feed(float target[], float flags[], // arc endpoints */ stat_t cm_arc_callback() { if (arc.run_state == MOVE_OFF) return STAT_NOOP; - if (mp_get_planner_buffers_available() < PLANNER_BUFFER_HEADROOM) return STAT_EAGAIN; arc.theta += arc.arc_segment_theta; diff --git a/src/plan/arc.h b/src/plan/arc.h index 73d6997..2815dcd 100644 --- a/src/plan/arc.h +++ b/src/plan/arc.h @@ -62,8 +62,6 @@ typedef struct arArcSingleton { // persistent planner and runtime variables extern arc_t arc; -/* arc function prototypes */ // NOTE: See canonical_machine.h for cm_arc_feed() prototype - void cm_arc_init(); stat_t cm_arc_callback(); void cm_abort_arc(); diff --git a/src/plan/exec.c b/src/plan/exec.c index 888eec5..4a116cb 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -5,25 +5,31 @@ * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. * Copyright (c) 2012 - 2015 Rob Giseburt * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . + * + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. + * + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #include "planner.h" @@ -63,29 +69,28 @@ stat_t mp_exec_move() { if (bf->move_type == MOVE_TYPE_ALINE && cm.motion_state == MOTION_STOP) cm_set_motion_state(MOTION_RUN); - if (bf->bf_func == 0) + if (!bf->bf_func) return cm_hard_alarm(STAT_INTERNAL_ERROR); // never supposed to get here return bf->bf_func(bf); // run the move callback in the planner buffer } -/*************************************************************************/ -/**** ALINE EXECUTION ROUTINES *******************************************/ -/************************************************************************* +/* Aline execution routines + * * ---> Everything here fires from interrupts and must be interrupt safe * - * _exec_aline() - acceleration line main routine - * _exec_aline_head() - helper for acceleration section - * _exec_aline_body() - helper for cruise section - * _exec_aline_tail() - helper for deceleration section + * _exec_aline() - acceleration line main routine + * _exec_aline_head() - helper for acceleration section + * _exec_aline_body() - helper for cruise section + * _exec_aline_tail() - helper for deceleration section * _exec_aline_segment() - helper for running a segment * * Returns: * STAT_OK move is done * STAT_EAGAIN move is not finished - has more segments to run - * STAT_NOOP cause no operation from the steppers - do not load the move - * STAT_xxxxx fatal error. Ends the move and frees the bf buffer + * STAT_NOOP cause no operation from the steppers - do not load the move + * STAT_xxxxx fatal error. Ends the move and frees the bf buffer * * This routine is called from the (LO) interrupt level. The interrupt * sequencing relies on the behaviors of the routines being exactly correct. @@ -101,10 +106,10 @@ stat_t mp_exec_move() { * position error will be compensated by the next move. * * Note 2 Solves a potential race condition where the current move ends but the - * new move has not started because the previous move is still being run + * new move has not started because the previous move is still being run * by the steppers. Planning can overwrite the new move. */ -/* OPERATION: +/* Operation: * Aline generates jerk-controlled S-curves as per Ed Red's course notes: * http://www.et.byu.edu/~ered/ME537/Notes/Ch5.pdf * http://www.scribd.com/doc/63521608/Ed-Red-Ch5-537-Jerk-Equations @@ -196,23 +201,23 @@ stat_t mp_exec_aline(mpBuf_t *bf) { mr.position[axis] + mr.unit[axis] * (mr.head_length + mr.body_length + mr.tail_length); } } - // NB: from this point on the contents of the bf buffer do not affect execution - //**** main dispatcher to process segments *** + // main dispatcher to process segments + // from this point on the contents of the bf buffer do not affect execution stat_t status = STAT_OK; if (mr.section == SECTION_HEAD) status = _exec_aline_head(); else if (mr.section == SECTION_BODY) status = _exec_aline_body(); else if (mr.section == SECTION_TAIL) status = _exec_aline_tail(); else if (mr.move_state == MOVE_SKIP_BLOCK) status = STAT_OK; - else return cm_hard_alarm(STAT_INTERNAL_ERROR); // never supposed to get here + else return cm_hard_alarm(STAT_INTERNAL_ERROR); // never supposed to get here // Feedhold processing. Refer to canonical_machine.h for state machine // Catch the feedhold request and start the planning the hold - if (cm.hold_state == FEEDHOLD_SYNC) { cm.hold_state = FEEDHOLD_PLAN;} + if (cm.hold_state == FEEDHOLD_SYNC) cm.hold_state = FEEDHOLD_PLAN; // Look for the end of the decel to go into HOLD state - if ((cm.hold_state == FEEDHOLD_DECEL) && (status == STAT_OK)) { + if (cm.hold_state == FEEDHOLD_DECEL && status == STAT_OK) { cm.hold_state = FEEDHOLD_HOLD; cm_set_motion_state(MOTION_HOLD); report_request(); @@ -226,7 +231,7 @@ stat_t mp_exec_aline(mpBuf_t *bf) { // STAT_OK MOVE_NEW mr done; bf must be run again (it's been reused) if (status == STAT_EAGAIN) report_request(); else { - mr.move_state = MOVE_OFF; // reset mr buffer + mr.move_state = MOVE_OFF; // reset mr buffer mr.section_state = SECTION_OFF; bf->nx->replannable = false; // prevent overplanning (Note 2) @@ -434,6 +439,7 @@ static stat_t _exec_aline_head() { } #else // __ JERK_EXEC + static stat_t _exec_aline_head() { if (mr.section_state == SECTION_NEW) { // initialize the move singleton (mr) if (fp_ZERO(mr.head_length)) { @@ -443,7 +449,7 @@ static stat_t _exec_aline_head() { // Time for entire accel region mr.gm.move_time = 2 * mr.head_length / (mr.entry_velocity + mr.cruise_velocity); - mr.segments = ceil(uSec(mr.gm.move_time) / NOM_SEGMENT_USEC);// # of segments for the section + mr.segments = ceil(uSec(mr.gm.move_time) / NOM_SEGMENT_USEC); // # of segments for the section mr.segment_time = mr.gm.move_time / mr.segments; _init_forward_diffs(mr.entry_velocity, mr.cruise_velocity); mr.segment_count = (uint32_t)mr.segments; @@ -664,7 +670,7 @@ static stat_t _exec_aline_tail() { } } - return STAT_EAGAIN; // should never get here + return STAT_EAGAIN; } #endif // __JERK_EXEC @@ -696,12 +702,11 @@ static stat_t _exec_aline_segment() { // If the segment ends on a section waypoint synchronize to the head, body or tail end // Otherwise if not at a section waypoint compute target from segment time and velocity // Don't do waypoint correction if you are going into a hold. - - if ((--mr.segment_count == 0) && (mr.section_state == SECTION_2nd_HALF) && - (cm.motion_state == MOTION_RUN) && (cm.cycle_state == CYCLE_MACHINING)) { + if (--mr.segment_count == 0 && mr.section_state == SECTION_2nd_HALF && + cm.motion_state == MOTION_RUN && cm.cycle_state == CYCLE_MACHINING) copy_vector(mr.gm.target, mr.waypoint[mr.section]); - } else { + else { float segment_length = mr.segment_velocity * mr.segment_time; for (i = 0; i < AXES; i++) @@ -711,8 +716,8 @@ static stat_t _exec_aline_segment() { // Convert target position to steps // Bucket-brigade the old target down the chain before getting the new target from kinematics // - // NB: The direct manipulation of steps to compute travel_steps only works for Cartesian kinematics. - // Other kinematics may require transforming travel distance as opposed to simply subtracting steps. + // The direct manipulation of steps to compute travel_steps only works for Cartesian kinematics. + // Other kinematics may require transforming travel distance as opposed to simply subtracting steps. for (i = 0; i < MOTORS; i++) { mr.commanded_steps[i] = mr.position_steps[i]; // previous segment's position, delayed by 1 segment @@ -730,9 +735,9 @@ static stat_t _exec_aline_segment() { ritorno(st_prep_line(travel_steps, mr.following_error, mr.segment_time)); copy_vector(mr.position, mr.gm.target); // update position from target #ifdef __JERK_EXEC - mr.elapsed_accel_time += mr.segment_accel_time; // needed by jerk-based exec (NB: ignored if running body) + mr.elapsed_accel_time += mr.segment_accel_time; // needed by jerk-based exec (ignored if running body) #endif - if (mr.segment_count == 0) return STAT_OK; // this section has run all its segments + if (!mr.segment_count) return STAT_OK; // this section has run all its segments return STAT_EAGAIN; // this section still has more segments to run } diff --git a/src/plan/kinematics.c b/src/plan/kinematics.c index d6b5a96..87d0f22 100644 --- a/src/plan/kinematics.c +++ b/src/plan/kinematics.c @@ -50,8 +50,9 @@ void ik_kinematics(const float travel[], float steps[]) { memcpy(joint, travel, sizeof(float) * AXES); //...or just do a memcpy for Cartesian machines // Map motors to axes and convert length units to steps - // Most of the conversion math has already been done in during config in steps_per_unit() - // which takes axis travel, step angle and microsteps into account. + // Most of the conversion math has already been done during config in + // steps_per_unit() which takes axis travel, step angle and microsteps into + // account. for (uint8_t axis = 0; axis < AXES; axis++) { if (cm.a[axis].axis_mode == AXIS_INHIBITED) joint[axis] = 0; diff --git a/src/plan/line.c b/src/plan/line.c index 6a92cfc..c083b69 100644 --- a/src/plan/line.c +++ b/src/plan/line.c @@ -5,25 +5,31 @@ * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. * Copyright (c) 2012 - 2015 Rob Giseburt * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . + * + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. + * + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #include "planner.h" @@ -75,20 +81,23 @@ uint8_t mp_get_runtime_busy() { } -/**************************************************************************************** - * Plan a line with acceleration / deceleration +/* Plan a line with acceleration / deceleration * - * This function uses constant jerk motion equations to plan acceleration and deceleration - * The jerk is the rate of change of acceleration; it's the 1st derivative of acceleration, - * and the 3rd derivative of position. Jerk is a measure of impact to the machine. - * Controlling jerk smooths transitions between moves and allows for faster feeds while - * controlling machine oscillations and other undesirable side-effects. + * This function uses constant jerk motion equations to plan + * acceleration and deceleration The jerk is the rate of change of + * acceleration; it's the 1st derivative of acceleration, and the + * 3rd derivative of position. Jerk is a measure of impact to the + * machine. Controlling jerk smooths transitions between moves and + * allows for faster feeds while controlling machine oscillations + * and other undesirable side-effects. * - * Note All math is done in absolute coordinates using single precision floating point (float). + * Note All math is done in absolute coordinates using single + * precision floating point (float). * - * Note: Returning a status that is not STAT_OK means the endpoint is NOT advanced. So lines - * that are too short to move will accumulate and get executed once the accumulated error - * exceeds the minimums. + * Note: Returning a status that is not STAT_OK means the endpoint + * is NOT advanced. So lines that are too short to move will + * accumulate and get executed once the accumulated error exceeds + * the minimums. */ stat_t mp_aline(GCodeState_t *gm_in) { mpBuf_t *bf; // current move pointer @@ -118,9 +127,11 @@ stat_t mp_aline(GCodeState_t *gm_in) { // The time of the move is determined by its initial velocity (Vi) and how much // acceleration will be occur. For this we need to look at the exit velocity of // the previous block. There are 3 possible cases: + // // (1) There is no previous block. Vi = 0 // (2) Previous block is optimally planned. Vi = previous block's exit_velocity - // (3) Previous block is not optimally planned. Vi <= previous block's entry_velocity + delta_velocity + // (3) Previous block is not optimally planned. Vi <= previous block's + // entry_velocity + delta_velocity _calc_move_times(gm_in, axis_length, axis_square); // set move & minimum time in state @@ -128,29 +139,30 @@ stat_t mp_aline(GCodeState_t *gm_in) { float delta_velocity = pow(length, 0.66666666) * mm.cbrt_jerk; // max velocity change for this move float entry_velocity = 0; // pre-set as if no previous block - if ((bf = mp_get_run_buffer()) != 0) { + if ((bf = mp_get_run_buffer())) { if (bf->replannable) // not optimally planned entry_velocity = bf->entry_velocity + bf->delta_vmax; else entry_velocity = bf->exit_velocity; // optimally planned } // compute execution time for this move - float move_time = (2 * length) / (2 * entry_velocity + delta_velocity); + float move_time = 2 * length / (2 * entry_velocity + delta_velocity); if (move_time < MIN_BLOCK_TIME) return STAT_MINIMUM_TIME_MOVE; } // get a cleared buffer and setup move variables - if ((bf = mp_get_write_buffer()) == 0) + if (!(bf = mp_get_write_buffer())) return cm_hard_alarm(STAT_BUFFER_FULL_FATAL); // never supposed to fail bf->bf_func = mp_exec_aline; // register callback to exec function bf->length = length; memcpy(&bf->gm, gm_in, sizeof(GCodeState_t)); // copy model state into planner buffer - // Compute the unit vector and find the right jerk to use (combined operations) - // To determine the jerk value to use for the block we want to find the axis for which - // the jerk cannot be exceeded - the 'jerk-limit' axis. This is the axis for which - // the time to decelerate from the target velocity to zero would be the longest. + // Compute the unit vector and find the right jerk to use (combined + // operations) To determine the jerk value to use for the block we + // want to find the axis for which the jerk cannot be exceeded - the + // 'jerk-limit' axis. This is the axis for which the time to + // decelerate from the target velocity to zero would be the longest. // // We can determine the "longest" deceleration in terms of time or distance. // @@ -170,16 +182,19 @@ stat_t mp_aline(GCodeState_t *gm_in) { // Since E is always zero in this calculation, we can simplify: // l = S*sqrt(S/J) // - // The final value we want is to know which one is *longest*, compared to the others, - // so we don't need the actual value. This means that the scale of the value doesn't - // matter, so for T we can remove the "2 *" and For L we can remove the "S*". - // This leaves both to be simply "sqrt(S/J)". Since we don't need the scale, - // it doesn't matter what speed we're coming from, so S can be replaced with 1. + // The final value we want is to know which one is *longest*, + // compared to the others, so we don't need the actual value. This + // means that the scale of the value doesn't matter, so for T we + // can remove the "2 *" and For L we can remove the "S*". This + // leaves both to be simply "sqrt(S/J)". Since we don't need the + // scale, it doesn't matter what speed we're coming from, so S can + // be replaced with 1. // - // However, we *do* need to compensate for the fact that each axis contributes - // differently to the move, so we will scale each contribution C[n] by the - // proportion of the axis movement length D[n] to the total length of the move L. - // Using that, we construct the following, with these definitions: + // However, we *do* need to compensate for the fact that each axis + // contributes differently to the move, so we will scale each + // contribution C[n] by the proportion of the axis movement length + // D[n] to the total length of the move L. 0Using that, we + // construct the following, with these definitions: // // J[n] = max_jerk for axis n // D[n] = distance traveled for this move for axis n @@ -188,25 +203,29 @@ stat_t mp_aline(GCodeState_t *gm_in) { // // For each axis n: C[n] = sqrt(1/J[n]) * (D[n]/L) // - // Keeping in mind that we only need a rank compared to the other axes, we can further - // optimize the calculations:: + // Keeping in mind that we only need a rank compared to the other + // axes, we can further optimize the calculations:: // // Square the expression to remove the square root: - // C[n]^2 = (1/J[n]) * (D[n]/L)^2 (We don't care the C is squared, we'll use it that way.) + // C[n]^2 = (1/J[n]) * (D[n]/L)^2 (We don't care the C is squared, + // we'll use it that way.) // - // Re-arranged to optimize divides for pre-calculated values, we create a value - // M that we compute once: + // Re-arranged to optimize divides for pre-calculated values, + // we create a value M that we compute once: // M = (1/(L^2)) - // Then we use it in the calculations for every axis: + // Then we use it in the calculations for every axis: // C[n] = (1/J[n]) * D[n]^2 * M // - // Also note that we already have (1/J[n]) calculated for each axis, which simplifies it further. + // Also note that we already have (1/J[n]) calculated for each axis, + // which simplifies it further. // - // Finally, the selected jerk term needs to be scaled by the reciprocal of the absolute value - // of the jerk-limit axis's unit vector term. This way when the move is finally decomposed into - // its constituent axes for execution the jerk for that axis will be at it's maximum value. + // Finally, the selected jerk term needs to be scaled by the + // reciprocal of the absolute value of the jerk-limit axis's unit + // vector term. This way when the move is finally decomposed into + // its constituent axes for execution the jerk for that axis will be + // at it's maximum value. - float C; // contribution term. C = T * a + float C; // contribution term. C = T * a float maxC = 0; float recip_L2 = 1 / length_square; @@ -247,8 +266,8 @@ stat_t mp_aline(GCodeState_t *gm_in) { bf->braking_velocity = bf->delta_vmax; // Note: these next lines must remain in exact order. Position must update before committing the buffer. - _plan_block_list(bf, &mr_flag); // replan block list - copy_vector(mm.position, bf->gm.target); // set the planner position + _plan_block_list(bf, &mr_flag); // replan block list + copy_vector(mm.position, bf->gm.target); // set the planner position mp_commit_write_buffer(MOVE_TYPE_ALINE); // commit current block (must follow the position update) return STAT_OK; @@ -258,16 +277,20 @@ stat_t mp_aline(GCodeState_t *gm_in) { /* * Compute optimal and minimum move times into the gcode_state * - * "Minimum time" is the fastest the move can be performed given the velocity constraints on each - * participating axis - regardless of the feed rate requested. The minimum time is the time limited - * by the rate-limiting axis. The minimum time is needed to compute the optimal time and is - * recorded for possible feed override computation.. + * "Minimum time" is the fastest the move can be performed given + * the velocity constraints on each participating axis - regardless + * of the feed rate requested. The minimum time is the time limited + * by the rate-limiting axis. The minimum time is needed to compute + * the optimal time and is recorded for possible feed override + * computation.. * - * "Optimal time" is either the time resulting from the requested feed rate or the minimum time if - * the requested feed rate is not achievable. Optimal times for traverses are always the minimum time. + * "Optimal time" is either the time resulting from the requested + * feed rate or the minimum time if the requested feed rate is not + * achievable. Optimal times for traverses are always the minimum + * time. * - * The gcode state must have targets set prior by having cm_set_target(). Axis modes are taken into - * account by this. + * The gcode state must have targets set prior by having + * cm_set_target(). Axis modes are taken into account by this. * * The following times are compared and the longest is returned: * - G93 inverse time (if G93 is active) @@ -277,40 +300,47 @@ stat_t mp_aline(GCodeState_t *gm_in) { * Sets the following variables in the gcode_state struct * - move_time is set to optimal time * - minimum_time is set to minimum time - */ -/* --- NIST RS274NGC_v3 Guidance --- * - * The following is verbatim text from NIST RS274NGC_v3. As I interpret A for moves that - * combine both linear and rotational movement, the feed rate should apply to the XYZ - * movement, with the rotational axis (or axes) timed to start and end at the same time - * the linear move is performed. It is possible under this case for the rotational move - * to rate-limit the linear move. + * NIST RS274NGC_v3 Guidance + * + * The following is verbatim text from NIST RS274NGC_v3. As I + * interpret A for moves that combine both linear and rotational + * movement, the feed rate should apply to the XYZ movement, with + * the rotational axis (or axes) timed to start and end at the same + * time the linear move is performed. It is possible under this + * case for the rotational move to rate-limit the linear move. * * 2.1.2.5 Feed Rate * - * The rate at which the controlled point or the axes move is nominally a steady rate - * which may be set by the user. In the Interpreter, the interpretation of the feed - * rate is as follows unless inverse time feed rate mode is being used in the - * RS274/NGC view (see Section 3.5.19). The canonical machining functions view of feed - * rate, as described in Section 4.3.5.1, has conditions under which the set feed rate - * is applied differently, but none of these is used in the Interpreter. - * - * A. For motion involving one or more of the X, Y, and Z axes (with or without - * simultaneous rotational axis motion), the feed rate means length units per - * minute along the programmed XYZ path, as if the rotational axes were not moving. - * - * B. For motion of one rotational axis with X, Y, and Z axes not moving, the - * feed rate means degrees per minute rotation of the rotational axis. - * - * C. For motion of two or three rotational axes with X, Y, and Z axes not moving, - * the rate is applied as follows. Let dA, dB, and dC be the angles in degrees - * through which the A, B, and C axes, respectively, must move. - * Let D = sqrt(dA^2 + dB^2 + dC^2). Conceptually, D is a measure of total - * angular motion, using the usual Euclidean metric. Let T be the amount of - * time required to move through D degrees at the current feed rate in degrees - * per minute. The rotational axes should be moved in coordinated linear motion - * so that the elapsed time from the start to the end of the motion is T plus - * any time required for acceleration or deceleration. + * The rate at which the controlled point or the axes move is + * nominally a steady rate which may be set by the user. In the + * Interpreter, the interpretation of the feed rate is as follows + * unless inverse time feed rate mode is being used in the + * RS274/NGC view (see Section 3.5.19). The canonical machining + * functions view of feed rate, as described in Section 4.3.5.1, + * has conditions under which the set feed rate is applied + * differently, but none of these is used in the Interpreter. + * + * A. For motion involving one or more of the X, Y, and Z axes + * (with or without simultaneous rotational axis motion), the + * feed rate means length units per minute along the programmed + * XYZ path, as if the rotational axes were not moving. + * + * B. For motion of one rotational axis with X, Y, and Z axes not + * moving, the feed rate means degrees per minute rotation of + * the rotational axis. + * + * C. For motion of two or three rotational axes with X, Y, and Z + * axes not moving, the rate is applied as follows. Let dA, dB, + * and dC be the angles in degrees through which the A, B, and + * C axes, respectively, must move. Let D = sqrt(dA^2 + dB^2 + + * dC^2). Conceptually, D is a measure of total angular motion, + * using the usual Euclidean metric. Let T be the amount of + * time required to move through D degrees at the current feed + * rate in degrees per minute. The rotational axes should be + * moved in coordinated linear motion so that the elapsed time + * from the start to the end of the motion is T plus any time + * required for acceleration or deceleration. */ static void _calc_move_times(GCodeState_t *gms, const float axis_length[], const float axis_square[]) { // gms = Gcode model state @@ -357,71 +387,79 @@ static void _calc_move_times(GCodeState_t *gms, const float axis_length[], const /* Plans the entire block list * - * The block list is the circular buffer of planner buffers (bf's). The block currently - * being planned is the "bf" block. The "first block" is the next block to execute; - * queued immediately behind the currently executing block, aka the "running" block. - * In some cases there is no first block because the list is empty or there is only - * one block and it is already running. + * The block list is the circular buffer of planner buffers + * (bf's). The block currently being planned is the "bf" block. The + * "first block" is the next block to execute; queued immediately + * behind the currently executing block, aka the "running" block. + * In some cases there is no first block because the list is empty + * or there is only one block and it is already running. * - * If blocks following the first block are already optimally planned (non replannable) - * the first block that is not optimally planned becomes the effective first block. + * If blocks following the first block are already optimally + * planned (non replannable) the first block that is not optimally + * planned becomes the effective first block. * - * _plan_block_list() plans all blocks between and including the (effective) first block - * and the bf. It sets entry, exit and cruise v's from vmax's then calls trapezoid generation. + * _plan_block_list() plans all blocks between and including the + * (effective) first block and the bf. It sets entry, exit and + * cruise v's from vmax's then calls trapezoid generation. * - * Variables that must be provided in the mpBuffers that will be processed: + * Variables that must be provided in the mpBuffers that will be + * processed: * - * bf (function arg) - end of block list (last block in time) - * bf->replannable - start of block list set by last FALSE value [Note 1] - * bf->move_type - typically MOVE_TYPE_ALINE. Other move_types should be set to + * bf (function arg) - end of block list (last block in time) + * bf->replannable - start of block list set by last FALSE value [Note 1] + * bf->move_type - typically MOVE_TYPE_ALINE. Other move_types should be set to * length=0, entry_vmax=0 and exit_vmax=0 and are treated * as a momentary stop (plan to zero and from zero). * * bf->length - provides block length * bf->entry_vmax - used during forward planning to set entry velocity - * bf->cruise_vmax - used during forward planning to set cruise velocity - * bf->exit_vmax - used during forward planning to set exit velocity + * bf->cruise_vmax - used during forward planning to set cruise velocity + * bf->exit_vmax - used during forward planning to set exit velocity * bf->delta_vmax - used during forward planning to set exit velocity * * bf->recip_jerk - used during trapezoid generation - * bf->cbrt_jerk - used during trapezoid generation + * bf->cbrt_jerk - used during trapezoid generation * * Variables that will be set during processing: * - * bf->replannable - set if the block becomes optimally planned + * bf->replannable - set if the block becomes optimally planned * - * bf->braking_velocity - set during backward planning + * bf->braking_velocity - set during backward planning * bf->entry_velocity - set during forward planning - * bf->cruise_velocity - set during forward planning - * bf->exit_velocity - set during forward planning + * bf->cruise_velocity - set during forward planning + * bf->exit_velocity - set during forward planning * - * bf->head_length - set during trapezoid generation - * bf->body_length - set during trapezoid generation - * bf->tail_length - set during trapezoid generation + * bf->head_length - set during trapezoid generation + * bf->body_length - set during trapezoid generation + * bf->tail_length - set during trapezoid generation * * Variables that are ignored but here's what you would expect them to be: + * * bf->move_state - NEW for all blocks but the earliest - * bf->target[] - block target position + * bf->target[] - block target position * bf->unit[] - block unit vector - * bf->time - gets set later - * bf->jerk - source of the other jerk variables. Used in mr. + * bf->time - gets set later + * bf->jerk - source of the other jerk variables. Used in mr. * * Notes: - * [1] Whether or not a block is planned is controlled by the bf->replannable - * setting (set TRUE if it should be). Replan flags are checked during the - * backwards pass and prune the replan list to include only the 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 - * the prep routines modify the contents of the mr buffer and re-shuffle - * the block list, re-enlisting the current bf buffer with new parameters. - * These routines also set all blocks in the list to be replannable so the - * list can be recomputed regardless of exact stops and previous replanning - * optimizations. - * - * [2] The mr_flag is used to tell replan to account for mr buffer's exit velocity (Vx) - * mr's Vx is always found in the provided bf buffer. Used to replan feedholds + * + * [1] Whether or not a block is planned is controlled by the + * bf->replannable setting (set TRUE if it should be). Replan flags + * are checked during the backwards pass and prune the replan list + * to include only the 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 the prep routines modify the + * contents of the mr buffer and re-shuffle the block list, + * re-enlisting the current bf buffer with new parameters. + * These routines also set all blocks in the list to be + * replannable so the list can be recomputed regardless of + * exact stops and previous replanning optimizations. + * + * [2] The mr_flag is used to tell replan to account for mr + * buffer's exit velocity (Vx) mr's Vx is always found in the + * provided bf buffer. Used to replan feedholds */ static void _plan_block_list(mpBuf_t *bf, uint8_t *mr_flag) { mpBuf_t *bp = bf; @@ -435,17 +473,17 @@ static void _plan_block_list(mpBuf_t *bf, uint8_t *mr_flag) { // forward planning pass - recomputes trapezoids in the list from the first block to the bf block. while ((bp = mp_get_next_buffer(bp)) != bf) { - if ((bp->pv == bf) || (*mr_flag == true)) { - bp->entry_velocity = bp->entry_vmax; // first block in the list + if (bp->pv == bf || *mr_flag == true) { + bp->entry_velocity = bp->entry_vmax; // first block in the list *mr_flag = false; - } else bp->entry_velocity = bp->pv->exit_velocity; // other blocks in the list + } else bp->entry_velocity = bp->pv->exit_velocity; // other blocks in the list bp->cruise_velocity = bp->cruise_vmax; - bp->exit_velocity = min4( bp->exit_vmax, - bp->nx->entry_vmax, - bp->nx->braking_velocity, - (bp->entry_velocity + bp->delta_vmax) ); + bp->exit_velocity = min4(bp->exit_vmax, + bp->nx->entry_vmax, + bp->nx->braking_velocity, + bp->entry_velocity + bp->delta_vmax); mp_calculate_trapezoid(bp); @@ -476,58 +514,68 @@ static void _reset_replannable_list() { } -/* - * Sonny's algorithm - simple - * - * Computes the maximum allowable junction speed by finding the velocity that will yield - * the centripetal acceleration in the corner_acceleration value. The value of delta sets - * the effective radius of curvature. Here's Sonny's (Sungeun K. Jeon's) explanation - * of what's going on: - * - * "First let's assume that at a junction we only look a centripetal acceleration to simply - * things. At a junction of two lines, let's place a circle such that both lines are tangent - * to the circle. The circular segment joining the lines represents the path for constant - * centripetal acceleration. This creates a deviation from the path (let's call this delta), - * which is the distance from the junction to the edge of the circular segment. Delta needs - * to be defined, so let's replace the term max_jerk (see note 1) with max_junction_deviation, - * or "delta". This indirectly sets the radius of the circle, and hence limits the velocity - * by the centripetal acceleration. Think of the this as widening the race track. If a race - * car is driving on a track only as wide as a car, it'll have to slow down a lot to turn - * corners. If we widen the track a bit, the car can start to use the track to go into the - * turn. The wider it is, the faster through the corner it can go. - * - * (Note 1: "max_jerk" refers to the old grbl/marlin max_jerk" approximation term, not the - * tinyG jerk terms) +/* Sonny's algorithm - simple + * + * Computes the maximum allowable junction speed by finding the + * velocity that will yield the centripetal acceleration in the + * corner_acceleration value. The value of delta sets the effective + * radius of curvature. Here's Sonny's (Sungeun K. Jeon's) + * explanation of what's going on: + * + * "First let's assume that at a junction we only look a centripetal + * acceleration to simply things. At a junction of two lines, let's + * place a circle such that both lines are tangent to the circle. The + * circular segment joining the lines represents the path for + * constant centripetal acceleration. This creates a deviation from + * the path (let's call this delta), which is the distance from the + * junction to the edge of the circular segment. Delta needs to be + * defined, so let's replace the term max_jerk (see note 1) with + * max_junction_deviation, or "delta". This indirectly sets the + * radius of the circle, and hence limits the velocity by the + * centripetal acceleration. Think of the this as widening the race + * track. If a race car is driving on a track only as wide as a car, + * it'll have to slow down a lot to turn corners. If we widen the + * track a bit, the car can start to use the track to go into the + * turn. The wider it is, the faster through the corner it can go. + * + * (Note 1: "max_jerk" refers to the old grbl/marlin max_jerk" + * approximation term, not the tinyG jerk terms) * * If you do the geometry in terms of the known variables, you get: - * sin(theta/2) = R/(R+delta) Re-arranging in terms of circle radius (R) + * sin(theta/2) = R/(R+delta) + * Re-arranging in terms of circle radius (R) * R = delta*sin(theta/2)/(1-sin(theta/2). * * Theta is the angle between line segments given by: * cos(theta) = dot(a,b)/(norm(a)*norm(b)). * - * Most of these calculations are already done in the planner. To remove the acos() - * and sin() computations, use the trig half angle identity: + * Most of these calculations are already done in the planner. + * To remove the acos() and sin() computations, use the trig half + * angle identity: * sin(theta/2) = +/- sqrt((1-cos(theta))/2). * - * For our applications, this should always be positive. Now just plug the equations into - * the centripetal acceleration equation: v_c = sqrt(a_max*R). You'll see that there are - * only two sqrt computations and no sine/cosines." + * For our applications, this should always be positive. Now just + * plug the equations into the centripetal acceleration equation: + * v_c = sqrt(a_max*R). You'll see that there are only two sqrt + * computations and no sine/cosines." * * How to compute the radius using brute-force trig: * float theta = acos(costheta); * float radius = delta * sin(theta/2)/(1-sin(theta/2)); * - * This version extends Chamnit's algorithm by computing a value for delta that takes - * the contributions of the individual axes in the move into account. This allows the - * control radius to vary by axis. This is necessary to support axes that have - * different dynamics; such as a Z axis that doesn't move as fast as X and Y (such as a - * screw driven Z axis on machine with a belt driven XY - like a Shapeoko), or rotary - * axes ABC that have completely different dynamics than their linear counterparts. + * This version extends Chamnit's algorithm by computing a value for + * delta that takes the contributions of the individual axes in the + * move into account. This allows the control radius to vary by + * axis. This is necessary to support axes that have different + * dynamics; such as a Z axis that doesn't move as fast as X and Y + * (such as a screw driven Z axis on machine with a belt driven XY - + * like a Shapeoko), or rotary axes ABC that have completely + * different dynamics than their linear counterparts. * - * The function takes the absolute values of the sum of the unit vector components as - * a measure of contribution to the move, then scales the delta values from the non-zero - * axes into a composite delta to be used for the move. Shown for an XY vector: + * The function takes the absolute values of the sum of the unit + * vector components as a measure of contribution to the move, then + * scales the delta values from the non-zero axes into a composite + * delta to be used for the move. Shown for an XY vector: * * U[i] Unit sum of i'th axis fabs(unit_a[i]) + fabs(unit_b[i]) * Usum Length of sums Ux + Uy @@ -569,11 +617,7 @@ static float _get_junction_vmax(const float a_unit[], const float b_unit[]) { } -/************************************************************************* - * feedholds - functions for performing holds - * - * mp_plan_hold_callback() - replan block list to execute hold - * mp_end_hold() - release the hold and restart block list +/* feedholds - functions for performing holds * * Feedhold is executed as cm.hold_state transitions executed inside * _exec_aline() and main loop callbacks to these functions: @@ -581,48 +625,59 @@ static float _get_junction_vmax(const float a_unit[], const float b_unit[]) { * * Holds work like this: * - * - Hold is asserted by calling cm_feedhold() (usually invoked via a ! char) - * If hold_state is OFF and motion_state is RUNning it sets - * hold_state to SYNC and motion_state to HOLD. - * - * - Hold state == SYNC tells the aline exec routine to execute the next aline - * segment then set hold_state to PLAN. This gives the planner sufficient - * time to replan the block list for the hold before the next aline segment - * needs to be processed. - * - * - Hold state == PLAN tells the planner to replan the mr buffer, the current - * run buffer (bf), and any subsequent bf buffers as necessary to execute a - * hold. Hold planning replans the planner buffer queue down to zero and then - * back up from zero. Hold state is set to DECEL when planning is complete. - * - * - Hold state == DECEL persists until the aline execution runs to zero - * velocity, at which point hold state transitions to HOLD. - * - * - Hold state == HOLD persists until the cycle is restarted. A cycle start - * is an asynchronous event that sets the cycle_start_flag TRUE. It can - * occur any time after the hold is requested - either before or after - * motion stops. - * - * - mp_end_hold() is executed from cm_feedhold_sequencing_callback() once the - * hold state == HOLD and a cycle_start has been requested.This sets the hold - * state to OFF which enables _exec_aline() to continue processing. Move - * execution begins with the first buffer after the hold. + * - Hold is asserted by calling cm_feedhold() (usually invoked + * via a ! char) If hold_state is OFF and motion_state is + * RUNning it sets hold_state to SYNC and motion_state to HOLD. + * + * - Hold state == SYNC tells the aline exec routine to execute + * the next aline segment then set hold_state to PLAN. This + * gives the planner sufficient time to replan the block list + * for the hold before the next aline segment needs to be + * processed. + * + * - Hold state == PLAN tells the planner to replan the mr + * buffer, the current run buffer (bf), and any subsequent bf + * buffers as necessary to execute a hold. Hold planning + * replans the planner buffer queue down to zero and then back + * up from zero. Hold state is set to DECEL when planning is + * complete. + * + * - Hold state == DECEL persists until the aline execution runs + * to zero velocity, at which point hold state transitions to + * HOLD. + * + * - Hold state == HOLD persists until the cycle is restarted. A + * cycle start is an asynchronous event that sets the + * cycle_start_flag TRUE. It can occur any time after the hold + * is requested - either before or after motion stops. + * + * - mp_end_hold() is executed from + * cm_feedhold_sequencing_callback() once the hold state == + * HOLD and a cycle_start has been requested.This sets the hold + * state to OFF which enables _exec_aline() to continue + * processing. Move execution begins with the first buffer + * after the hold. * * Terms used: - * - mr is the runtime buffer. It was initially loaded from the bf buffer + * - mr is the runtime buffer. It was initially loaded from the bf + * buffer * - bp+0 is the "companion" bf buffer to the mr buffer. * - bp+1 is the bf buffer following bp+0. This runs through bp+N - * - bp (by itself) just refers to the current buffer being adjusted / replanned - * - * Details: Planning re-uses bp+0 as an "extra" buffer. Normally bp+0 is returned - * to the buffer pool as it is redundant once mr is loaded. Use the extra - * buffer to split the move in two where the hold decelerates to zero. Use - * one buffer to go to zero, the other to replan up from zero. All buffers past - * that point are unaffected other than that they need to be replanned for velocity. - * - * Note: There are multiple opportunities for more efficient organization of - * code in this module, but the code is so complicated I just left it - * organized for clarity and hoped for the best from compiler optimization. + * - bp (by itself) just refers to the current buffer being + * adjusted / replanned + * + * Details: Planning re-uses bp+0 as an "extra" buffer. Normally + * bp+0 is returned to the buffer pool as it is redundant once + * mr is loaded. Use the extra buffer to split the move in two + * where the hold decelerates to zero. Use one buffer to go to + * zero, the other to replan up from zero. All buffers past + * that point are unaffected other than that they need to be + * replanned for velocity. + * + * Note: There are multiple opportunities for more efficient + * organization of code in this module, but the code is so + * complicated I just left it organized for clarity and hoped + * for the best from compiler optimization. */ static float _compute_next_segment_velocity() { if (mr.section == SECTION_BODY) return mr.segment_velocity; @@ -634,16 +689,19 @@ static float _compute_next_segment_velocity() { } +/// replan block list to execute hold stat_t mp_plan_hold_callback() { - if (cm.hold_state != FEEDHOLD_PLAN) return STAT_NOOP; // not planning a feedhold + if (cm.hold_state != FEEDHOLD_PLAN) + return STAT_NOOP; // not planning a feedhold - mpBuf_t *bp; // working buffer pointer - if ((bp = mp_get_run_buffer()) == 0) return STAT_NOOP; // Oops! nothing's running + mpBuf_t *bp; // working buffer pointer + if ((bp = mp_get_run_buffer()) == 0) + return STAT_NOOP; // Oops! nothing's running - uint8_t mr_flag = true; // used to tell replan to account for mr buffer Vx - float mr_available_length; // available length left in mr buffer for deceleration - float braking_velocity; // velocity left to shed to brake to zero - float braking_length; // distance required to brake to zero from braking_velocity + uint8_t mr_flag = true; // used to tell replan to account for mr buffer Vx + float mr_available_length; // available length left in mr buffer for deceleration + float braking_velocity; // velocity left to shed to brake to zero + float braking_length; // distance required to brake to zero from braking_velocity // examine and process mr buffer mr_available_length = get_axis_vector_length(mr.target, mr.position); @@ -652,11 +710,13 @@ stat_t mp_plan_hold_callback() { braking_velocity = _compute_next_segment_velocity(); braking_length = mp_get_target_length(braking_velocity, 0, bp); // bp is OK to use here - // Hack to prevent Case 2 moves for perfect-fit decels. Happens in homing situations - // The real fix: The braking velocity cannot simply be the mr.segment_velocity as this - // is the velocity of the last segment, not the one that's going to be executed next. - // The braking_velocity needs to be the velocity of the next segment that has not yet - // been computed. In the mean time, this hack will work. + // Hack to prevent Case 2 moves for perfect-fit decels. Happens in + // homing situations The real fix: The braking velocity cannot + // simply be the mr.segment_velocity as this is the velocity of the + // last segment, not the one that's going to be executed next. The + // braking_velocity needs to be the velocity of the next segment + // that has not yet been computed. In the mean time, this hack will + // work. if ((braking_length > mr_available_length) && (fp_ZERO(bp->exit_velocity))) braking_length = mr_available_length; @@ -735,19 +795,18 @@ stat_t mp_plan_hold_callback() { } -/// End a feedhold +/// End a feedhold, release the hold and restart block list stat_t mp_end_hold() { if (cm.hold_state == FEEDHOLD_END_HOLD) { cm.hold_state = FEEDHOLD_OFF; - mpBuf_t *bf; - if ((bf = mp_get_run_buffer()) == 0) { // 0 means nothing's running + if (!mp_get_run_buffer()) { // 0 means nothing's running cm_set_motion_state(MOTION_STOP); return STAT_NOOP; } cm.motion_state = MOTION_RUN; - st_request_exec_move(); // restart the steppers + st_request_exec_move(); // restart the steppers } return STAT_OK; diff --git a/src/plan/planner.c b/src/plan/planner.c index aadc908..b392574 100644 --- a/src/plan/planner.c +++ b/src/plan/planner.c @@ -134,7 +134,7 @@ void mp_set_runtime_position(uint8_t axis, const float position) {mr.position[ax void mp_set_steps_to_runtime_position() { float step_position[MOTORS]; - ik_kinematics(mr.position, step_position); // convert lengths to steps in floating point + ik_kinematics(mr.position, step_position); // convert lengths to steps in floating point for (uint8_t motor = 0; motor < MOTORS; motor++) { mr.target_steps[motor] = step_position[motor]; diff --git a/src/plan/planner.h b/src/plan/planner.h index a40a862..ba347d4 100644 --- a/src/plan/planner.h +++ b/src/plan/planner.h @@ -5,25 +5,31 @@ * Copyright (c) 2013 - 2015 Alden S. Hart, Jr. * Copyright (c) 2013 - 2015 Robert Giseburt * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #ifndef PLANNER_H @@ -64,32 +70,30 @@ enum sectionState { SECTION_2nd_HALF // second half of S curve or running a BODY (cruise) }; -// Most of these factors are the result of a lot of tweaking. Change with caution. -#define ARC_SEGMENT_LENGTH ((float)0.1) // Arc segment size (mm).(0.03) +// Most of these factors are the result of a lot of tweaking. +// Change with caution. +#define ARC_SEGMENT_LENGTH ((float)0.1) // Arc segment size (mm).(0.03) #define MIN_ARC_RADIUS ((float)0.1) #define JERK_MULTIPLIER ((float)1000000) -#define JERK_MATCH_PRECISION ((float)1000) // precision jerk must match to be considered same +/// precision jerk must match to be considered same +#define JERK_MATCH_PRECISION ((float)1000) -#define NOM_SEGMENT_USEC ((float)5000) // nominal segment time -#define MIN_SEGMENT_USEC ((float)2500) // minimum segment time / minimum move time -#define MIN_ARC_SEGMENT_USEC ((float)10000) // minimum arc segment time +#define NOM_SEGMENT_USEC ((float)5000) // nominal segment time +/// minimum segment time / minimum move time +#define MIN_SEGMENT_USEC ((float)2500) +#define MIN_ARC_SEGMENT_USEC ((float)10000) // minimum arc segment time #define NOM_SEGMENT_TIME (NOM_SEGMENT_USEC / MICROSECONDS_PER_MINUTE) #define MIN_SEGMENT_TIME (MIN_SEGMENT_USEC / MICROSECONDS_PER_MINUTE) #define MIN_ARC_SEGMENT_TIME (MIN_ARC_SEGMENT_USEC / MICROSECONDS_PER_MINUTE) -#define MIN_TIME_MOVE MIN_SEGMENT_TIME // minimum time a move can be is one segment -#define MIN_BLOCK_TIME MIN_SEGMENT_TIME // factor for minimum size Gcode block to process +/// minimum time a move can be is one segment +#define MIN_TIME_MOVE MIN_SEGMENT_TIME +/// factor for minimum size Gcode block to process +#define MIN_BLOCK_TIME MIN_SEGMENT_TIME -#define MIN_SEGMENT_TIME_PLUS_MARGIN ((MIN_SEGMENT_USEC+1) / MICROSECONDS_PER_MINUTE) - -/* PLANNER_STARTUP_DELAY_SECONDS - * Used to introduce a short dwell before planning an idle machine. - * If you don't do this the first block will always plan to zero as it will - * start executing before the next block arrives from the serial port. - * This causes the machine to stutter once on startup. - */ -//#define PLANNER_STARTUP_DELAY_SECONDS ((float)0.05) // in seconds +#define MIN_SEGMENT_TIME_PLUS_MARGIN \ + ((MIN_SEGMENT_USEC+1) / MICROSECONDS_PER_MINUTE) /* PLANNER_BUFFER_POOL_SIZE * Should be at least the number of buffers requires to support optimal @@ -130,17 +134,17 @@ enum mpBufferState { // bf->buffer_state values }; -typedef struct mpBuffer { // See Planning Velocity Notes for variable usage +typedef struct mpBuffer { // See Planning Velocity Notes struct mpBuffer *pv; // static pointer to previous buffer struct mpBuffer *nx; // static pointer to next buffer stat_t (*bf_func)(struct mpBuffer *bf); // callback to buffer exec function - cm_exec_t cm_func; // callback to canonical machine execution function + cm_exec_t cm_func; // callback to canonical machine float naiive_move_time; uint8_t buffer_state; // used to manage queuing/dequeuing uint8_t move_type; // used to dispatch to run routine - uint8_t move_code; // byte that can be used by used exec functions + uint8_t move_code; // byte used by used exec functions uint8_t move_state; // move state machine sequence uint8_t replannable; // TRUE if move can be re-planned @@ -150,7 +154,7 @@ typedef struct mpBuffer { // See Planning Velocity Notes for variable us float head_length; float body_length; float tail_length; - // *** SEE NOTES ON THESE VARIABLES, in aline() *** + // See notes on these variables, in aline() float entry_velocity; // entry velocity requested for the move float cruise_velocity; // cruise velocity requested & achieved float exit_velocity; // exit velocity requested for the move @@ -161,26 +165,26 @@ typedef struct mpBuffer { // See Planning Velocity Notes for variable us float delta_vmax; // max velocity difference for this move float braking_velocity; // current value for braking velocity - uint8_t jerk_axis; // rate limiting axis used to compute jerk for the move + uint8_t jerk_axis; // rate limiting axis used to compute jerk float jerk; // maximum linear jerk term for this move - float recip_jerk; // 1/Jm used for planning (computed and cached) - float cbrt_jerk; // cube root of Jm used for planning (computed and cached) - - GCodeState_t gm; // Gode model state - passed from model, used by planner and runtime + float recip_jerk; // 1/Jm used for planning (computed & cached) + float cbrt_jerk; // cube root of Jm (computed & cached) + GCodeState_t gm; // Gode model state, used by planner & runtime } mpBuf_t; -typedef struct mpBufferPool { // ring buffer for sub-moves - uint8_t buffers_available; // running count of available buffers - mpBuf_t *w; // get_write_buffer pointer - mpBuf_t *q; // queue_write_buffer pointer - mpBuf_t *r; // get/end_run_buffer pointer +typedef struct mpBufferPool { // ring buffer for sub-moves + uint8_t buffers_available; // running count of available buffers + mpBuf_t *w; // get_write_buffer pointer + mpBuf_t *q; // queue_write_buffer pointer + mpBuf_t *r; // get/end_run_buffer pointer mpBuf_t bf[PLANNER_BUFFER_POOL_SIZE]; // buffer storage } mpBufferPool_t; -typedef struct mpMoveMasterSingleton { // common variables for planning (move master) +/// common variables for planning (move master) +typedef struct mpMoveMasterSingleton { float position[AXES]; // final move position for planning purposes float jerk; // jerk values cached from previous block @@ -190,23 +194,33 @@ typedef struct mpMoveMasterSingleton { // common variables for planning (move ma typedef struct mpMoveRuntimeSingleton { // persistent runtime variables - uint8_t move_state; // state of the overall move - uint8_t section; // what section is the move in? - uint8_t section_state; // state within a move section - - float unit[AXES]; // unit vector for axis scaling & planning - float target[AXES]; // final target for bf (used to correct rounding errors) - float position[AXES]; // current move position - float position_c[AXES]; // for Kahan summation in _exec_aline_segment() - float waypoint[SECTIONS][AXES]; // head/body/tail endpoints for correction - - float target_steps[MOTORS]; // current MR target (absolute target as steps) - float position_steps[MOTORS]; // current MR position (target from previous segment) - float commanded_steps[MOTORS]; // will align with next encoder sample (target 2nd previous segment) - float encoder_steps[MOTORS]; // encoder position in steps - ideally the same as commanded_steps - float following_error[MOTORS]; // difference between encoder_steps and commanded steps - - float head_length; // copies of bf variables of same name + uint8_t move_state; // state of the overall move + uint8_t section; // what section is the move in? + uint8_t section_state; // state within a move section + + /// unit vector for axis scaling & planning + float unit[AXES]; + /// final target for bf (used to correct rounding errors) + float target[AXES]; + /// current move position + float position[AXES]; + /// for Kahan summation in _exec_aline_segment() + float position_c[AXES]; + /// head/body/tail endpoints for correction + float waypoint[SECTIONS][AXES]; + /// current MR target (absolute target as steps) + float target_steps[MOTORS]; + /// current MR position (target from previous segment) + float position_steps[MOTORS]; + /// will align with next encoder sample (target 2nd previous segment) + float commanded_steps[MOTORS]; + /// encoder position in steps - ideally the same as commanded_steps + float encoder_steps[MOTORS]; + /// difference between encoder & commanded steps + float following_error[MOTORS]; + + /// copies of bf variables of same name + float head_length; float body_length; float tail_length; @@ -214,31 +228,31 @@ typedef struct mpMoveRuntimeSingleton { // persistent runtime variables float cruise_velocity; float exit_velocity; - float segments; // number of segments in line (also used by arc generation) + float segments; // number of segments in line or arc uint32_t segment_count; // count of running segments float segment_velocity; // computed velocity for aline segment float segment_time; // actual time increment per aline segment float jerk; // max linear jerk -#ifdef __JERK_EXEC // values used exclusively by computed jerk acceleration +#ifdef __JERK_EXEC // values used exclusively by computed jerk acceleration float jerk_div2; // cached value for efficiency float midpoint_velocity; // velocity at accel/decel midpoint float midpoint_acceleration; float accel_time; float segment_accel_time; float elapsed_accel_time; -#else // values used exclusively by forward differencing acceleration +#else // values used exclusively by forward differencing acceleration float forward_diff_1; // forward difference level 1 float forward_diff_2; // forward difference level 2 float forward_diff_3; // forward difference level 3 float forward_diff_4; // forward difference level 4 float forward_diff_5; // forward difference level 5 #ifdef __KAHAN - float forward_diff_1_c; // forward difference level 1 floating-point compensation - float forward_diff_2_c; // forward difference level 2 floating-point compensation - float forward_diff_3_c; // forward difference level 3 floating-point compensation - float forward_diff_4_c; // forward difference level 4 floating-point compensation - float forward_diff_5_c; // forward difference level 5 floating-point compensation + float forward_diff_1_c; // level 1 floating-point compensation + float forward_diff_2_c; // level 2 floating-point compensation + float forward_diff_3_c; // level 3 floating-point compensation + float forward_diff_4_c; // level 4 floating-point compensation + float forward_diff_5_c; // level 5 floating-point compensation #endif #endif @@ -260,7 +274,8 @@ void mp_set_planner_position(uint8_t axis, const float position); void mp_set_runtime_position(uint8_t axis, const float position); void mp_set_steps_to_runtime_position(); -void mp_queue_command(void(*cm_exec_t)(float[], float[]), float *value, float *flag); +void mp_queue_command(void(*cm_exec_t)(float[], float[]), float *value, + float *flag); stat_t mp_runtime_command(mpBuf_t *bf); stat_t mp_dwell(const float seconds); @@ -275,21 +290,17 @@ stat_t mp_feed_rate_override(uint8_t flag, float parameter); // planner buffer handlers uint8_t mp_get_planner_buffers_available(); void mp_init_buffers(); -mpBuf_t * mp_get_write_buffer(); +mpBuf_t *mp_get_write_buffer(); void mp_unget_write_buffer(); void mp_commit_write_buffer(const uint8_t move_type); - mpBuf_t *mp_get_run_buffer(); uint8_t mp_free_run_buffer(); mpBuf_t *mp_get_first_buffer(); mpBuf_t *mp_get_last_buffer(); - /// Returns pointer to prev buffer in linked list #define mp_get_prev_buffer(b) ((mpBuf_t *)(b->pv)) - /// Returns pointer to next buffer in linked list #define mp_get_next_buffer(b) ((mpBuf_t *)(b->nx)) - void mp_clear_buffer(mpBuf_t *bf); void mp_copy_buffer(mpBuf_t *bf, const mpBuf_t *bp); diff --git a/src/plan/zoid.c b/src/plan/zoid.c index 62c1d98..f6952eb 100644 --- a/src/plan/zoid.c +++ b/src/plan/zoid.c @@ -44,20 +44,20 @@ * (Note: sections, not moves) so we can compute entry and exits for adjacent sections. * * Inputs used are: - * bf->length - actual block length (length is never changed) + * bf->length - actual block length (length is never changed) * bf->entry_velocity - requested Ve (entry velocity is never changed) - * bf->cruise_velocity - requested Vt (is often changed) - * bf->exit_velocity - requested Vx (may be changed for degenerate cases) - * bf->cruise_vmax - used in some comparisons + * bf->cruise_velocity - requested Vt (is often changed) + * bf->exit_velocity - requested Vx (may be changed for degenerate cases) + * bf->cruise_vmax - used in some comparisons * bf->delta_vmax - used to degrade velocity of pathologically short blocks * * Variables that may be set/updated are: - * bf->entry_velocity - requested Ve - * bf->cruise_velocity - requested Vt - * bf->exit_velocity - requested Vx - * bf->head_length - bf->length allocated to head - * bf->body_length - bf->length allocated to body - * bf->tail_length - bf->length allocated to tail + * bf->entry_velocity - requested Ve + * bf->cruise_velocity - requested Vt + * bf->exit_velocity - requested Vx + * bf->head_length - bf->length allocated to head + * bf->body_length - bf->length allocated to body + * bf->tail_length - bf->length allocated to tail * * Note: The following conditions must be met on entry: * bf->length must be non-zero (filter these out upstream) @@ -77,7 +77,7 @@ * the entry velocity to the exit velocity in the available length. These * velocities are not negotiable, so a degraded solution is found. * - * In worst cases the move cannot be executed as the required execution time is + * In worst cases the move cannot be executed as the required execution time is * less than the minimum segment time. The first degradation is to reduce the * move to a body-only segment with an average velocity. If that still doesn't * fit then the move velocity is reduced so it fits into a minimum segment. @@ -87,26 +87,26 @@ * Various cases handled (H=head, B=body, T=tail) * * Requested-Fit cases - * HBT VeVx sufficient length exists for all parts (corner case: HBT') - * HB VeVx enter at full speed and decelerate (corner case: T') - * HT Ve & Vx perfect fit HT (very rare). May be symmetric or asymmetric - * H VeVx perfect fit T (common, results from planning) + * HBT VeVx sufficient length exists for all parts (corner case: HBT') + * HB VeVx enter at full speed and decelerate (corner case: T') + * HT Ve & Vx perfect fit HT (very rare). May be symmetric or asymmetric + * H VeVx perfect fit T (common, results from planning) * B Ve=Vt=Vx Velocities are close to each other and within matching tolerance * * Rate-Limited cases - Ve and Vx can be satisfied but Vt cannot * HT (Ve=Vx)Vx Ve is degraded (velocity step). Vx is met - * B" line is very short but drawable; is treated as a body only - * F force fit: This block is slowed down until it can be executed + * B" line is very short but drawable; is treated as a body only + * F force fit: This block is slowed down until it can be executed * * NOTE: The order of the cases/tests in the code is pretty important. Start with the * shortest cases first and work up. Not only does this simplify the order of the tests, @@ -224,7 +224,8 @@ void mp_calculate_trapezoid(mpBuf_t *bf) { if (fabs(bf->entry_velocity - bf->exit_velocity) < TRAPEZOID_VELOCITY_TOLERANCE) { bf->head_length = bf->length/2; bf->tail_length = bf->head_length; - bf->cruise_velocity = min(bf->cruise_vmax, mp_get_target_velocity(bf->entry_velocity, bf->head_length, bf)); + bf->cruise_velocity = + min(bf->cruise_vmax, mp_get_target_velocity(bf->entry_velocity, bf->head_length, bf)); if (bf->head_length < MIN_HEAD_LENGTH) { // Convert this to a body-only move @@ -304,14 +305,14 @@ void mp_calculate_trapezoid(mpBuf_t *bf) { /* * This set of functions returns the fourth thing knowing the other three. * - * Jm = the given maximum jerk + * Jm = the given maximum jerk * T = time of the entire move - * Vi = initial velocity - * Vf = final velocity + * Vi = initial velocity + * Vf = final velocity * T = 2*sqrt((Vt-Vi)/Jm) * As = The acceleration at inflection point between convex and concave portions of the S-curve. * As = (Jm*T)/2 - * Ar = ramp acceleration + * Ar = ramp acceleration * Ar = As/2 = (Jm*T)/4 * * mp_get_target_length() is a convenient function for determining the optimal_length (L) @@ -319,18 +320,18 @@ void mp_calculate_trapezoid(mpBuf_t *bf) { * * The length (distance) equation is derived from: * - * a) L = (Vf-Vi) * T - (Ar*T^2)/2 ... which becomes b) with substitutions for Ar and T - * b) L = (Vf-Vi) * 2*sqrt((Vf-Vi)/Jm) - (2*sqrt((Vf-Vi)/Jm) * (Vf-Vi))/2 - * c) L = (Vf-Vi)^(3/2) / sqrt(Jm) ...is an alternate form of b) (see Wolfram Alpha) - * c')L = (Vf-Vi) * sqrt((Vf-Vi)/Jm) ... second alternate form; requires Vf >= Vi + * a) L = (Vf-Vi) * T - (Ar*T^2)/2 ... which becomes b) with substitutions for Ar and T + * b) L = (Vf-Vi) * 2*sqrt((Vf-Vi)/Jm) - (2*sqrt((Vf-Vi)/Jm) * (Vf-Vi))/2 + * c) L = (Vf-Vi)^(3/2) / sqrt(Jm) ...is an alternate form of b) (see Wolfram Alpha) + * c') L = (Vf-Vi) * sqrt((Vf-Vi)/Jm) ... second alternate form; requires Vf >= Vi * * Notes: Ar = (Jm*T)/4 Ar is ramp acceleration - * T = 2*sqrt((Vf-Vi)/Jm) T is time + * T = 2*sqrt((Vf-Vi)/Jm) T is time * Assumes Vi, Vf and L are positive or zero * Cannot assume Vf>=Vi due to rounding errors and use of PLANNER_VELOCITY_TOLERANCE - * necessitating the introduction of fabs() + * necessitating the introduction of fabs() * - * mp_get_target_velocity() is a convenient function for determining Vf target velocity for + * mp_get_target_velocity() is a convenient function for determining Vf target velocity for * a given the initial velocity (Vi), length (L), and maximum jerk (Jm). * Equation d) is b) solved for Vf. Equation e) is c) solved for Vf. Use e) (obviously) * @@ -365,14 +366,15 @@ float mp_get_target_length(const float Vi, const float Vf, const mpBuf_t *bf) { * There are (at least) two such functions we can use: * L from J, Vi, and Vf * L = sqrt((Vf - Vi) / J) (Vi + Vf) + * * Replacing Vf with x, and subtracting the known L: * 0 = sqrt((x - Vi) / J) (Vi + x) - L * Z(x) = sqrt((x - Vi) / J) (Vi + x) - L * * OR - * * J from L, Vi, and Vf * J = ((Vf - Vi) (Vi + Vf)²) / L² + * * Replacing Vf with x, and subtracting the known J: * 0 = ((x - Vi) (Vi + x)²) / L² - J * Z(x) = ((x - Vi) (Vi + x)²) / L² - J diff --git a/src/stepper.c b/src/stepper.c index 32b22de..6d2e168 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -5,29 +5,35 @@ * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. * Copyright (c) 2013 - 2015 Robert Giseburt * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/* This module provides the low-level stepper drivers and some related functions. - * See stepper.h for a detailed explanation of this module. +/* This module provides the low-level stepper drivers and some related + * functions. See stepper.h for a detailed explanation of this module. */ #include "stepper.h" @@ -46,6 +52,7 @@ #include #include + stConfig_t st_cfg; stPrepSingleton_t st_pre; static stRunSingleton_t st_run; @@ -54,7 +61,6 @@ static void _load_move(); static void _request_load_move(); -// handy macro #define _f_to_period(f) (uint16_t)((float)F_CPU / (float)f) #define MOTOR_1 0 @@ -65,25 +71,36 @@ static void _request_load_move(); #define MOTOR_6 5 -/* - * Initialize stepper motor subsystem +static VPORT_t *vports[] = { + &PORT_MOTOR_1_VPORT, + &PORT_MOTOR_2_VPORT, + &PORT_MOTOR_3_VPORT, + &PORT_MOTOR_4_VPORT +}; + + +/* Initialize stepper motor subsystem * - * Notes: - * - This init requires sys_init() to be run beforehand - * - microsteps are setup during config_init() - * - motor polarity is setup during config_init() - * - high level interrupts must be enabled in main() once all inits are complete + * Notes: + * - This init requires sys_init() to be run beforehand + * - microsteps are setup during config_init() + * - motor polarity is setup during config_init() + * - high level interrupts must be enabled in main() once all inits are + * complete */ void stepper_init() { - memset(&st_run, 0, sizeof(st_run)); // clear all values, pointers and status + memset(&st_run, 0, sizeof(st_run)); // clear all values, pointers and status // Configure virtual ports - PORTCFG.VPCTRLA = PORTCFG_VP0MAP_PORT_MOTOR_1_gc | PORTCFG_VP1MAP_PORT_MOTOR_2_gc; - PORTCFG.VPCTRLB = PORTCFG_VP2MAP_PORT_MOTOR_3_gc | PORTCFG_VP3MAP_PORT_MOTOR_4_gc; + PORTCFG.VPCTRLA = + PORTCFG_VP0MAP_PORT_MOTOR_1_gc | PORTCFG_VP1MAP_PORT_MOTOR_2_gc; + PORTCFG.VPCTRLB = + PORTCFG_VP2MAP_PORT_MOTOR_3_gc | PORTCFG_VP3MAP_PORT_MOTOR_4_gc; // setup ports and data structures for (uint8_t i = 0; i < MOTORS; i++) { - hw.st_port[i]->DIR = MOTOR_PORT_DIR_gm; // sets outputs for motors & GPIO1, and GPIO2 inputs + // motors outputs & GPIO1 and GPIO2 inputs + hw.st_port[i]->DIR = MOTOR_PORT_DIR_gm; hw.st_port[i]->OUTSET = MOTOR_ENABLE_BIT_bm; // disable motor } @@ -120,7 +137,7 @@ void stepper_init() { st_cfg.mot[MOTOR_1].microsteps = M1_MICROSTEPS; st_cfg.mot[MOTOR_1].polarity = M1_POLARITY; st_cfg.mot[MOTOR_1].power_mode = M1_POWER_MODE; -#if (MOTORS >= 2) +#if 1 < MOTORS st_cfg.mot[MOTOR_2].motor_map = M2_MOTOR_MAP; st_cfg.mot[MOTOR_2].step_angle = M2_STEP_ANGLE; st_cfg.mot[MOTOR_2].travel_rev = M2_TRAVEL_PER_REV; @@ -128,7 +145,7 @@ void stepper_init() { st_cfg.mot[MOTOR_2].polarity = M2_POLARITY; st_cfg.mot[MOTOR_2].power_mode = M2_POWER_MODE; #endif -#if (MOTORS >= 3) +#if 2 < MOTORS st_cfg.mot[MOTOR_3].motor_map = M3_MOTOR_MAP; st_cfg.mot[MOTOR_3].step_angle = M3_STEP_ANGLE; st_cfg.mot[MOTOR_3].travel_rev = M3_TRAVEL_PER_REV; @@ -136,7 +153,7 @@ void stepper_init() { st_cfg.mot[MOTOR_3].polarity = M3_POLARITY; st_cfg.mot[MOTOR_3].power_mode = M3_POWER_MODE; #endif -#if (MOTORS >= 4) +#if 3 < MOTORS st_cfg.mot[MOTOR_4].motor_map = M4_MOTOR_MAP; st_cfg.mot[MOTOR_4].step_angle = M4_STEP_ANGLE; st_cfg.mot[MOTOR_4].travel_rev = M4_TRAVEL_PER_REV; @@ -144,39 +161,19 @@ void stepper_init() { st_cfg.mot[MOTOR_4].polarity = M4_POLARITY; st_cfg.mot[MOTOR_4].power_mode = M4_POWER_MODE; #endif -#if (MOTORS >= 5) - st_cfg.mot[MOTOR_5].motor_map = M5_MOTOR_MAP; - st_cfg.mot[MOTOR_5].step_angle = M5_STEP_ANGLE; - st_cfg.mot[MOTOR_5].travel_rev = M5_TRAVEL_PER_REV; - st_cfg.mot[MOTOR_5].microsteps = M5_MICROSTEPS; - st_cfg.mot[MOTOR_5].polarity = M5_POLARITY; - st_cfg.mot[MOTOR_5].power_mode = M5_POWER_MODE; -#endif -#if (MOTORS >= 6) - st_cfg.mot[MOTOR_6].motor_map = M6_MOTOR_MAP; - st_cfg.mot[MOTOR_6].step_angle = M6_STEP_ANGLE; - st_cfg.mot[MOTOR_6].travel_rev = M6_TRAVEL_PER_REV; - st_cfg.mot[MOTOR_6].microsteps = M6_MICROSTEPS; - st_cfg.mot[MOTOR_6].polarity = M6_POLARITY; - st_cfg.mot[MOTOR_6].power_mode = M6_POWER_MODE; -#endif // Init steps per unit for (int m = 0; m < MOTORS; m++) st_cfg.mot[m].steps_per_unit = - (360 * st_cfg.mot[m].microsteps) / (st_cfg.mot[m].travel_rev * st_cfg.mot[m].step_angle); + (360 * st_cfg.mot[m].microsteps) / + (st_cfg.mot[m].travel_rev * st_cfg.mot[m].step_angle); - st_reset(); // reset steppers to known state + st_reset(); // reset steppers to known state } -/* - * return TRUE if runtime is busy: - * - * Busy conditions: - * - motors are running - * - dwell is running - */ +/// return true if runtime is busy: +/// Busy conditions: 1. motors are running, 2. dwell is running uint8_t st_runtime_isbusy() { return st_run.dda_ticks_downcount != 0; } @@ -186,68 +183,43 @@ uint8_t st_runtime_isbusy() { void st_reset() { for (uint8_t motor = 0; motor < MOTORS; motor++) { st_pre.mot[motor].prev_direction = STEP_INITIAL_DIRECTION; - st_run.mot[motor].substep_accumulator = 0; // will become max negative during per-motor setup; - st_pre.mot[motor].corrected_steps = 0; // diagnostic only - no action effect + // will become max negative during per-motor setup; + st_run.mot[motor].substep_accumulator = 0; + st_pre.mot[motor].corrected_steps = 0; // diagnostic only - no effect } mp_set_steps_to_runtime_position(); } -/* - * Motor power management functions - * - * _deenergize_motor() - remove power from a motor - * _energize_motor() - apply power to a motor - * - * st_energize_motors() - apply power to all motors - * st_deenergize_motors() - remove power from all motors - * st_motor_power_callback() - callback to manage motor power sequencing - */ static uint8_t _motor_is_enabled(uint8_t motor) { - uint8_t port; - switch(motor) { - case (MOTOR_1): port = PORT_MOTOR_1_VPORT.OUT; break; - case (MOTOR_2): port = PORT_MOTOR_2_VPORT.OUT; break; - case (MOTOR_3): port = PORT_MOTOR_3_VPORT.OUT; break; - case (MOTOR_4): port = PORT_MOTOR_4_VPORT.OUT; break; - default: port = 0xff; // defaults to disabled for bad motor input value - } - + uint8_t port = 0xff; + if (motor < 4) port = vports[motor]->OUT; // returns 1 if motor is enabled (motor is actually active low) return port & MOTOR_ENABLE_BIT_bm ? 0 : 1; } +/// Remove power from a motor static void _deenergize_motor(const uint8_t motor) { - switch (motor) { - case (MOTOR_1): PORT_MOTOR_1_VPORT.OUT |= MOTOR_ENABLE_BIT_bm; break; - case (MOTOR_2): PORT_MOTOR_2_VPORT.OUT |= MOTOR_ENABLE_BIT_bm; break; - case (MOTOR_3): PORT_MOTOR_3_VPORT.OUT |= MOTOR_ENABLE_BIT_bm; break; - case (MOTOR_4): PORT_MOTOR_4_VPORT.OUT |= MOTOR_ENABLE_BIT_bm; break; - } - + if (motor < 4) vports[motor]->OUT |= MOTOR_ENABLE_BIT_bm; st_run.mot[motor].power_state = MOTOR_OFF; } +/// Apply power to a motor static void _energize_motor(const uint8_t motor) { if (st_cfg.mot[motor].power_mode == MOTOR_DISABLED) { _deenergize_motor(motor); return; } - switch(motor) { - case (MOTOR_1): PORT_MOTOR_1_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; break; - case (MOTOR_2): PORT_MOTOR_2_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; break; - case (MOTOR_3): PORT_MOTOR_3_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; break; - case (MOTOR_4): PORT_MOTOR_4_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; break; - } - + if (motor < 4) vports[motor]->OUT &= ~MOTOR_ENABLE_BIT_bm; st_run.mot[motor].power_state = MOTOR_POWER_TIMEOUT_START; } +/// Apply power to all motors void st_energize_motors() { for (uint8_t motor = MOTOR_1; motor < MOTORS; motor++) { _energize_motor(motor); @@ -256,46 +228,43 @@ void st_energize_motors() { } +/// Remove power from all motors void st_deenergize_motors() { for (uint8_t motor = MOTOR_1; motor < MOTORS; motor++) _deenergize_motor(motor); } -/* - * Callback to manage motor power sequencing - * Handles motor power-down timing, low-power idle, and adaptive motor power - */ + +/// Callback to manage motor power sequencing +/// Handles motor power-down timing, low-power idle, and adaptive motor power stat_t st_motor_power_callback() { // called by controller - // manage power for each motor individually + // Manage power for each motor individually for (uint8_t m = 0; m < MOTORS; m++) { - - // de-energize motor if it's set to MOTOR_DISABLED + // De-energize motor if it's set to MOTOR_DISABLED if (st_cfg.mot[m].power_mode == MOTOR_DISABLED) { _deenergize_motor(m); continue; } - // energize motor if it's set to MOTOR_ALWAYS_POWERED + // Energize motor if it's set to MOTOR_ALWAYS_POWERED if (st_cfg.mot[m].power_mode == MOTOR_ALWAYS_POWERED) { - if (! _motor_is_enabled(m)) _energize_motor(m); + if (!_motor_is_enabled(m)) _energize_motor(m); continue; } - // start a countdown if MOTOR_POWERED_IN_CYCLE or MOTOR_POWERED_ONLY_WHEN_MOVING + // Start a countdown if MOTOR_POWERED_IN_CYCLE or + // MOTOR_POWERED_ONLY_WHEN_MOVING if (st_run.mot[m].power_state == MOTOR_POWER_TIMEOUT_START) { st_run.mot[m].power_state = MOTOR_POWER_TIMEOUT_COUNTDOWN; st_run.mot[m].power_systick = rtc_get_time() + (st_cfg.motor_power_timeout * 1000); } - // do not process countdown if in a feedhold - if (cm_get_combined_state() == COMBINED_HOLD) continue; - - // do not process countdown if in a feedhold + // Do not process countdown if in a feedhold if (cm_get_combined_state() == COMBINED_HOLD) continue; - // run the countdown if you are in a countdown + // Run the countdown if you are in a countdown if (st_run.mot[m].power_state == MOTOR_POWER_TIMEOUT_COUNTDOWN) { if (rtc_get_time() > st_run.mot[m].power_systick ) { st_run.mot[m].power_state = MOTOR_IDLE; @@ -309,328 +278,189 @@ stat_t st_motor_power_callback() { // called by controller } -/*** - Stepper Interrupt Service Routine - DDA timer interrupt routine - service ticks from DDA timer +static inline void _step_motor(int motor) { + st_run.mot[motor].substep_accumulator += st_run.mot[motor].substep_increment; - Uses direct struct addresses and literal values for hardware devices - it's faster than - using indexed timer and port accesses. I checked. Even when -0s or -03 is used. -*/ -ISR(TIMER_DDA_ISR_vect) { - if ((st_run.mot[MOTOR_1].substep_accumulator += st_run.mot[MOTOR_1].substep_increment) > 0) { - PORT_MOTOR_1_VPORT.OUT ^= STEP_BIT_bm; // toggle step line - st_run.mot[MOTOR_1].substep_accumulator -= st_run.dda_ticks_X_substeps; - INCREMENT_ENCODER(MOTOR_1); + if (0 < st_run.mot[motor].substep_accumulator) { + vports[motor]->OUT ^= STEP_BIT_bm; // toggle step line + st_run.mot[motor].substep_accumulator -= st_run.dda_ticks_X_substeps; + INCREMENT_ENCODER(motor); } +} - if ((st_run.mot[MOTOR_2].substep_accumulator += st_run.mot[MOTOR_2].substep_increment) > 0) { - PORT_MOTOR_2_VPORT.OUT ^= STEP_BIT_bm; - st_run.mot[MOTOR_2].substep_accumulator -= st_run.dda_ticks_X_substeps; - INCREMENT_ENCODER(MOTOR_2); - } - if ((st_run.mot[MOTOR_3].substep_accumulator += st_run.mot[MOTOR_3].substep_increment) > 0) { - PORT_MOTOR_3_VPORT.OUT ^= STEP_BIT_bm; - st_run.mot[MOTOR_3].substep_accumulator -= st_run.dda_ticks_X_substeps; - INCREMENT_ENCODER(MOTOR_3); - } - if ((st_run.mot[MOTOR_4].substep_accumulator += st_run.mot[MOTOR_4].substep_increment) > 0) { - PORT_MOTOR_4_VPORT.OUT ^= STEP_BIT_bm; - st_run.mot[MOTOR_4].substep_accumulator -= st_run.dda_ticks_X_substeps; - INCREMENT_ENCODER(MOTOR_4); - } +/// Stepper Interrupt Service Routine +/// DDA timer interrupt routine - service ticks from DDA timer +ISR(TIMER_DDA_ISR_vect) { + _step_motor(MOTOR_1); + _step_motor(MOTOR_2); + _step_motor(MOTOR_3); + _step_motor(MOTOR_4); - if (--st_run.dda_ticks_downcount != 0) return; + if (--st_run.dda_ticks_downcount) return; - TIMER_DDA.CTRLA = STEP_TIMER_DISABLE; // disable DDA timer - _load_move(); // load the next move + TIMER_DDA.CTRLA = STEP_TIMER_DISABLE; // disable DDA timer + _load_move(); // load the next move } -/// DDA timer interrupt routine - service ticks from DDA timer -ISR(TIMER_DWELL_ISR_vect) { // DWELL timer interrupt +/// DWELL timer interrupt +ISR(TIMER_DWELL_ISR_vect) { if (--st_run.dda_ticks_downcount == 0) { - TIMER_DWELL.CTRLA = STEP_TIMER_DISABLE; // disable DWELL timer + TIMER_DWELL.CTRLA = STEP_TIMER_DISABLE; // disable DWELL timer _load_move(); } } -/**************************************************************************************** - * Exec sequencing code - computes and prepares next load segment - * st_request_exec_move() - SW interrupt to request to execute a move - * exec_timer interrupt - interrupt handler for calling exec function - */ +/// SW interrupt to request to execute a move void st_request_exec_move() { - if (st_pre.buffer_state == PREP_BUFFER_OWNED_BY_EXEC) {// bother interrupting + if (st_pre.buffer_state == PREP_BUFFER_OWNED_BY_EXEC) { TIMER_EXEC.PER = EXEC_TIMER_PERIOD; - TIMER_EXEC.CTRLA = EXEC_TIMER_ENABLE; // trigger a LO interrupt + TIMER_EXEC.CTRLA = EXEC_TIMER_ENABLE; // trigger a LO interrupt } } -ISR(TIMER_EXEC_ISR_vect) { // exec move SW interrupt - TIMER_EXEC.CTRLA = EXEC_TIMER_DISABLE; // disable SW interrupt timer +/// Interrupt handler for calling exec function +ISR(TIMER_EXEC_ISR_vect) { + TIMER_EXEC.CTRLA = EXEC_TIMER_DISABLE; // disable SW interrupt timer - // exec_move - if (st_pre.buffer_state == PREP_BUFFER_OWNED_BY_EXEC) { - if (mp_exec_move() != STAT_NOOP) { - st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // flip it back - _request_load_move(); - } + // Exec move + if (st_pre.buffer_state == PREP_BUFFER_OWNED_BY_EXEC && + mp_exec_move() != STAT_NOOP) { + st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // flip it back + _request_load_move(); } } -/**************************************************************************************** - * Loader sequencing code - * st_request_load_move() - fires a software interrupt (timer) to request to load a move - * load_move interrupt - interrupt handler for running the loader - * - * _load_move() can only be called be called from an ISR at the same or higher level as - * the DDA or dwell ISR. A software interrupt has been provided to allow a non-ISR to - * request a load (see st_request_load_move()) - */ + +/// Fires a software interrupt (timer) to request to load a move static void _request_load_move() { - if (st_runtime_isbusy()) return; // don't request a load if the runtime is busy + if (st_runtime_isbusy()) return; // don't request a load if runtime is busy - if (st_pre.buffer_state == PREP_BUFFER_OWNED_BY_LOADER) { // bother interrupting + if (st_pre.buffer_state == PREP_BUFFER_OWNED_BY_LOADER) { TIMER_LOAD.PER = LOAD_TIMER_PERIOD; - TIMER_LOAD.CTRLA = LOAD_TIMER_ENABLE; // trigger a HI interrupt + TIMER_LOAD.CTRLA = LOAD_TIMER_ENABLE; // trigger a HI interrupt } } -ISR(TIMER_LOAD_ISR_vect) { // load steppers SW interrupt - TIMER_LOAD.CTRLA = LOAD_TIMER_DISABLE; // disable SW interrupt timer +/// Interrupt handler for running the loader +ISR(TIMER_LOAD_ISR_vect) { + TIMER_LOAD.CTRLA = LOAD_TIMER_DISABLE; // disable SW interrupt timer + + // _load_move() can only be called be called from an ISR at the same or + // higher level as the DDA or dwell ISR. A software interrupt has been + // provided to allow a non-ISR to request a load + // (see st_request_load_move()) _load_move(); } -/**************************************************************************************** - * _load_move() - Dequeue move and load into stepper struct - * - * This routine can only be called be called from an ISR at the same or - * higher level as the DDA or dwell ISR. A software interrupt has been - * provided to allow a non-ISR to request a load (see st_request_load_move()) - * - * In aline() code: - * - All axes must set steps and compensate for out-of-range pulse phasing. - * - If axis has 0 steps the direction setting can be omitted - * - If axis has 0 steps the motor must not be enabled to support power mode = 1 - */ -static void _load_move() { - // Be aware that dda_ticks_downcount must equal zero for the loader to run. - // So the initial load must also have this set to zero as part of initialization - if (st_runtime_isbusy()) return; // exit if the runtime is busy - if (st_pre.buffer_state != PREP_BUFFER_OWNED_BY_LOADER) return; // if there are no moves to load... - - // handle aline loads first (most common case) - if (st_pre.move_type == MOVE_TYPE_ALINE) { - //**** setup the new segment **** - st_run.dda_ticks_downcount = st_pre.dda_ticks; - st_run.dda_ticks_X_substeps = st_pre.dda_ticks_X_substeps; - - //**** MOTOR_1 LOAD **** +static inline void _load_motor_move(int motor) { + stRunMotor_t *run_mot = &st_run.mot[motor]; + stPrepMotor_t *prep_mot = &st_pre.mot[motor]; + cfgMotor_t *cfg_mot = &st_cfg.mot[motor]; - // These sections are somewhat optimized for execution speed. The whole load operation - // is supposed to take < 10 uSec (Xmega). Be careful if you mess with this. + // Set or zero runtime substep increment + run_mot->substep_increment = prep_mot->substep_increment; - // the following if() statement sets the runtime substep increment value or zeroes it - if ((st_run.mot[MOTOR_1].substep_increment = st_pre.mot[MOTOR_1].substep_increment) != 0) { - // NB: If motor has 0 steps the following is all skipped. This ensures that state comparisons - // always operate on the last segment actually run by this motor, regardless of how many - // segments it may have been inactive in between. + if (run_mot->substep_increment) { + // If motor has 0 steps the following is all skipped. This ensures that + // state comparisons always operate on the last segment actually run by + // this motor, regardless of how many segments it may have been inactive + // in between. - // Apply accumulator correction if the time base has changed since previous segment - if (st_pre.mot[MOTOR_1].accumulator_correction_flag == true) { - st_pre.mot[MOTOR_1].accumulator_correction_flag = false; - st_run.mot[MOTOR_1].substep_accumulator *= st_pre.mot[MOTOR_1].accumulator_correction; - } - - // Detect direction change and if so: - // - Set the direction bit in hardware. - // - Compensate for direction change by flipping substep accumulator value about its midpoint. - if (st_pre.mot[MOTOR_1].direction != st_pre.mot[MOTOR_1].prev_direction) { - st_pre.mot[MOTOR_1].prev_direction = st_pre.mot[MOTOR_1].direction; - st_run.mot[MOTOR_1].substep_accumulator = -(st_run.dda_ticks_X_substeps + st_run.mot[MOTOR_1].substep_accumulator); - - if (st_pre.mot[MOTOR_1].direction == DIRECTION_CW) - PORT_MOTOR_1_VPORT.OUT &= ~DIRECTION_BIT_bm; - else PORT_MOTOR_1_VPORT.OUT |= DIRECTION_BIT_bm; - } - - SET_ENCODER_STEP_SIGN(MOTOR_1, st_pre.mot[MOTOR_1].step_sign); - - // Enable the stepper and start motor power management - if (st_cfg.mot[MOTOR_1].power_mode != MOTOR_DISABLED) { - PORT_MOTOR_1_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; // energize motor - st_run.mot[MOTOR_1].power_state = MOTOR_POWER_TIMEOUT_START;// set power management state - } - - } else if (st_cfg.mot[MOTOR_1].power_mode == MOTOR_POWERED_IN_CYCLE) { - // Motor has 0 steps; might need to energize motor for power mode processing - PORT_MOTOR_1_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; // energize motor - st_run.mot[MOTOR_1].power_state = MOTOR_POWER_TIMEOUT_START; + // Apply accumulator correction if the time base has changed since + // previous segment + if (prep_mot->accumulator_correction_flag) { + prep_mot->accumulator_correction_flag = false; + run_mot->substep_accumulator *= prep_mot->accumulator_correction; } - // accumulate counted steps to the step position and zero out counted steps for the segment currently being loaded - ACCUMULATE_ENCODER(MOTOR_1); - -#if (MOTORS >= 2) //**** MOTOR_2 LOAD **** - if ((st_run.mot[MOTOR_2].substep_increment = st_pre.mot[MOTOR_2].substep_increment) != 0) { - if (st_pre.mot[MOTOR_2].accumulator_correction_flag == true) { - st_pre.mot[MOTOR_2].accumulator_correction_flag = false; - st_run.mot[MOTOR_2].substep_accumulator *= st_pre.mot[MOTOR_2].accumulator_correction; - } - - if (st_pre.mot[MOTOR_2].direction != st_pre.mot[MOTOR_2].prev_direction) { - st_pre.mot[MOTOR_2].prev_direction = st_pre.mot[MOTOR_2].direction; - st_run.mot[MOTOR_2].substep_accumulator = -(st_run.dda_ticks_X_substeps + st_run.mot[MOTOR_2].substep_accumulator); - if (st_pre.mot[MOTOR_2].direction == DIRECTION_CW) - PORT_MOTOR_2_VPORT.OUT &= ~DIRECTION_BIT_bm; - else PORT_MOTOR_2_VPORT.OUT |= DIRECTION_BIT_bm; - } - - SET_ENCODER_STEP_SIGN(MOTOR_2, st_pre.mot[MOTOR_2].step_sign); - - if (st_cfg.mot[MOTOR_2].power_mode != MOTOR_DISABLED) { - PORT_MOTOR_2_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; - st_run.mot[MOTOR_2].power_state = MOTOR_POWER_TIMEOUT_START; - } - } else if (st_cfg.mot[MOTOR_2].power_mode == MOTOR_POWERED_IN_CYCLE) { - PORT_MOTOR_2_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; - st_run.mot[MOTOR_2].power_state = MOTOR_POWER_TIMEOUT_START; + // Detect direction change and if so: + // - Set the direction bit in hardware. + // - Compensate for direction change by flipping substep accumulator + // value about its midpoint. + if (prep_mot->direction != prep_mot->prev_direction) { + prep_mot->prev_direction = prep_mot->direction; + run_mot->substep_accumulator = + -(st_run.dda_ticks_X_substeps + run_mot->substep_accumulator); + + if (prep_mot->direction == DIRECTION_CW) + vports[motor]->OUT &= ~DIRECTION_BIT_bm; + else vports[motor]->OUT |= DIRECTION_BIT_bm; } - ACCUMULATE_ENCODER(MOTOR_2); -#endif - -#if (MOTORS >= 3) //**** MOTOR_3 LOAD **** - if ((st_run.mot[MOTOR_3].substep_increment = st_pre.mot[MOTOR_3].substep_increment) != 0) { - if (st_pre.mot[MOTOR_3].accumulator_correction_flag == true) { - st_pre.mot[MOTOR_3].accumulator_correction_flag = false; - st_run.mot[MOTOR_3].substep_accumulator *= st_pre.mot[MOTOR_3].accumulator_correction; - } - - if (st_pre.mot[MOTOR_3].direction != st_pre.mot[MOTOR_3].prev_direction) { - st_pre.mot[MOTOR_3].prev_direction = st_pre.mot[MOTOR_3].direction; - st_run.mot[MOTOR_3].substep_accumulator = -(st_run.dda_ticks_X_substeps + st_run.mot[MOTOR_3].substep_accumulator); - if (st_pre.mot[MOTOR_3].direction == DIRECTION_CW) - PORT_MOTOR_3_VPORT.OUT &= ~DIRECTION_BIT_bm; - else PORT_MOTOR_3_VPORT.OUT |= DIRECTION_BIT_bm; - } - - SET_ENCODER_STEP_SIGN(MOTOR_3, st_pre.mot[MOTOR_3].step_sign); + SET_ENCODER_STEP_SIGN(motor, prep_mot->step_sign); - if (st_cfg.mot[MOTOR_3].power_mode != MOTOR_DISABLED) { - PORT_MOTOR_3_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; - st_run.mot[MOTOR_3].power_state = MOTOR_POWER_TIMEOUT_START; - } - - } else if (st_cfg.mot[MOTOR_3].power_mode == MOTOR_POWERED_IN_CYCLE) { - PORT_MOTOR_3_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; - st_run.mot[MOTOR_3].power_state = MOTOR_POWER_TIMEOUT_START; + // Enable the stepper and start motor power management + if (cfg_mot->power_mode != MOTOR_DISABLED) { + vports[motor]->OUT &= ~MOTOR_ENABLE_BIT_bm; // energize motor + run_mot->power_state = MOTOR_POWER_TIMEOUT_START; // set power management } - ACCUMULATE_ENCODER(MOTOR_3); -#endif - -#if (MOTORS >= 4) //**** MOTOR_4 LOAD **** - if ((st_run.mot[MOTOR_4].substep_increment = st_pre.mot[MOTOR_4].substep_increment) != 0) { - if (st_pre.mot[MOTOR_4].accumulator_correction_flag == true) { - st_pre.mot[MOTOR_4].accumulator_correction_flag = false; - st_run.mot[MOTOR_4].substep_accumulator *= st_pre.mot[MOTOR_4].accumulator_correction; - } - - if (st_pre.mot[MOTOR_4].direction != st_pre.mot[MOTOR_4].prev_direction) { - st_pre.mot[MOTOR_4].prev_direction = st_pre.mot[MOTOR_4].direction; - st_run.mot[MOTOR_4].substep_accumulator = -(st_run.dda_ticks_X_substeps + st_run.mot[MOTOR_4].substep_accumulator); + } else if (cfg_mot->power_mode == MOTOR_POWERED_IN_CYCLE) { + // Motor has 0 steps; might need to energize motor for power mode + // processing + vports[motor]->OUT &= ~MOTOR_ENABLE_BIT_bm; // energize motor + run_mot->power_state = MOTOR_POWER_TIMEOUT_START; + } - if (st_pre.mot[MOTOR_4].direction == DIRECTION_CW) - PORT_MOTOR_4_VPORT.OUT &= ~DIRECTION_BIT_bm; - else PORT_MOTOR_4_VPORT.OUT |= DIRECTION_BIT_bm; - } + // Accumulate counted steps to the step position and zero out counted steps + // for the segment currently being loaded + ACCUMULATE_ENCODER(motor); +} - SET_ENCODER_STEP_SIGN(MOTOR_4, st_pre.mot[MOTOR_4].step_sign); - if (st_cfg.mot[MOTOR_4].power_mode != MOTOR_DISABLED) { - PORT_MOTOR_4_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; - st_run.mot[MOTOR_4].power_state = MOTOR_POWER_TIMEOUT_START; - } +/* Dequeue move and load into stepper struct + * + * This routine can only be called be called from an ISR at the same or + * higher level as the DDA or dwell ISR. A software interrupt has been + * provided to allow a non-ISR to request a load (see st_request_load_move()) + * + * In aline() code: + * - All axes must set steps and compensate for out-of-range pulse phasing. + * - If axis has 0 steps the direction setting can be omitted + * - If axis has 0 steps the motor must not be enabled to support power + * mode = 1 + */ +static void _load_move() { + // Be aware that dda_ticks_downcount must equal zero for the loader to run. + // So the initial load must also have this set to zero as part of + // initialization + if (st_runtime_isbusy()) return; // exit if the runtime is busy + if (st_pre.buffer_state != PREP_BUFFER_OWNED_BY_LOADER) + return; // if there are no moves to load... - } else if (st_cfg.mot[MOTOR_4].power_mode == MOTOR_POWERED_IN_CYCLE) { - PORT_MOTOR_4_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; - st_run.mot[MOTOR_4].power_state = MOTOR_POWER_TIMEOUT_START; - } + // Handle aline loads first (most common case) + if (st_pre.move_type == MOVE_TYPE_ALINE) { + // Setup the new segment + st_run.dda_ticks_downcount = st_pre.dda_ticks; + st_run.dda_ticks_X_substeps = st_pre.dda_ticks_X_substeps; - ACCUMULATE_ENCODER(MOTOR_4); + _load_motor_move(MOTOR_1); +#if 1 < MOTORS + _load_motor_move(MOTOR_2); #endif - -#if (MOTORS >= 5) //**** MOTOR_5 LOAD **** - if ((st_run.mot[MOTOR_5].substep_increment = st_pre.mot[MOTOR_5].substep_increment) != 0) { - if (st_pre.mot[MOTOR_5].accumulator_correction_flag == true) { - st_pre.mot[MOTOR_5].accumulator_correction_flag = false; - st_run.mot[MOTOR_5].substep_accumulator *= st_pre.mot[MOTOR_5].accumulator_correction; - } - - if (st_pre.mot[MOTOR_5].direction != st_pre.mot[MOTOR_5].prev_direction) { - st_pre.mot[MOTOR_5].prev_direction = st_pre.mot[MOTOR_5].direction; - st_run.mot[MOTOR_5].substep_accumulator = -(st_run.dda_ticks_X_substeps + st_run.mot[MOTOR_5].substep_accumulator); - - if (st_pre.mot[MOTOR_5].direction == DIRECTION_CW) - PORT_MOTOR_5_VPORT.OUT &= ~DIRECTION_BIT_bm; - else PORT_MOTOR_5_VPORT.OUT |= DIRECTION_BIT_bm; - } - - PORT_MOTOR_5_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; - st_run.mot[MOTOR_5].power_state = MOTOR_POWER_TIMEOUT_START; - SET_ENCODER_STEP_SIGN(MOTOR_5, st_pre.mot[MOTOR_5].step_sign); - - } else if (st_cfg.mot[MOTOR_5].power_mode == MOTOR_POWERED_IN_CYCLE) { - PORT_MOTOR_5_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; - st_run.mot[MOTOR_5].power_state = MOTOR_POWER_TIMEOUT_START; - } - - ACCUMULATE_ENCODER(MOTOR_5); +#if 2 < MOTORS + _load_motor_move(MOTOR_3); #endif - -#if (MOTORS >= 6) //**** MOTOR_6 LOAD **** - if ((st_run.mot[MOTOR_6].substep_increment = st_pre.mot[MOTOR_6].substep_increment) != 0) { - if (st_pre.mot[MOTOR_6].accumulator_correction_flag == true) { - st_pre.mot[MOTOR_6].accumulator_correction_flag = false; - st_run.mot[MOTOR_6].substep_accumulator *= st_pre.mot[MOTOR_6].accumulator_correction; - } - - if (st_pre.mot[MOTOR_6].direction != st_pre.mot[MOTOR_6].prev_direction) { - st_pre.mot[MOTOR_6].prev_direction = st_pre.mot[MOTOR_6].direction; - st_run.mot[MOTOR_6].substep_accumulator = -(st_run.dda_ticks_X_substeps + st_run.mot[MOTOR_6].substep_accumulator); - if (st_pre.mot[MOTOR_6].direction == DIRECTION_CW) - PORT_MOTOR_6_VPORT.OUT &= ~DIRECTION_BIT_bm; - else PORT_MOTOR_6_VPORT.OUT |= DIRECTION_BIT_bm; - } - - PORT_MOTOR_6_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; - st_run.mot[MOTOR_6].power_state = MOTOR_POWER_TIMEOUT_START; - SET_ENCODER_STEP_SIGN(MOTOR_6, st_pre.mot[MOTOR_6].step_sign); - - } else if (st_cfg.mot[MOTOR_6].power_mode == MOTOR_POWERED_IN_CYCLE) { - PORT_MOTOR_6_VPORT.OUT &= ~MOTOR_ENABLE_BIT_bm; - st_run.mot[MOTOR_6].power_state = MOTOR_POWER_TIMEOUT_START; - } - - ACCUMULATE_ENCODER(MOTOR_6); +#if 3 < MOTORS + _load_motor_move(MOTOR_4); #endif - //**** do this last **** + // do this last TIMER_DDA.PER = st_pre.dda_period; - TIMER_DDA.CTRLA = STEP_TIMER_ENABLE; // enable the DDA timer + TIMER_DDA.CTRLA = STEP_TIMER_ENABLE; // enable the DDA timer } else if (st_pre.move_type == MOVE_TYPE_DWELL) { // handle dwells st_run.dda_ticks_downcount = st_pre.dda_ticks; - TIMER_DWELL.PER = st_pre.dda_period; // load dwell timer period - TIMER_DWELL.CTRLA = STEP_TIMER_ENABLE; // enable the dwell timer + TIMER_DWELL.PER = st_pre.dda_period; // load dwell timer period + TIMER_DWELL.CTRLA = STEP_TIMER_ENABLE; // enable the dwell timer } else if (st_pre.move_type == MOVE_TYPE_COMMAND) // handle synchronous commands @@ -638,46 +468,58 @@ static void _load_move() { // all other cases drop to here (e.g. Null moves after Mcodes skip to here) st_pre.move_type = MOVE_TYPE_0; - st_pre.buffer_state = PREP_BUFFER_OWNED_BY_EXEC; // we are done with the prep buffer - flip the flag back - st_request_exec_move(); // exec and prep next move + // we are done with the prep buffer - flip the flag back + st_pre.buffer_state = PREP_BUFFER_OWNED_BY_EXEC; + st_request_exec_move(); // exec and prep next move } -/*********************************************************************************** - * st_prep_line() - Prepare the next move for the loader +/* Prepare the next move for the loader * - * This function does the math on the next pulse segment and gets it ready for - * the loader. It deals with all the DDA optimizations and timer setups so that - * loading can be performed as rapidly as possible. It works in joint space - * (motors) and it works in steps, not length units. All args are provided as - * floats and converted to their appropriate integer types for the loader. + * This function does the math on the next pulse segment and gets it ready for + * the loader. It deals with all the DDA optimizations and timer setups so that + * loading can be performed as rapidly as possible. It works in joint space + * (motors) and it works in steps, not length units. All args are provided as + * floats and converted to their appropriate integer types for the loader. * * Args: - * - travel_steps[] are signed relative motion in steps for each motor. Steps are - * floats that typically have fractional values (fractional steps). The sign - * indicates direction. Motors that are not in the move should be 0 steps on input. + * - travel_steps[] are signed relative motion in steps for each motor. Steps + * are floats that typically have fractional values (fractional steps). The + * sign indicates direction. Motors that are not in the move should be 0 + * steps on input. * - * - following_error[] is a vector of measured errors to the step count. Used for correction. + * - following_error[] is a vector of measured errors to the step count. Used + * for correction. * - * - segment_time - how many minutes the segment should run. If timing is not - * 100% accurate this will affect the move velocity, but not the distance traveled. + * - segment_time - how many minutes the segment should run. If timing is not + * 100% accurate this will affect the move velocity, but not the distance + * traveled. * - * NOTE: Many of the expressions are sensitive to casting and execution order to avoid long-term - * accuracy errors due to floating point round off. One earlier failed attempt was: - * dda_ticks_X_substeps = (int32_t)((microseconds / 1000000) * f_dda * dda_substeps); + * NOTE: Many of the expressions are sensitive to casting and execution order to + * avoid long-term accuracy errors due to floating point round off. One earlier + * failed attempt was: + * dda_ticks_X_substeps = + * (int32_t)((microseconds / 1000000) * f_dda * dda_substeps); */ -stat_t st_prep_line(float travel_steps[], float following_error[], float segment_time) { - // trap conditions that would prevent queueing the line - if (st_pre.buffer_state != PREP_BUFFER_OWNED_BY_EXEC) return cm_hard_alarm(STAT_INTERNAL_ERROR); - else if (isinf(segment_time)) return cm_hard_alarm(STAT_PREP_LINE_MOVE_TIME_IS_INFINITE); // never supposed to happen - else if (isnan(segment_time)) return cm_hard_alarm(STAT_PREP_LINE_MOVE_TIME_IS_NAN); // never supposed to happen +stat_t st_prep_line(float travel_steps[], float following_error[], + float segment_time) { + // Trap conditions that would prevent queueing the line + if (st_pre.buffer_state != PREP_BUFFER_OWNED_BY_EXEC) + return cm_hard_alarm(STAT_INTERNAL_ERROR); + else if (isinf(segment_time)) // never supposed to happen + return cm_hard_alarm(STAT_PREP_LINE_MOVE_TIME_IS_INFINITE); + else if (isnan(segment_time)) // never supposed to happen + return cm_hard_alarm(STAT_PREP_LINE_MOVE_TIME_IS_NAN); else if (segment_time < EPSILON) return STAT_MINIMUM_TIME_MOVE; // setup segment parameters - // - dda_ticks is the integer number of DDA clock ticks needed to play out the segment - // - ticks_X_substeps is the maximum depth of the DDA accumulator (as a negative number) + // - dda_ticks is the integer number of DDA clock ticks needed to play out the + // segment + // - ticks_X_substeps is the maximum depth of the DDA accumulator (as a + // negative number) st_pre.dda_period = _f_to_period(FREQUENCY_DDA); - st_pre.dda_ticks = (int32_t)(segment_time * 60 * FREQUENCY_DDA);// NB: converts minutes to seconds + // converts minutes to seconds + st_pre.dda_ticks = (int32_t)(segment_time * 60 * FREQUENCY_DDA); st_pre.dda_ticks_X_substeps = st_pre.dda_ticks * DDA_SUBSTEPS; // setup motor parameters @@ -689,8 +531,9 @@ stat_t st_prep_line(float travel_steps[], float following_error[], float segment } // Setup the direction, compensating for polarity. - // Set the step_sign which is used by the stepper ISR to accumulate step position - if (travel_steps[motor] >= 0) { // positive direction + // Set the step_sign which is used by the stepper ISR to accumulate step + // position + if (0 <= travel_steps[motor]) { // positive direction st_pre.mot[motor].direction = DIRECTION_CW ^ st_cfg.mot[motor].polarity; st_pre.mot[motor].step_sign = 1; @@ -699,13 +542,16 @@ stat_t st_prep_line(float travel_steps[], float following_error[], float segment st_pre.mot[motor].step_sign = -1; } - // Detect segment time changes and setup the accumulator correction factor and flag. - // Putting this here computes the correct factor even if the motor was dormant for some - // number of previous moves. Correction is computed based on the last segment time actually used. - if (fabs(segment_time - st_pre.mot[motor].prev_segment_time) > 0.0000001) { // highly tuned FP != compare - if (fp_NOT_ZERO(st_pre.mot[motor].prev_segment_time)) { // special case to skip first move + // Detect segment time changes and setup the accumulator correction factor + // and flag. Putting this here computes the correct factor even if the motor + // was dormant for some number of previous moves. Correction is computed + // based on the last segment time actually used. + if (fabs(segment_time - st_pre.mot[motor].prev_segment_time) > 0.0000001) { + // special case to skip first move + if (fp_NOT_ZERO(st_pre.mot[motor].prev_segment_time)) { st_pre.mot[motor].accumulator_correction_flag = true; - st_pre.mot[motor].accumulator_correction = segment_time / st_pre.mot[motor].prev_segment_time; + st_pre.mot[motor].accumulator_correction = + segment_time / st_pre.mot[motor].prev_segment_time; } st_pre.mot[motor].prev_segment_time = segment_time; @@ -714,7 +560,8 @@ stat_t st_prep_line(float travel_steps[], float following_error[], float segment #ifdef __STEP_CORRECTION float correction_steps; - // 'Nudge' correction strategy. Inject a single, scaled correction value then hold off + // 'Nudge' correction strategy. Inject a single, scaled correction value + // then hold off if ((--st_pre.mot[motor].correction_holdoff < 0) && (fabs(following_error[motor]) > STEP_CORRECTION_THRESHOLD)) { @@ -722,23 +569,27 @@ stat_t st_prep_line(float travel_steps[], float following_error[], float segment correction_steps = following_error[motor] * STEP_CORRECTION_FACTOR; if (correction_steps > 0) - correction_steps = min3(correction_steps, fabs(travel_steps[motor]), STEP_CORRECTION_MAX); - else correction_steps = max3(correction_steps, -fabs(travel_steps[motor]), -STEP_CORRECTION_MAX); + correction_steps = min3(correction_steps, fabs(travel_steps[motor]), + STEP_CORRECTION_MAX); + else correction_steps = max3(correction_steps, -fabs(travel_steps[motor]), + -STEP_CORRECTION_MAX); st_pre.mot[motor].corrected_steps += correction_steps; travel_steps[motor] -= correction_steps; } #endif - // Compute substeb increment. The accumulator must be *exactly* the incoming - // fractional steps times the substep multiplier or positional drift will occur. - // Rounding is performed to eliminate a negative bias in the uint32 conversion - // that results in long-term negative drift. (fabs/round order doesn't matter) - st_pre.mot[motor].substep_increment = round(fabs(travel_steps[motor] * DDA_SUBSTEPS)); + // Compute substeb increment. The accumulator must be *exactly* the + // incoming fractional steps times the substep multiplier or positional + // drift will occur. Rounding is performed to eliminate a negative bias + // in the uint32 conversion that results in long-term negative drift. + // (fabs/round order doesn't matter) + st_pre.mot[motor].substep_increment = + round(fabs(travel_steps[motor] * DDA_SUBSTEPS)); } st_pre.move_type = MOVE_TYPE_ALINE; - st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // signal that prep buffer is ready + st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // signal prep buffer ready return STAT_OK; } @@ -747,7 +598,7 @@ stat_t st_prep_line(float travel_steps[], float following_error[], float segment /// Keeps the loader happy. Otherwise performs no action void st_prep_null() { st_pre.move_type = MOVE_TYPE_0; - st_pre.buffer_state = PREP_BUFFER_OWNED_BY_EXEC; // signal that prep buffer is empty + st_pre.buffer_state = PREP_BUFFER_OWNED_BY_EXEC; // signal prep buffer empty } @@ -755,7 +606,7 @@ void st_prep_null() { void st_prep_command(void *bf) { st_pre.move_type = MOVE_TYPE_COMMAND; st_pre.bf = (mpBuf_t *)bf; - st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // signal that prep buffer is ready + st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // signal prep buffer ready } @@ -764,7 +615,7 @@ void st_prep_dwell(float microseconds) { st_pre.move_type = MOVE_TYPE_DWELL; st_pre.dda_period = _f_to_period(FREQUENCY_DWELL); st_pre.dda_ticks = (uint32_t)(microseconds / 1000000 * FREQUENCY_DWELL); - st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // signal that prep buffer is ready + st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // signal prep buffer ready } diff --git a/src/stepper.h b/src/stepper.h index ddb14bd..1a81d7f 100644 --- a/src/stepper.h +++ b/src/stepper.h @@ -5,426 +5,474 @@ * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. * Copyright (c) 2013 - 2015 Robert Giseburt * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . + * + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. + * + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +#ifndef STEPPER_H +#define STEPPER_H + /* - * Coordinated motion (line drawing) is performed using a classic - * Bresenham DDA. A number of additional steps are taken to - * optimize interpolation and pulse train timing accuracy to - * minimize pulse jitter and make for very smooth motion and - * surface finish. - * - * - The DDA is not used as a 'ramp' for acceleration - * management. Accel is computed upstream in the motion planner - * as 3rd order (controlled jerk) equations. These generate - * accel/decel segments that are passed to the DDA for step - * output. - * - * - The DDA accepts and processes fractional motor steps as - * floating point numbers from the planner. Steps do not need - * to be whole numbers, and are not expected to be. The step - * values are converted to integer by multiplying by a - * fixed-point precision (DDA_SUBSTEPS, 100000). Rounding is - * performed to avoid a truncation bias. - * - * - Constant Rate DDA clock: The DDA runs at a constant, maximum - * rate for every segment regardless of actual step rate - * required. This means that the DDA clock is not tuned to the - * step rate (or a multiple) of the major axis, as is typical - * for most DDAs. Running the DDA flat out might appear to be - * "wasteful", but it ensures that the best aliasing results - * are achieved, and is part of maintaining step accuracy - * across motion segments. - * - * The observation is that TinyG is a hard real-time system in - * which every clock cycle is knowable and can be accounted - * for. So if the system is capable of sustaining max pulse - * rate for the fastest move, it's capable of sustaining this - * rate for any move. So we just run it flat out and get the - * best pulse resolution for all moves. If we were running - * from batteries or otherwise cared about the energy budget we - * might not be so cavalier about this. - * - * At 50 KHz constant clock rate we have 20 uSec between pulse - * timer (DDA) interrupts. On the Xmega we consume <10 uSec in - * the interrupt - a whopping 50% of available cycles going - * into pulse generation. - * - * - Pulse timing is also helped by minimizing the time spent - * loading the next move segment. The time budget for the load - * is less than the time remaining before the next DDA clock - * tick. This means that the load must take < 10 uSec or the - * time between pulses will stretch out when changing - * segments. This does not affect positional accuracy but it - * would affect jitter and smoothness. To this end as much as - * possible about that move is pre-computed during move - * execution (prep cycles). Also, all moves are loaded from - * the DDA interrupt level (HI), avoiding the need for mutual - * exclusion locking or volatiles (which slow things down). + * Coordinated motion (line drawing) is performed using a classic + * Bresenham DDA. A number of additional steps are taken to + * optimize interpolation and pulse train timing accuracy to + * minimize pulse jitter and make for very smooth motion and + * surface finish. + * + * - The DDA is not used as a 'ramp' for acceleration + * management. Accel is computed upstream in the motion planner + * as 3rd order (controlled jerk) equations. These generate + * accel/decel segments that are passed to the DDA for step + * output. + * + * - The DDA accepts and processes fractional motor steps as + * floating point numbers from the planner. Steps do not need + * to be whole numbers, and are not expected to be. The step + * values are converted to integer by multiplying by a + * fixed-point precision (DDA_SUBSTEPS, 100000). Rounding is + * performed to avoid a truncation bias. + * + * - Constant Rate DDA clock: The DDA runs at a constant, maximum + * rate for every segment regardless of actual step rate + * required. This means that the DDA clock is not tuned to the + * step rate (or a multiple) of the major axis, as is typical + * for most DDAs. Running the DDA flat out might appear to be + * "wasteful", but it ensures that the best aliasing results + * are achieved, and is part of maintaining step accuracy + * across motion segments. + * + * The observation is that TinyG is a hard real-time system in + * which every clock cycle is knowable and can be accounted + * for. So if the system is capable of sustaining max pulse + * rate for the fastest move, it's capable of sustaining this + * rate for any move. So we just run it flat out and get the + * best pulse resolution for all moves. If we were running + * from batteries or otherwise cared about the energy budget we + * might not be so cavalier about this. + * + * At 50 KHz constant clock rate we have 20 uSec between pulse + * timer (DDA) interrupts. On the Xmega we consume <10 uSec in + * the interrupt - a whopping 50% of available cycles going + * into pulse generation. + * + * - Pulse timing is also helped by minimizing the time spent + * loading the next move segment. The time budget for the load + * is less than the time remaining before the next DDA clock + * tick. This means that the load must take < 10 uSec or the + * time between pulses will stretch out when changing + * segments. This does not affect positional accuracy but it + * would affect jitter and smoothness. To this end as much as + * possible about that move is pre-computed during move + * execution (prep cycles). Also, all moves are loaded from + * the DDA interrupt level (HI), avoiding the need for mutual + * exclusion locking or volatiles (which slow things down). */ -/* -**** Move generation overview and timing illustration **** -* -* This ASCII art illustrates a 4 segment move to show stepper sequencing timing. -* -* LOAD/STEP (~5000uSec) [L1][segment1][L2][segment2][L3][segment3][L4][segment4][Lb1] -* PREP (100 uSec) [P1] [P2] [P3] [P4] [Pb1] -* EXEC (400 uSec) [EXEC1] [EXEC2] [EXEC3] [EXEC4] [EXECb1] -* PLAN (<4ms) [planmoveA][plan move B][plan move C][plan move D][plan move E] etc. -* -* The move begins with the planner PLANning move A [planmoveA]. When this is done the -* computations for the first segment of move A's S curve are performed by the planner -* runtime, EXEC1. The runtime computes the number of segments and the segment-by-segment -* accelerations and decelerations for the move. Each call to EXEC generates the values -* for the next segment to be run. Once the move is running EXEC is executed as a -* callback from the step loader. -* -* When the runtime calculations are done EXEC calls the segment PREParation function [P1]. -* PREP turns the EXEC results into values needed for the loader and does some encoder work. -* The combined exec and prep take about 400 uSec. -* -* PREP takes care of heavy numerics and other cycle-intesive operations so the step loader -* L1 can run as fast as possible. The time budget for LOAD is about 10 uSec. In the diagram, -* when P1 is done segment 1 is loaded into the stepper runtime [L1] -* -* Once the segment is loaded it will pulse out steps for the duration of the segment. -* Segment timing can vary, but segments take around 5 Msec to pulse out, which is 250 DDA -* ticks at a 50 KHz step clock. -* -* Now the move is pulsing out segment 1 (at HI interrupt level). Once the L1 loader is -* finished it invokes the exec function for the next segment (at LO interrupt level). -* [EXEC2] and [P2] compute and prepare the segment 2 for the loader so it can be loaded -* as soon as segment 1 is complete [L2]. When move A is done EXEC pulls the next move -* (moveB) from the planner queue, The process repeats until there are no more segments or moves. -* -* While all this is happening subsequent moves (B, C, and D) are being planned in background. -* As long as a move takes less than the segment times (5ms x N) the timing budget is satisfied, -* -* A few things worth noting: -* - This scheme uses 2 interrupt levels and background, for 3 levels of execution: -* - STEP pulsing and LOADs occur at HI interrupt level -* - EXEC and PREP occur at LO interrupt level (leaving MED int level for serial IO) -* - move PLANning occurs in background and is managed by the controller -* -* - Because of the way the timing is laid out there is no contention for resources between -* the STEP, LOAD, EXEC, and PREP phases. PLANing is similarly isolated. Very few volatiles -* or mutexes are needed, which makes the code simpler and faster. With the exception of -* the actual values used in step generation (which runs continuously) you can count on -* LOAD, EXEC, PREP and PLAN not stepping on each other's variables. -*/ - -/**** Line planning and execution (in more detail) **** - * - * Move planning, execution and pulse generation takes place at 3 levels: - * - * Move planning occurs in the main-loop. The canonical machine calls the planner to - * generate lines, arcs, dwells, synchronous stop/starts, and any other cvommand that - * needs to be syncronized wsith motion. The planner module generates blocks (bf's) - * that hold parameters for lines and the other move types. The blocks are backplanned - * to join lines and to take dwells and stops into account. ("plan" stage). - * - * Arc movement is planned above the line planner. The arc planner generates short - * lines that are passed to the line planner. - * - * Once lines are planned the must be broken up into "segments" of about 5 milliseconds - * to be run. These segments are how S curves are generated. This is the job of the move - * runtime (aka. exec or mr). - * - * Move execution and load prep takes place at the LOW interrupt level. Move execution - * generates the next acceleration, cruise, or deceleration segment for planned lines, - * or just transfers parameters needed for dwells and stops. This layer also prepares - * segments for loading by pre-calculating the values needed by the DDA and converting - * the segment into parameters that can be directly loaded into the steppers ("exec" - * and "prep" stages). - * - * Pulse train generation takes place at the HI interrupt level. The stepper DDA fires - * timer interrupts that generate the stepper pulses. This level also transfers new - * stepper parameters once each pulse train ("segment") is complete ("load" and "run" stages). +/* Move generation overview and timing illustration + * + * This ASCII art illustrates a 4 segment move to show stepper sequencing + * timing. + * + * LOAD/STEP (~5000uSec) [L1][segment1][L2][segment2][L3] + * PREP (100 uSec) [P1] [P2] [P3] + * EXEC (400 uSec) [EXEC1] [EXEC2] [EXEC3] + * PLAN (<4ms) [planmoveA][plan move B][plan move C][plan move D] etc. + * + * The move begins with the planner PLANning move A + * [planmoveA]. When this is done the computations for the first + * segment of move A's S curve are performed by the planner runtime, + * EXEC1. The runtime computes the number of segments and the + * segment-by-segment accelerations and decelerations for the + * move. Each call to EXEC generates the values for the next segment + * to be run. Once the move is running EXEC is executed as a + * callback from the step loader. + * + * When the runtime calculations are done EXEC calls the segment + * PREParation function [P1]. PREP turns the EXEC results into + * values needed for the loader and does some encoder work. The + * combined exec and prep take about 400 uSec. + * + * PREP takes care of heavy numerics and other cycle-intesive + * operations so the step loader L1 can run as fast as possible. The + * time budget for LOAD is about 10 uSec. In the diagram, when P1 is + * done segment 1 is loaded into the stepper runtime [L1] + * + * Once the segment is loaded it will pulse out steps for the + * duration of the segment. Segment timing can vary, but segments + * take around 5 Msec to pulse out, which is 250 DDA ticks at a 50 + * KHz step clock. + * + * Now the move is pulsing out segment 1 (at HI interrupt + * level). Once the L1 loader is finished it invokes the exec + * function for the next segment (at LO interrupt level). [EXEC2] + * and [P2] compute and prepare the segment 2 for the loader so it + * can be loaded as soon as segment 1 is complete [L2]. When move A + * is done EXEC pulls the next move (moveB) from the planner queue, + * The process repeats until there are no more segments or moves. + * + * While all this is happening subsequent moves (B, C, and D) are + * being planned in background. As long as a move takes less than + * the segment times (5ms x N) the timing budget is satisfied, + * + * A few things worth noting: + * - This scheme uses 2 interrupt levels and background, for 3 + * levels of execution: + * - STEP pulsing and LOADs occur at HI interrupt level + * - EXEC and PREP occur at LO interrupt level (leaving MED int + * level for serial IO) + * - move PLANning occurs in background and is managed by the controller + * + * - Because of the way the timing is laid out there is no contention + * for resources between the STEP, LOAD, EXEC, and PREP phases. PLANing + * is similarly isolated. Very few volatiles or mutexes are needed, which + * makes the code simpler and faster. With the exception of the actual + * values used in step generation (which runs continuously) you can count + * on LOAD, EXEC, PREP and PLAN not stepping on each other's variables. */ -/* What happens when the pulse generator is done with the current pulse train (segment) - * is a multi-stage "pull" queue that looks like this: - * - * As long as the steppers are running the sequence of events is: - * - * - The stepper interrupt (HI) runs the DDA to generate a pulse train for the - * current move. This runs for the length of the pulse train currently executing - * - the "segment", usually 5ms worth of pulses - * - * - When the current segment is finished the stepper interrupt LOADs the next segment - * from the prep buffer, reloads the timers, and starts the next segment. At the end - * of the load the stepper interrupt routine requests an "exec" of the next move in - * order to prepare for the next load operation. It does this by calling the exec - * using a software interrupt (actually a timer, since that's all we've got). - * - * - As a result of the above, the EXEC handler fires at the LO interrupt level. It - * computes the next accel/decel or cruise (body) segment for the current move - * (i.e. the move in the planner's runtime buffer) by calling back to the exec - * routine in planner.c. If there are no more segments to run for the move the - * exec first gets the next buffer in the planning queue and begins execution. - * - * In some cases the mext "move" is not actually a move, but a dewll, stop, IO - * operation (e.g. M5). In this case it executes the requested operation, and may - * attempt to get the next buffer from the planner when its done. - * - * - Once the segment has been computed the exec handler finshes up by running the - * PREP routine in stepper.c. This computes the DDA values and gets the segment - * into the prep buffer - and ready for the next LOAD operation. - * - * - The main loop runs in background to receive gcode blocks, parse them, and send - * them to the planner in order to keep the planner queue full so that when the - * planner's runtime buffer completes the next move (a gcode block or perhaps an - * arc segment) is ready to run. - * - * If the steppers are not running the above is similar, except that the exec is - * invoked from the main loop by the software interrupt, and the stepper load is - * invoked from the exec by another software interrupt. +/* Line planning and execution (in more detail) + * + * Move planning, execution and pulse generation takes place at 3 + * levels: + * + * Move planning occurs in the main-loop. The canonical machine calls + * the planner to generate lines, arcs, dwells, synchronous + * stop/starts, and any other cvommand that needs to be syncronized + * wsith motion. The planner module generates blocks (bf's) that hold + * parameters for lines and the other move types. The blocks are + * backplanned to join lines and to take dwells and stops into + * account. ("plan" stage). + * + * Arc movement is planned above the line planner. The arc planner + * generates short lines that are passed to the line planner. + * + * Once lines are planned the must be broken up into "segments" of + * about 5 milliseconds to be run. These segments are how S curves are + * generated. This is the job of the move runtime (aka. exec or mr). + * + * Move execution and load prep takes place at the LOW interrupt + * level. Move execution generates the next acceleration, cruise, or + * deceleration segment for planned lines, or just transfers + * parameters needed for dwells and stops. This layer also prepares + * segments for loading by pre-calculating the values needed by the + * DDA and converting the segment into parameters that can be directly + * loaded into the steppers ("exec" and "prep" stages). + * + * Pulse train generation takes place at the HI interrupt level. The + * stepper DDA fires timer interrupts that generate the stepper + * pulses. This level also transfers new stepper parameters once each + * pulse train ("segment") is complete ("load" and "run" stages). */ -/* Control flow can be a bit confusing. This is a typical sequence for planning - * executing, and running an acceleration planned line: +/* What happens when the pulse generator is done with the current pulse train + * (segment) is a multi-stage "pull" queue that looks like this: + * + * As long as the steppers are running the sequence of events is: + * + * - The stepper interrupt (HI) runs the DDA to generate a pulse train for + * the current move. This runs for the length of the pulse train currently + * executing + * - the "segment", usually 5ms worth of pulses + * + * - When the current segment is finished the stepper interrupt LOADs the + * next segment from the prep buffer, reloads the timers, and starts the + * next segment. At the of the load the stepper interrupt routine requests + * an "exec" of the next move in order to prepare for the next load + * operation. It does this by calling the exec using a software interrupt + * (actually a timer, since that's all we've got). + * + * - As a result of the above, the EXEC handler fires at the LO interrupt + * level. It computes the next accel/decel or cruise (body) segment for the + * current move (i.e. the move in the planner's runtime buffer) by calling + * back to the exec routine in planner.c. If there are no more segments to + * run for the move the exec first gets the next buffer in the planning + * queue and begins execution. + * + * In some cases the mext "move" is not actually a move, but a dewll, stop, + * IO operation (e.g. M5). In this case it executes the requested operation, + * and may attempt to get the next buffer from the planner when its done. + * + * - Once the segment has been computed the exec handler finshes up by + * running the PREP routine in stepper.c. This computes the DDA values and + * gets the segment into the prep buffer - and ready for the next LOAD + * operation. + * + * - The main loop runs in background to receive gcode blocks, parse them, + * and send them to the planner in order to keep the planner queue full so + * that when the planner's runtime buffer completes the next move (a gcode + * block or perhaps an arc segment) is ready to run. + * + * If the steppers are not running the above is similar, except that the exec is + * invoked from the main loop by the software interrupt, and the stepper load is + * invoked from the exec by another software interrupt. + */ + +/* Control flow can be a bit confusing. This is a typical sequence for planning + * executing, and running an acceleration planned line: * - * 1 planner.mp_aline() is called, which populates a planning buffer (bf) - * and back-plans any pre-existing buffers. + * 1 planner.mp_aline() is called, which populates a planning buffer (bf) + * and back-plans any pre-existing buffers. * - * 2 When a new buffer is added _mp_queue_write_buffer() tries to invoke - * execution of the move by calling stepper.st_request_exec_move(). + * 2 When a new buffer is added _mp_queue_write_buffer() tries to invoke + * execution of the move by calling stepper.st_request_exec_move(). * - * 3a If the steppers are running this request is ignored. - * 3b If the steppers are not running this will set a timer to cause an - * EXEC "software interrupt" that will ultimately call st_exec_move(). + * 3a If the steppers are running this request is ignored. + * 3b If the steppers are not running this will set a timer to cause an + * EXEC "software interrupt" that will ultimately call st_exec_move(). * - * 4 At this point a call to _exec_move() is made, either by the - * software interrupt from 3b, or once the steppers finish running - * the current segment and have loaded the next segment. In either - * case the call is initated via the EXEC software interrupt which - * causes _exec_move() to run at the MEDium interupt level. + * 4 At this point a call to _exec_move() is made, either by the + * software interrupt from 3b, or once the steppers finish running + * the current segment and have loaded the next segment. In either + * case the call is initated via the EXEC software interrupt which + * causes _exec_move() to run at the MEDium interupt level. * - * 5 _exec_move() calls back to planner.mp_exec_move() which generates - * the next segment using the mr singleton. + * 5 _exec_move() calls back to planner.mp_exec_move() which generates + * the next segment using the mr singleton. * - * 6 When this operation is complete mp_exec_move() calls the appropriate - * PREP routine in stepper.c to derive the stepper parameters that will - * be needed to run the move - in this example st_prep_line(). + * 6 When this operation is complete mp_exec_move() calls the appropriate + * PREP routine in stepper.c to derive the stepper parameters that will + * be needed to run the move - in this example st_prep_line(). * - * 7 st_prep_line() generates the timer and DDA values and stages these into - * the prep structure (sp) - ready for loading into the stepper runtime struct + * 7 st_prep_line() generates the timer and DDA values and stages these into + * the prep structure (sp) - ready for loading into the stepper runtime + * struct * - * 8 stepper.st_prep_line() returns back to planner.mp_exec_move(), which - * frees the planning buffer (bf) back to the planner buffer pool if the - * move is complete. This is done by calling _mp_request_finalize_run_buffer() + * 8 stepper.st_prep_line() returns back to planner.mp_exec_move(), which + * frees the planning buffer (bf) back to the planner buffer pool if the + * move is complete. This is done by calling + * _mp_request_finalize_run_buffer() * - * 9 At this point the MED interrupt is complete, but the planning buffer has - * not actually been returned to the pool yet. The buffer will be returned - * by the main-loop prior to testing for an available write buffer in order - * to receive the next Gcode block. This handoff prevents possible data - * conflicts between the interrupt and main loop. + * 9 At this point the MED interrupt is complete, but the planning buffer has + * not actually been returned to the pool yet. The buffer will be returned + * by the main-loop prior to testing for an available write buffer in order + * to receive the next Gcode block. This handoff prevents possible data + * conflicts between the interrupt and main loop. * - * 10 The final step in the sequence is _load_move() requesting the next - * segment to be executed and prepared by calling st_request_exec() - * - control goes back to step 4. + * 10 The final step in the sequence is _load_move() requesting the next + * segment to be executed and prepared by calling st_request_exec() + * - control goes back to step 4. * - * Note: For this to work you have to be really careful about what structures - * are modified at what level, and use volatiles where necessary. + * Note: For this to work you have to be really careful about what structures + * are modified at what level, and use volatiles where necessary. */ /* Partial steps and phase angle compensation * - * The DDA accepts partial steps as input. Fractional steps are managed by the - * sub-step value as explained elsewhere. The fraction initially loaded into - * the DDA and the remainder left at the end of a move (the "residual") can - * be thought of as a phase angle value for the DDA accumulation. Each 360 - * degrees of phase angle results in a step being generated. + * The DDA accepts partial steps as input. Fractional steps are managed by the + * sub-step value as explained elsewhere. The fraction initially loaded into + * the DDA and the remainder left at the end of a move (the "residual") can + * be thought of as a phase angle value for the DDA accumulation. Each 360 + * degrees of phase angle results in a step being generated. */ -#ifndef STEPPER_H -#define STEPPER_H - #include "config.h" #include "status.h" -// See hardware.h for platform specific stepper definitions enum prepBufferState { - PREP_BUFFER_OWNED_BY_LOADER = 0, // staging buffer is ready for load - PREP_BUFFER_OWNED_BY_EXEC // staging buffer is being loaded + PREP_BUFFER_OWNED_BY_LOADER = 0, // staging buffer is ready for load + PREP_BUFFER_OWNED_BY_EXEC // staging buffer is being loaded }; // Currently there is no distinction between IDLE and OFF (DEENERGIZED) // In the future IDLE will be powered at a low, torque-maintaining current -enum motorPowerState { // used w/start and stop flags to sequence motor power - MOTOR_OFF = 0, // motor stopped and deenergized - MOTOR_IDLE, // motor stopped and may be partially energized - MOTOR_RUNNING, // motor is running (and fully energized) - MOTOR_POWER_TIMEOUT_START, // transitional state to start power-down timeout - MOTOR_POWER_TIMEOUT_COUNTDOWN // count down the time to de-energizing motors +// Used w/start and stop flags to sequence motor power +enum motorPowerState { + MOTOR_OFF = 0, // motor stopped and deenergized + MOTOR_IDLE, // motor stopped and may be partially energized + MOTOR_RUNNING, // motor is running (and fully energized) + MOTOR_POWER_TIMEOUT_START, // transition state to start power-down timeout + MOTOR_POWER_TIMEOUT_COUNTDOWN // count down the time to de-energizing motors }; enum cmMotorPowerMode { - MOTOR_DISABLED = 0, // motor enable is deactivated - MOTOR_ALWAYS_POWERED, // motor is always powered while machine is ON - MOTOR_POWERED_IN_CYCLE, // motor fully powered during cycles, de-powered out of cycle - MOTOR_POWERED_ONLY_WHEN_MOVING, // idles shortly after it's stopped - even in cycle - MOTOR_POWER_MODE_MAX_VALUE // for input range checking + MOTOR_DISABLED = 0, // motor enable is deactivated + MOTOR_ALWAYS_POWERED, // motor is always powered while machine is ON + MOTOR_POWERED_IN_CYCLE, // motor fully powered during cycles, + // de-powered out of cycle + MOTOR_POWERED_ONLY_WHEN_MOVING, // idles shortly after stopped, even in cycle + MOTOR_POWER_MODE_MAX_VALUE // for input range checking }; -// Min/Max timeouts allowed for motor disable. Allow for inertial stop; must be non-zero -#define MOTOR_TIMEOUT_SECONDS_MIN (float)0.1 // seconds !!! SHOULD NEVER BE ZERO !!! -#define MOTOR_TIMEOUT_SECONDS_MAX (float)4294967 // (4294967295/1000) -- for conversion to uint32_t -#define MOTOR_TIMEOUT_SECONDS (float)0.1 // seconds in DISABLE_AXIS_WHEN_IDLE mode -#define MOTOR_TIMEOUT_WHEN_MOVING (float)0.25 // timeout for a motor in _ONLY_WHEN_MOVING mode +/// Min/Max timeouts allowed for motor disable. Allow for inertial stop. +/// Must be non-zero +#define MOTOR_TIMEOUT_SECONDS_MIN (float)0.1 +/// For conversion to uint32_t (4294967295/1000) +#define MOTOR_TIMEOUT_SECONDS_MAX (float)4294967 +/// seconds in DISABLE_AXIS_WHEN_IDLE mode +#define MOTOR_TIMEOUT_SECONDS (float)0.1 +/// timeout for a motor in _ONLY_WHEN_MOVING mode +#define MOTOR_TIMEOUT_WHEN_MOVING (float)0.25 /* DDA substepping - * DDA Substepping is a fixed.point scheme to increase the resolution of the DDA pulse generation - * while still using integer math (as opposed to floating point). Improving the accuracy of the DDA - * results in more precise pulse timing and therefore less pulse jitter and smoother motor operation. - * - * The DDA accumulator is an int32_t, so the accumulator has the number range of about 2.1 billion. - * The DDA_SUBSTEPS is used to multiply step count for a segment to maximally use this number range. - * DDA_SUBSTEPS can be computed for a given DDA clock rate and segment time not to exceed available - * number range. Variables are: * - * MAX_LONG == 2^31, maximum signed long (depth of accumulator. NB: values are negative) - * FREQUENCY_DDA == DDA clock rate in Hz. - * NOM_SEGMENT_TIME == upper bound of segment time in minutes - * 0.90 == a safety factor used to reduce the result from theoretical maximum - * - * The number is about 8.5 million for the Xmega running a 50 KHz DDA with 5 millisecond segments + * DDA Substepping is a fixed.point scheme to increase the resolution + * of the DDA pulse generation while still using integer math (as + * opposed to floating point). Improving the accuracy of the DDA + * results in more precise pulse timing and therefore less pulse + * jitter and smoother motor operation. + * + * The DDA accumulator is an int32_t, so the accumulator has the + * number range of about 2.1 billion. The DDA_SUBSTEPS is used to + * multiply step count for a segment to maximally use this number + * range. DDA_SUBSTEPS can be computed for a given DDA clock rate and + * segment time not to exceed available number range. Variables are: + * + * MAX_LONG 2^31, maximum signed long (depth of accumulator. + * values are negative) + * FREQUENCY_DDA DDA clock rate in Hz. + * NOM_SEGMENT_TIME upper bound of segment time in minutes + * 0.90 a safety factor used to reduce the result from + * theoretical maximum + * + * The number is about 8.5 million for the Xmega running a 50 KHz DDA with 5 + * millisecond segments */ -#define DDA_SUBSTEPS ((MAX_LONG * 0.90) / (FREQUENCY_DDA * (NOM_SEGMENT_TIME * 60))) +#define DDA_SUBSTEPS \ + ((MAX_LONG * 0.90) / (FREQUENCY_DDA * (NOM_SEGMENT_TIME * 60))) /* Step correction settings - * Step correction settings determine how the encoder error is fed back to correct position errors. - * Since the following_error is running 2 segments behind the current segment you have to be careful - * not to overcompensate. The threshold determines if a correction should be applied, and the factor - * is how much. The holdoff is how many segments before applying another correction. If threshold - * is too small and/or amount too large and/or holdoff is too small you may get a runaway correction - * and error will grow instead of shrink (or oscillate). + * + * Step correction settings determine how the encoder error is fed + * back to correct position errors. Since the following_error is + * running 2 segments behind the current segment you have to be + * careful not to overcompensate. The threshold determines if a + * correction should be applied, and the factor is how much. The + * holdoff is how many segments before applying another correction. If + * threshold is too small and/or amount too large and/or holdoff is + * too small you may get a runaway correction and error will grow + * instead of shrink (or oscillate). */ -#define STEP_CORRECTION_THRESHOLD (float)2.00 // magnitude of forwarding error (in steps) -#define STEP_CORRECTION_FACTOR (float)0.25 // apply to step correction for a single segment -#define STEP_CORRECTION_MAX (float)0.60 // max step correction allowed in a single segment -#define STEP_CORRECTION_HOLDOFF 5 // minimum wait between error correction +/// magnitude of forwarding error (in steps) +#define STEP_CORRECTION_THRESHOLD (float)2.00 +/// apply to step correction for a single segment +#define STEP_CORRECTION_FACTOR (float)0.25 +/// max step correction allowed in a single segment +#define STEP_CORRECTION_MAX (float)0.60 +/// minimum wait between error correction +#define STEP_CORRECTION_HOLDOFF 5 #define STEP_INITIAL_DIRECTION DIRECTION_CW /* * Stepper control structures * - * There are 5 main structures involved in stepper operations; + * There are 5 main structures involved in stepper operations; * - * data structure: found in: runs primarily at: - * mpBuffer planning buffers (bf) planner.c main loop - * mrRuntimeSingleton (mr) planner.c MED ISR - * stConfig (st_cfg) stepper.c write=bkgd, read=ISRs - * stPrepSingleton (st_pre) stepper.c MED ISR - * stRunSingleton (st_run) stepper.c HI ISR + * data structure: found in: runs primarily at: + * mpBuffer planning buffers (bf) planner.c main loop + * mrRuntimeSingleton (mr) planner.c MED ISR + * stConfig (st_cfg) stepper.c write=bkgd, read=ISRs + * stPrepSingleton (st_pre) stepper.c MED ISR + * stRunSingleton (st_run) stepper.c HI ISR * - * Care has been taken to isolate actions on these structures to the execution level - * in which they run and to use the minimum number of volatiles in these structures. - * This allows the compiler to optimize the stepper inner-loops better. + * Care has been taken to isolate actions on these structures to the execution + * level in which they run and to use the minimum number of volatiles in these + * structures. This allows the compiler to optimize the stepper inner-loops + * better. */ // Motor config structure -typedef struct cfgMotor { // per-motor configs - uint8_t motor_map; // map motor to axis - uint32_t microsteps; // microsteps to apply for each axis (ex: 8) - uint8_t polarity; // 0=normal polarity, 1=reverse motor direction - uint8_t power_mode; // See cmMotorPowerMode for enum - float step_angle; // degrees per whole step (ex: 1.8) - float travel_rev; // mm or deg of travel per motor revolution - float steps_per_unit; // microsteps per mm (or degree) of travel +typedef struct cfgMotor { // per-motor configs + uint8_t motor_map; // map motor to axis + uint32_t microsteps; // microsteps to apply for each axis (ex: 8) + uint8_t polarity; // 0=normal polarity, 1=reverse motor direction + uint8_t power_mode; // See cmMotorPowerMode for enum + float step_angle; // degrees per whole step (ex: 1.8) + float travel_rev; // mm or deg of travel per motor revolution + float steps_per_unit; // microsteps per mm (or degree) of travel } cfgMotor_t; -typedef struct stConfig { // stepper configs - float motor_power_timeout; // seconds before set to idle current (currently this is OFF) - cfgMotor_t mot[MOTORS]; // settings for motors 1-N +typedef struct stConfig { // stepper configs + float motor_power_timeout; // seconds before idle current + cfgMotor_t mot[MOTORS]; // settings for motors 1-N } stConfig_t; // Motor runtime structure. Used exclusively by step generation ISR (HI) -typedef struct stRunMotor { // one per controlled motor - uint32_t substep_increment; // total steps in axis times substeps factor - int32_t substep_accumulator; // DDA phase angle accumulator - uint8_t power_state; // state machine for managing motor power - uint32_t power_systick; // sys_tick for next motor power state transition +typedef struct stRunMotor { // one per controlled motor + uint32_t substep_increment; // total steps in axis times substeps factor + int32_t substep_accumulator; // DDA phase angle accumulator + uint8_t power_state; // state machine for managing motor power + uint32_t power_systick; // for next motor power state transition } stRunMotor_t; -typedef struct stRunSingleton { // Stepper static values and axis parameters - uint32_t dda_ticks_downcount; // tick down-counter (unscaled) - uint32_t dda_ticks_X_substeps; // ticks multiplied by scaling factor - stRunMotor_t mot[MOTORS]; // runtime motor structures +typedef struct stRunSingleton { // Stepper static values and axis parameters + uint32_t dda_ticks_downcount; // tick down-counter (unscaled) + uint32_t dda_ticks_X_substeps; // ticks multiplied by scaling factor + stRunMotor_t mot[MOTORS]; // runtime motor structures } stRunSingleton_t; // Motor prep structure. Used by exec/prep ISR (MED) and read-only during load // Must be careful about volatiles in this one typedef struct stPrepMotor { - uint32_t substep_increment; // total steps in axis times substep factor + uint32_t substep_increment; // total steps in axis times substep factor // direction and direction change - int8_t direction; // travel direction corrected for polarity - uint8_t prev_direction; // travel direction from previous segment run for this motor - int8_t step_sign; // set to +1 or -1 for encoders + int8_t direction; // travel direction corrected for polarity + uint8_t prev_direction; // travel direction from previous segment run + int8_t step_sign; // set to +1 or -1 for encoders // following error correction - int32_t correction_holdoff; // count down segments between corrections - float corrected_steps; // accumulated correction steps for the cycle (diagnostic) + int32_t correction_holdoff; // count down segments between corrections + float corrected_steps; // accumulated for cycle (diagnostic) // accumulator phase correction - float prev_segment_time; // segment time from previous segment run for this motor - float accumulator_correction; // factor for adjusting accumulator between segments - uint8_t accumulator_correction_flag;// signals accumulator needs correction + float prev_segment_time; // segment time from previous run for motor + float accumulator_correction; // factor for adjusting between segments + uint8_t accumulator_correction_flag; // signals accumulator needs correction } stPrepMotor_t; typedef struct stPrepSingleton { - volatile uint8_t buffer_state; // prep buffer state - owned by exec or loader - struct mpBuffer *bf; // static pointer to relevant buffer - uint8_t move_type; // move type - - uint16_t dda_period; // DDA or dwell clock period setting - uint32_t dda_ticks; // DDA or dwell ticks for the move - uint32_t dda_ticks_X_substeps; // DDA ticks scaled by substep factor - stPrepMotor_t mot[MOTORS]; // prep time motor structs + volatile uint8_t buffer_state; // prep buffer state - owned by exec or loader + struct mpBuffer *bf; // static pointer to relevant buffer + uint8_t move_type; // move type + + uint16_t dda_period; // DDA or dwell clock period setting + uint32_t dda_ticks; // DDA or dwell ticks for the move + uint32_t dda_ticks_X_substeps; // DDA ticks scaled by substep factor + stPrepMotor_t mot[MOTORS]; // prep time motor structs } stPrepSingleton_t; -extern stConfig_t st_cfg; // config struct is exposed. The rest are private -extern stPrepSingleton_t st_pre; // used by planner +extern stConfig_t st_cfg; // used by kienmatics.c +extern stPrepSingleton_t st_pre; // used by planner.c void stepper_init(); uint8_t st_runtime_isbusy(); void st_reset(); -void st_cycle_start(); -void st_cycle_end(); void st_energize_motors(); void st_deenergize_motors(); @@ -432,8 +480,9 @@ stat_t st_motor_power_callback(); void st_request_exec_move(); void st_prep_null(); -void st_prep_command(void *bf); // use a void pointer since we don't know about mpBuf_t yet) +void st_prep_command(void *bf); // void * since mpBuf_t is not visible here void st_prep_dwell(float microseconds); -stat_t st_prep_line(float travel_steps[], float following_error[], float segment_time); +stat_t st_prep_line(float travel_steps[], float following_error[], + float segment_time); #endif // STEPPER_H diff --git a/src/vars.def b/src/vars.def index 9b85c4f..f797ae1 100644 --- a/src/vars.def +++ b/src/vars.def @@ -33,7 +33,7 @@ // VAR(name, type, index, settable, default, help) VAR(pos, float, MOTORS, 0, 0, "Current axis position") -VAR(vel, float, 0, 0, 0, "Current velosity") +VAR(vel, float, 0, 0, 0, "Current velocity") VAR(ang, float, MOTORS, 1, 0, "Rotation angle in deg per full step") VAR(trvl, float, MOTORS, 1, 0, "Travel in mm per revolution") VAR(mstep, uint16_t, MOTORS, 1, 0, "Microsteps per full step") -- 2.27.0