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)
}
}
+
/*
* cm_test_soft_limits() - return error code if soft limit is exceeded
*
cm_cycle_start(); // required for homing & other cycles
mp_aline(&cm.gm); // send the move to the planner
cm_finalize_move();
+
return STAT_OK;
}
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);
}
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];
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);
// 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)
// 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
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).
-/// 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);
-}
*
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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"
#include <stdlib.h>
#include <stdio.h>
-/**** 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);
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);
cm_cycle_end();
cm.cycle_state = CYCLE_OFF;
- printf("{\"jog\":0}\n");
return STAT_OK;
}
// 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
*/
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;
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();
* 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 <http://www.gnu.org/licenses/>.
- *
- * 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
+ * <http://www.gnu.org/licenses/>.
+ *
+ * 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"
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.
* 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
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();
// 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)
}
#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)) {
// 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;
}
}
- return STAT_EAGAIN; // should never get here
+ return STAT_EAGAIN;
}
#endif // __JERK_EXEC
// 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++)
// 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
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
}
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;
* 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 <http://www.gnu.org/licenses/>.
- *
- * 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
+ * <http://www.gnu.org/licenses/>.
+ *
+ * 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"
}
-/****************************************************************************************
- * 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
// 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
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.
//
// 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
//
// 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;
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;
/*
* 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)
* 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
/* 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;
// 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);
}
-/*
- * 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
}
-/*************************************************************************
- * 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:
*
* 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;
}
+/// 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);
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;
}
-/// 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;
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];
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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
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
};
-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
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
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
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;
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
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);
// 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);
* (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)
* 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.
* Various cases handled (H=head, B=body, T=tail)
*
* Requested-Fit cases
- * HBT Ve<Vt>Vx sufficient length exists for all parts (corner case: HBT')
- * HB Ve<Vt=Vx head accelerates to cruise - exits at full speed (corner case: H')
- * BT Ve=Vt>Vx enter at full speed and decelerate (corner case: T')
- * HT Ve & Vx perfect fit HT (very rare). May be symmetric or asymmetric
- * H Ve<Vx perfect fit H (common, results from planning)
- * T Ve>Vx perfect fit T (common, results from planning)
+ * HBT Ve<Vt>Vx sufficient length exists for all parts (corner case: HBT')
+ * HB Ve<Vt=Vx head accelerates to cruise - exits at full speed (corner case: H')
+ * BT Ve=Vt>Vx enter at full speed and decelerate (corner case: T')
+ * HT Ve & Vx perfect fit HT (very rare). May be symmetric or asymmetric
+ * H Ve<Vx perfect fit H (common, results from planning)
+ * T Ve>Vx 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)<Vt symmetric case. Split the length and compute Vt.
- * HT' (Ve!=Vx)<Vt asymmetric case. Find H and T by successive approximation.
- * HBT' body length < min body length - treated as an HT case
- * H' body length < min body length - subsume body into head length
- * T' body length < min body length - subsume body into tail length
+ * HT' (Ve!=Vx)<Vt asymmetric case. Find H and T by successive approximation.
+ * HBT' body length < min body length - treated as an HT case
+ * H' body length < min body length - subsume body into head length
+ * T' body length < min body length - subsume body into tail length
*
* Degraded fit cases - line is too short to satisfy both Ve and Vx
- * H" Ve<Vx Ve is degraded (velocity step). Vx is met
+ * H" Ve<Vx Ve is degraded (velocity step). Vx is met
* T" Ve>Vx Ve is degraded (velocity step). Vx is met
- * B" <short> line is very short but drawable; is treated as a body only
- * F <too short> force fit: This block is slowed down until it can be executed
+ * B" <short> line is very short but drawable; is treated as a body only
+ * F <too short> 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,
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
/*
* 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)
*
* 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)
*
* 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
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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"
#include <math.h>
#include <stdio.h>
+
stConfig_t st_cfg;
stPrepSingleton_t st_pre;
static stRunSingleton_t st_run;
static void _request_load_move();
-// handy macro
#define _f_to_period(f) (uint16_t)((float)F_CPU / (float)f)
#define MOTOR_1 0
#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
}
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;
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;
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;
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;
}
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);
}
+/// 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;
}
-/***
- 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
// 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
}
// 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;
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;
#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)) {
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;
}
/// 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
}
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
}
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
}
* 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 <http://www.gnu.org/licenses/>.
- *
- * 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
+ * <http://www.gnu.org/licenses/>.
+ *
+ * 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();
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
// 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")
projects / bbctrl-firmware / commitdiff