* OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
-/*
- *
- * This code is a loose implementation of Kramer, Proctor and Messina's
- * canonical machining functions as described in the NIST RS274/NGC v3
+/* This code is a loose implementation of Kramer, Proctor and Messina's
+ * canonical machining functions as described in the NIST RS274/NGC v3
*
- * The canonical machine is the layer between the Gcode parser and
- * the motion control code for a specific robot. It keeps state and
- * executes commands - passing the stateless commands to the motion
- * planning layer.
+ * The canonical machine is the layer between the Gcode parser and
+ * the motion control code for a specific robot. It keeps state and
+ * executes commands - passing the stateless commands to the motion
+ * planning layer.
*
* System state contexts - Gcode models
*
- * Useful reference for doing C callbacks
- * http://www.newty.de/fpt/fpt.html
- *
- * There are 3 temporal contexts for system state: - The gcode
- * model in the canonical machine (the MODEL context, held in gm) -
- * The gcode model used by the planner (PLANNER context, held in
- * bf's and mm) - The gcode model used during motion for reporting
- * (RUNTIME context, held in mr)
- *
- * It's a bit more complicated than this. The 'gm' struct contains
- * the core Gcode model context. This originates in the canonical
- * machine and is copied to each planner buffer (bf buffer) during
- * motion planning. Finally, the gm context is passed to the
- * runtime (mr) for the RUNTIME context. So at last count the Gcode
- * model exists in as many as 30 copies in the system. (1+28+1)
- *
- * Depending on the need, any one of these contexts may be called
- * for reporting or by a function. Most typically, all new commends
- * from the gcode parser work form the MODEL context, and status
- * reports pull from the RUNTIME while in motion, and from MODEL
- * when at rest. A convenience is provided in the ACTIVE_MODEL
- * pointer to point to the right context.
+ * Useful reference for doing C callbacks
+ * http://www.newty.de/fpt/fpt.html
+ *
+ * There are 3 temporal contexts for system state: - The gcode
+ * model in the canonical machine (the MODEL context, held in gm) -
+ * The gcode model used by the planner (PLANNER context, held in
+ * bf's and mm) - The gcode model used during motion for reporting
+ * (RUNTIME context, held in mr)
+ *
+ * It's a bit more complicated than this. The 'gm' struct contains
+ * the core Gcode model context. This originates in the canonical
+ * machine and is copied to each planner buffer (bf buffer) during
+ * motion planning. Finally, the gm context is passed to the
+ * runtime (mr) for the RUNTIME context. So at last count the Gcode
+ * model exists in as many as 30 copies in the system. (1+28+1)
+ *
+ * Depending on the need, any one of these contexts may be called
+ * for reporting or by a function. Most typically, all new commends
+ * from the gcode parser work form the MODEL context, and status
+ * reports pull from the RUNTIME while in motion, and from MODEL
+ * when at rest. A convenience is provided in the ACTIVE_MODEL
+ * pointer to point to the right context.
*
* Synchronizing command execution
*
- * Some gcode commands only set the MODEL state for interpretation
- * of the current Gcode block. For example,
- * cm_set_feed_rate(). This sets the MODEL so the move time is
- * properly calculated for the current (and subsequent) blocks, so
- * it's effected immediately.
+ * Some gcode commands only set the MODEL state for interpretation
+ * of the current Gcode block. For example,
+ * cm_set_feed_rate(). This sets the MODEL so the move time is
+ * properly calculated for the current (and subsequent) blocks, so
+ * it's effected immediately.
*
- * "Synchronous commands" are commands that affect the runtime need
- * to be synchronized with movement. Examples include G4 dwells,
- * program stops and ends, and most M commands. These are queued
- * into the planner queue and execute from the queue. Synchronous
- * commands work like this:
+ * "Synchronous commands" are commands that affect the runtime need
+ * to be synchronized with movement. Examples include G4 dwells,
+ * program stops and ends, and most M commands. These are queued
+ * into the planner queue and execute from the queue. Synchronous
+ * commands work like this:
*
- * - Call the cm_xxx_xxx() function which will do any input
- * validation and return an error if it detects one.
+ * - Call the cm_xxx_xxx() function which will do any input
+ * validation and return an error if it detects one.
*
- * - The cm_ function calls mp_queue_command(). Arguments are a
- * callback to the _exec_...() function, which is the runtime
- * execution routine, and any arguments that are needed by the
- * runtime. See typedef for *exec in planner.h for details
+ * - The cm_ function calls mp_queue_command(). Arguments are a
+ * callback to the _exec_...() function, which is the runtime
+ * execution routine, and any arguments that are needed by the
+ * runtime. See typedef for *exec in planner.h for details
*
- * - mp_queue_command() stores the callback and the args in a
+ * - mp_queue_command() stores the callback and the args in a
planner buffer.
*
- * - When planner execution reaches the buffer it executes the
- * callback w/ the args. Take careful note that the callback
- * executes under an interrupt, so beware of variables that may
- * need to be volatile.
+ * - When planner execution reaches the buffer it executes the
+ * callback w/ the args. Take careful note that the callback
+ * executes under an interrupt, so beware of variables that may
+ * need to be volatile.
*
- * Note: - The synchronous command execution mechanism uses 2
- * vectors in the bf buffer to store and return values for the
- * callback. It's obvious, but impractical to pass the entire bf
- * buffer to the callback as some of these commands are actually
- * executed locally and have no buffer.
+ * Note: - The synchronous command execution mechanism uses 2
+ * vectors in the bf buffer to store and return values for the
+ * callback. It's obvious, but impractical to pass the entire bf
+ * buffer to the callback as some of these commands are actually
+ * executed locally and have no buffer.
*/
#include "canonical_machine.h"
#include <math.h>
#include <stdio.h>
-cmSingleton_t cm; // canonical machine controller singleton
-// command execution callbacks from planner queue
+cmSingleton_t cm; // canonical machine controller singleton
+
+
+// Command execution callbacks from planner queue
static void _exec_offset(float *value, float *flag);
static void _exec_change_tool(float *value, float *flag);
static void _exec_select_tool(float *value, float *flag);
static void _exec_absolute_origin(float *value, float *flag);
static void _exec_program_finalize(float *value, float *flag);
-/*
- * Canonical Machine State functions
- *
- * cm_get_combined_state() - combines raw states into something a user might want to see
- * cm_get_machine_state()
- * cm_get_motion_state()
- * cm_get_cycle_state()
- * cm_get_hold_state()
- * cm_get_homing_state()
- * cm_set_motion_state() - adjusts active model pointer as well
- */
+// Canonical Machine State functions
+
+/// Combines raw states into something a user might want to see
uint8_t cm_get_combined_state() {
if (cm.cycle_state == CYCLE_OFF) cm.combined_state = cm.machine_state;
else if (cm.cycle_state == CYCLE_PROBE) cm.combined_state = COMBINED_PROBE;
if (cm.motion_state == MOTION_HOLD) cm.combined_state = COMBINED_HOLD;
}
- if (cm.machine_state == MACHINE_SHUTDOWN) cm.combined_state = COMBINED_SHUTDOWN;
+ if (cm.machine_state == MACHINE_SHUTDOWN)
+ cm.combined_state = COMBINED_SHUTDOWN;
return cm.combined_state;
}
uint8_t cm_get_homing_state() {return cm.homing_state;}
+/// Adjusts active model pointer as well
void cm_set_motion_state(uint8_t motion_state) {
cm.motion_state = motion_state;
}
-/* These getters and setters will work on any gm model with inputs:
- * MODEL (GCodeState_t *)&cm.gm // absolute pointer from canonical machine gm model
- * PLANNER (GCodeState_t *)&bf->gm // relative to buffer *bf is currently pointing to
- * RUNTIME (GCodeState_t *)&mr.gm // absolute pointer from runtime mm struct
- * ACTIVE_MODEL cm.am // active model pointer is maintained by state management
- */
-uint32_t cm_get_linenum(GCodeState_t *gcode_state) {return gcode_state->linenum;}
-uint8_t cm_get_motion_mode(GCodeState_t *gcode_state) {return gcode_state->motion_mode;}
-uint8_t cm_get_coord_system(GCodeState_t *gcode_state) {return gcode_state->coord_system;}
-uint8_t cm_get_units_mode(GCodeState_t *gcode_state) {return gcode_state->units_mode;}
-uint8_t cm_get_select_plane(GCodeState_t *gcode_state) {return gcode_state->select_plane;}
-uint8_t cm_get_path_control(GCodeState_t *gcode_state) {return gcode_state->path_control;}
-uint8_t cm_get_distance_mode(GCodeState_t *gcode_state) {return gcode_state->distance_mode;}
-uint8_t cm_get_feed_rate_mode(GCodeState_t *gcode_state) {return gcode_state->feed_rate_mode;}
+// These getters and setters will work on any gm model with inputs:
+// MODEL, PLANNER, RUNTIME, ACTIVE_MODEL
+uint32_t cm_get_linenum(GCodeState_t *gcode_state) {
+ return gcode_state->linenum;
+}
+
+
+uint8_t cm_get_motion_mode(GCodeState_t *gcode_state) {
+ return gcode_state->motion_mode;
+}
+
+
+uint8_t cm_get_coord_system(GCodeState_t *gcode_state) {
+ return gcode_state->coord_system;
+}
+
+
+uint8_t cm_get_units_mode(GCodeState_t *gcode_state) {
+ return gcode_state->units_mode;
+}
+
+
+uint8_t cm_get_select_plane(GCodeState_t *gcode_state) {
+ return gcode_state->select_plane;
+}
+
+
+uint8_t cm_get_path_control(GCodeState_t *gcode_state) {
+ return gcode_state->path_control;
+}
+
+
+uint8_t cm_get_distance_mode(GCodeState_t *gcode_state) {
+ return gcode_state->distance_mode;
+}
+
+
+uint8_t cm_get_feed_rate_mode(GCodeState_t *gcode_state) {
+ return gcode_state->feed_rate_mode;
+}
+
+
uint8_t cm_get_tool(GCodeState_t *gcode_state) {return gcode_state->tool;}
-uint8_t cm_get_spindle_mode(GCodeState_t *gcode_state) {return gcode_state->spindle_mode;}
+
+
+uint8_t cm_get_spindle_mode(GCodeState_t *gcode_state) {
+ return gcode_state->spindle_mode;
+}
+
+
uint8_t cm_get_block_delete_switch() {return cm.gmx.block_delete_switch;}
uint8_t cm_get_runtime_busy() {return mp_get_runtime_busy();}
-float cm_get_feed_rate(GCodeState_t *gcode_state) {return gcode_state->feed_rate;}
+
+float cm_get_feed_rate(GCodeState_t *gcode_state) {
+ return gcode_state->feed_rate;
+}
void cm_set_motion_mode(GCodeState_t *gcode_state, uint8_t motion_mode) {
}
-/***********************************************************************************
- * COORDINATE SYSTEMS AND OFFSETS
+/* Coordinate systems and offsets
+ *
* Functions to get, set and report coordinate systems and work offsets
* These functions are not part of the NIST defined functions
- ***********************************************************************************/
+ */
/*
* Notes on Coordinate System and Offset functions
*
- * All positional information in the canonical machine is kept as absolute coords and in
- * canonical units (mm). The offsets are only used to translate in and out of canonical form
- * during interpretation and response.
+ * All positional information in the canonical machine is kept as
+ * absolute coords and in canonical units (mm). The offsets are only
+ * used to translate in and out of canonical form during
+ * interpretation and response.
*
- * Managing the coordinate systems & offsets is somewhat complicated. The following affect offsets:
+ * Managing the coordinate systems & offsets is somewhat complicated.
+ * The following affect offsets:
* - coordinate system selected. 1-9 correspond to G54-G59
- * - absolute override: forces current move to be interpreted in machine coordinates: G53 (system 0)
- * - G92 offsets are added "on top of" the coord system offsets -- if origin_offset_enable == true
+ * - absolute override: forces current move to be interpreted in machine
+ * coordinates: G53 (system 0)
+ * - G92 offsets are added "on top of" the coord system offsets --
+ * if origin_offset_enable
* - G28 and G30 moves; these are run in absolute coordinates
*
- * The offsets themselves are considered static, are kept in cm, and are supposed to be persistent.
+ * The offsets themselves are considered static, are kept in cm, and are
+ * supposed to be persistent.
*
* To reduce complexity and data load the following is done:
- * - Full data for coordinates/offsets is only accessible by the canonical machine, not the downstream
- * - Resolved set of coord and G92 offsets, with per-move exceptions can be captured as "work_offsets"
- * - The core gcode context (gm) only knows about the active coord system and the work offsets
+ * - Full data for coordinates/offsets is only accessible by the canonical
+ * machine, not the downstream
+ * - Resolved set of coord and G92 offsets, with per-move exceptions can
+ * be captured as "work_offsets"
+ * - The core gcode context (gm) only knows about the active coord system
+ * and the work offsets
*/
-/*
- * cm_get_active_coord_offset() - return the currently active coordinate offset for an axis
+/* Return the currently active coordinate offset for an axis
*
- * Takes G5x, G92 and absolute override into account to return the active offset for this move
+ * Takes G5x, G92 and absolute override into account to return the
+ * active offset for this move
*
- * This function is typically used to evaluate and set offsets, as opposed to cm_get_work_offset()
- * which merely returns what's in the work_offset[] array.
+ * This function is typically used to evaluate and set offsets, as
+ * opposed to cm_get_work_offset() which merely returns what's in the
+ * work_offset[] array.
*/
float cm_get_active_coord_offset(uint8_t axis) {
- if (cm.gm.absolute_override == true) return 0; // no offset if in absolute override mode
+ if (cm.gm.absolute_override) return 0; // no offset in absolute override mode
float offset = cm.offset[cm.gm.coord_system][axis];
- if (cm.gmx.origin_offset_enable == true)
- offset += cm.gmx.origin_offset[axis]; // includes G5x and G92 components
+
+ if (cm.gmx.origin_offset_enable)
+ offset += cm.gmx.origin_offset[axis]; // includes G5x and G92 components
+
return offset;
}
-/*
- * cm_get_work_offset() - return a coord offset from the gcode_state
+/* Return a coord offset from the gcode_state
*
- * This function accepts as input:
- * MODEL (GCodeState_t *)&cm.gm // absolute pointer from canonical machine gm model
- * PLANNER (GCodeState_t *)&bf->gm // relative to buffer *bf is currently pointing to
- * RUNTIME (GCodeState_t *)&mr.gm // absolute pointer from runtime mm struct
- * ACTIVE_MODEL cm.am // active model pointer is maintained by state management
+ * This function accepts as input: MODEL, PLANNER, RUNTIME, ACTIVE_MODEL
*/
float cm_get_work_offset(GCodeState_t *gcode_state, uint8_t axis) {
return gcode_state->work_offset[axis];
}
-/*
- * cm_set_work_offsets() - capture coord offsets from the model into absolute values in the gcode_state
+/* Capture coord offsets from the model into absolute values in the gcode_state
*
- * This function accepts as input:
- * MODEL (GCodeState_t *)&cm.gm // absolute pointer from canonical machine gm model
- * PLANNER (GCodeState_t *)&bf->gm // relative to buffer *bf is currently pointing to
- * RUNTIME (GCodeState_t *)&mr.gm // absolute pointer from runtime mm struct
- * ACTIVE_MODEL cm.am // active model pointer maintained by state management
+ * This function accepts as input: MODEL, PLANNER, RUNTIME, ACTIVE_MODEL
*/
void cm_set_work_offsets(GCodeState_t *gcode_state) {
for (uint8_t axis = AXIS_X; axis < AXES; axis++)
}
-/*
- * Get position of axis in absolute coordinates
+/* Get position of axis in absolute coordinates
*
- * This function accepts as input:
- * MODEL (GCodeState_t *)&cm.gm // absolute pointer from canonical machine gm model
- * RUNTIME (GCodeState_t *)&mr.gm // absolute pointer from runtime mm struct
+ * This function accepts as input: MODEL, RUNTIME
*
- * NOTE: Only MODEL and RUNTIME are supported (no PLANNER or bf's)
- * NOTE: Machine position is always returned in mm mode. No units conversion is performed
+ * NOTE: Only MODEL and RUNTIME are supported (no PLANNER or bf's)
+ * NOTE: Machine position is always returned in mm mode. No units conversion
+ * is performed
*/
float cm_get_absolute_position(GCodeState_t *gcode_state, uint8_t axis) {
if (gcode_state == MODEL) return cm.gmx.position[axis];
}
-/*
- * Return work position in external form
+/* Return work position in external form
*
* ... that means in prevailing units (mm/inch) and with all offsets applied
*
- * NOTE: This function only works after the gcode_state struct as had the work_offsets setup by
- * calling cm_get_model_coord_offset_vector() first.
+ * NOTE: This function only works after the gcode_state struct as had the
+ * work_offsets setup by calling cm_get_model_coord_offset_vector() first.
*
* This function accepts as input:
- * MODEL (GCodeState_t *)&cm.gm // absolute pointer from canonical machine gm model
- * RUNTIME (GCodeState_t *)&mr.gm // absolute pointer from runtime mm struct
+ * MODEL
+ * RUNTIME
*
* NOTE: Only MODEL and RUNTIME are supported (no PLANNER or bf's)
*/
}
-/***********************************************************************************
- * CRITICAL HELPERS
+/* Critical helpers
+ *
* Core functions supporting the canonical machining functions
* These functions are not part of the NIST defined functions
- ***********************************************************************************/
+ */
-/*
- * Perform final operations for a traverse or feed
+/* Perform final operations for a traverse or feed
*
- * These routines set the point position in the gcode model.
+ * These routines set the point position in the gcode model.
*
- * Note: As far as the canonical machine is concerned the final position of a Gcode block (move)
- * is achieved as soon as the move is planned and the move target becomes the new model position.
- * In reality the planner will (in all likelihood) have only just queued the move for later
- * execution, and the real tool position is still close to the starting point.
+ * Note: As far as the canonical machine is concerned the final
+ * position of a Gcode block (move) is achieved as soon as the move is
+ * planned and the move target becomes the new model position. In
+ * reality the planner will (in all likelihood) have only just queued
+ * the move for later execution, and the real tool position is still
+ * close to the starting point.
*/
void cm_finalize_move() {
copy_vector(cm.gmx.position, cm.gm.target); // update model position
}
-/*
- * Set target vector in GM model
+/* Set target vector in GM model
*
* This is a core routine. It handles:
* - conversion of linear units to internal canonical form (mm)
* - conversion of relative mode to absolute (internal canonical form)
- * - translation of work coordinates to machine coordinates (internal canonical form)
+ * - translation of work coordinates to machine coordinates (internal
+ * canonical form)
* - computation and application of axis modes as so:
*
* DISABLED - Incoming value is ignored. Target value is not changed
* INHIBITED - Same processing as ENABLED, but axis will not actually be run
* RADIUS - ABC axis value is provided in Gcode block in linear units
* - Target is set to degrees based on axis' Radius value
- * - Radius mode is only processed for ABC axes. Application to XYZ is ignored.
+ * - Radius mode is only processed for ABC axes. Application to
+ * XYZ is ignored.
*
* Target coordinates are provided in target[]
* Axes that need processing are signaled in flag[]
*/
-// ESTEE: _calc_ABC is a fix to workaround a gcc compiler bug wherein it runs out of spill
-// registers we moved this block into its own function so that we get a fresh stack push
+// ESTEE: _calc_ABC is a fix to workaround a gcc compiler bug wherein it runs
+// out of spill registers we moved this block into its own function so that we
+// get a fresh stack push
// ALDEN: This shows up in avr-gcc 4.7.0 and avr-libc 1.8.0
static float _calc_ABC(uint8_t axis, float target[], float flag[]) {
- if ((cm.a[axis].axis_mode == AXIS_STANDARD) || (cm.a[axis].axis_mode == AXIS_INHIBITED))
+ if (cm.a[axis].axis_mode == AXIS_STANDARD ||
+ cm.a[axis].axis_mode == AXIS_INHIBITED)
return target[axis]; // no mm conversion - it's in degrees
return _to_millimeters(target[axis]) * 360 / (2 * M_PI * cm.a[axis].radius);
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) {
+ 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]);
+ cm.gm.target[axis] =
+ cm_get_active_coord_offset(axis) + _to_millimeters(target[axis]);
else cm.gm.target[axis] += _to_millimeters(target[axis]);
}
}
else tmp = _calc_ABC(axis, target, flag);
if (cm.gm.distance_mode == ABSOLUTE_MODE)
- cm.gm.target[axis] = tmp + cm_get_active_coord_offset(axis); // sacidu93's fix to Issue #22
+ // sacidu93's fix to Issue #22
+ cm.gm.target[axis] = tmp + cm_get_active_coord_offset(axis);
else cm.gm.target[axis] += tmp;
}
}
-/*
- * cm_test_soft_limits() - return error code if soft limit is exceeded
+/* Return error code if soft limit is exceeded
*
- * Must be called with target properly set in GM struct. Best done after cm_set_model_target().
+ * Must be called with target properly set in GM struct. Best done
+ * after cm_set_model_target().
*
- * Tests for soft limit for any homed axis if min and max are different values. You can set min
- * and max to 0,0 to disable soft limits for an axis. Also will not test a min or a max if the
- * value is < -1000000 (negative one million). This allows a single end to be tested w/the other
- * disabled, should that requirement ever arise.
+ * Tests for soft limit for any homed axis if min and max are
+ * different values. You can set min and max to 0,0 to disable soft
+ * limits for an axis. Also will not test a min or a max if the value
+ * is < -1000000 (negative one million). This allows a single end to
+ * be tested w/the other disabled, should that requirement ever arise.
*/
stat_t cm_test_soft_limits(float target[]) {
if (cm.soft_limit_enable)
if (fp_EQ(cm.a[axis].travel_min, cm.a[axis].travel_max)) continue;
- if ((cm.a[axis].travel_min > DISABLE_SOFT_LIMIT) && (target[axis] < cm.a[axis].travel_min))
+ if (cm.a[axis].travel_min > DISABLE_SOFT_LIMIT &&
+ target[axis] < cm.a[axis].travel_min)
return STAT_SOFT_LIMIT_EXCEEDED;
- if ((cm.a[axis].travel_max > DISABLE_SOFT_LIMIT) && (target[axis] > cm.a[axis].travel_max))
+ if (cm.a[axis].travel_max > DISABLE_SOFT_LIMIT &&
+ target[axis] > cm.a[axis].travel_max)
return STAT_SOFT_LIMIT_EXCEEDED;
}
}
-/*************************************************************************
- * CANONICAL MACHINING FUNCTIONS
+/* Canonical machining functions
* Values are passed in pre-unit_converted state (from gn structure)
* All operations occur on gm (current model state)
*
* These are organized by section number (x.x.x) in the order they are
* found in NIST RS274 NGCv3
- ************************************************************************/
+ */
-/******************************************
- * Initialization and Termination (4.3.2) *
- ******************************************/
+// Initialization and Termination (4.3.2)
-/// canonical_machine_init() - Config init cfg_init() must have been run beforehand
+/// Config init cfg_init() must have been run beforehand
void canonical_machine_init() {
- // If you can assume all memory has been zeroed by a hard reset you don't need this code:
- memset(&cm.gm, 0, sizeof(GCodeState_t)); // clear all values, pointers and status
+ // If you can assume all memory has been zeroed by a hard reset you don't
+ // need this code:
+ memset(&cm.gm, 0, sizeof(GCodeState_t));
memset(&cm.gn, 0, sizeof(GCodeInput_t));
memset(&cm.gf, 0, sizeof(GCodeInput_t));
- ACTIVE_MODEL = MODEL; // setup initial Gcode model pointer
+ ACTIVE_MODEL = MODEL; // setup initial Gcode model pointer
// axes defaults
cm.a[AXIS_X].axis_mode = X_AXIS_MODE;
return status;
}
-/**************************
- * Representation (4.3.3) *
- **************************/
+// Representation (4.3.3)
+//
+// Affect the Gcode model only (asynchronous)
+// These functions assume input validation occurred upstream.
-/**************************************************************************
- * Representation functions that affect the Gcode model only (asynchronous)
- *
- * cm_select_plane() - G17,G18,G19 select axis plane
- * cm_set_units_mode() - G20, G21
- * cm_set_distance_mode() - G90, G91
- * cm_set_coord_offsets() - G10 (delayed persistence)
- *
- * These functions assume input validation occurred upstream.
- */
+/// G17, G18, G19 select axis plane
stat_t cm_select_plane(uint8_t plane) {
cm.gm.select_plane = plane;
return STAT_OK;
}
+/// G20, G21
stat_t cm_set_units_mode(uint8_t mode) {
cm.gm.units_mode = mode; // 0 = inches, 1 = mm.
return STAT_OK;
}
+/// G90, G91
stat_t cm_set_distance_mode(uint8_t mode) {
cm.gm.distance_mode = mode; // 0 = absolute mode, 1 = incremental
return STAT_OK;
}
-/*
- * cm_set_coord_offsets() - G10 L2 Pn (affects MODEL only)
+/* G10 L2 Pn, affects MODEL only, delayed persistence
*
* This function applies the offset to the GM model. You can also
* use $g54x - $g59c config functions to change offsets.
*/
stat_t cm_set_coord_offsets(uint8_t coord_system, float offset[],
float flag[]) {
- if ((coord_system < G54) || (coord_system > COORD_SYSTEM_MAX))
+ if (coord_system < G54 || coord_system > COORD_SYSTEM_MAX)
return STAT_INPUT_VALUE_RANGE_ERROR; // you can't set G53
for (uint8_t axis = AXIS_X; axis < AXES; axis++)
// Representation functions that affect gcode model and are queued to planner
// (synchronous)
-/*
- * cm_set_coord_system() - G54-G59
- * _exec_offset() - callback from planner
- */
+
+/// G54-G59
stat_t cm_set_coord_system(uint8_t coord_system) {
cm.gm.coord_system = coord_system;
static void _exec_offset(float *value, float *flag) {
- uint8_t coord_system = ((uint8_t)value[0]); // coordinate system is passed in value[0] element
+ // coordinate system is passed in value[0] element
+ uint8_t coord_system = ((uint8_t)value[0]);
float offsets[AXES];
for (uint8_t axis = AXIS_X; axis < AXES; axis++)
- offsets[axis] = cm.offset[coord_system][axis] + (cm.gmx.origin_offset[axis] * cm.gmx.origin_offset_enable);
+ offsets[axis] = cm.offset[coord_system][axis] +
+ (cm.gmx.origin_offset[axis] * cm.gmx.origin_offset_enable);
mp_set_runtime_work_offset(offsets);
- cm_set_work_offsets(MODEL); // set work offsets in the Gcode model
+ cm_set_work_offsets(MODEL); // set work offsets in the Gcode model
}
-/*
- * cm_set_position() - set the position of a single axis in the model, planner and runtime
+/* Set the position of a single axis in the model, planner and runtime
*
- * This command sets an axis/axes to a position provided as an argument.
- * This is useful for setting origins for homing, probing, and other operations.
+ * This command sets an axis/axes to a position provided as an argument.
+ * This is useful for setting origins for homing, probing, and other operations.
*
- * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
* !!!!! DO NOT CALL THIS FUNCTION WHILE IN A MACHINING CYCLE !!!!!
- * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
*
- * More specifically, do not call this function if there are any moves in the planner or
- * if the runtime is moving. The system must be quiescent or you will introduce positional
- * errors. This is true because the planned / running moves have a different reference frame
- * than the one you are now going to set. These functions should only be called during
- * initialization sequences and during cycles (such as homing cycles) when you know there
- * are no more moves in the planner and that all motion has stopped.
- * Use cm_get_runtime_busy() to be sure the system is quiescent.
+ * More specifically, do not call this function if there are any moves
+ * in the planner or if the runtime is moving. The system must be
+ * quiescent or you will introduce positional errors. This is true
+ * because the planned / running moves have a different reference
+ * frame than the one you are now going to set. These functions should
+ * only be called during initialization sequences and during cycles
+ * (such as homing cycles) when you know there are no more moves in
+ * the planner and that all motion has stopped. Use
+ * cm_get_runtime_busy() to be sure the system is quiescent.
*/
void cm_set_position(uint8_t axis, float position) {
// TODO: Interlock involving runtime_busy test
}
-/*** G28.3 functions and support ***
- *
- * cm_set_absolute_origin() - G28.3 - model, planner and queue to runtime
- * _exec_absolute_origin() - callback from planner
+// G28.3 functions and support
+
+/* G28.3 - model, planner and queue to runtime
*
- * cm_set_absolute_origin() takes a vector of origins (presumably 0's, but not necessarily)
- * and applies them to all axes where the corresponding position in the flag vector is true (1).
+ * Takes a vector of origins (presumably 0's, but not necessarily) and
+ * applies them to all axes where the corresponding position in the
+ * flag vector is true (1).
*
- * This is a 2 step process. The model and planner contexts are set immediately, the runtime
- * command is queued and synchronized with the planner queue. This includes the runtime position
- * and the step recording done by the encoders. At that point any axis that is set is also marked
- * as homed.
+ * This is a 2 step process. The model and planner contexts are set
+ * immediately, the runtime command is queued and synchronized with
+ * the planner queue. This includes the runtime position and the step
+ * recording done by the encoders. At that point any axis that is set
+ * is also marked as homed.
*/
stat_t cm_set_absolute_origin(float origin[], float flag[]) {
float value[AXES];
for (uint8_t axis = AXIS_X; axis < AXES; axis++)
if (fp_TRUE(flag[axis])) {
mp_set_runtime_position(axis, value[axis]);
- cm.homed[axis] = true; // G28.3 is not considered homed until you get here
+ cm.homed[axis] = true; // G28.3 is not considered homed until here
}
mp_set_steps_to_runtime_position();
}
-/*
- * cm_set_origin_offsets() - G92
- * cm_reset_origin_offsets() - G92.1
- * cm_suspend_origin_offsets() - G92.2
- * cm_resume_origin_offsets() - G92.3
- *
- * G92's behave according to NIST 3.5.18 & LinuxCNC G92
+/* G92's behave according to NIST 3.5.18 & LinuxCNC G92
* http://linuxcnc.org/docs/html/gcode/gcode.html#sec:G92-G92.1-G92.2-G92.3
*/
+
+/// G92
stat_t cm_set_origin_offsets(float offset[], float flag[]) {
// set offsets in the Gcode model extended context
cm.gmx.origin_offset_enable = 1;
cm.gmx.origin_offset[axis] = cm.gmx.position[axis] -
cm.offset[cm.gm.coord_system][axis] - _to_millimeters(offset[axis]);
- // now pass the offset to the callback - setting the coordinate system also applies the offsets
- float value[AXES] = { (float)cm.gm.coord_system,0,0,0,0,0 }; // pass coordinate system in value[0] element
- mp_queue_command(_exec_offset, value, value); // second vector is not used
+ // now pass the offset to the callback - setting the coordinate system also
+ // applies the offsets
+ // pass coordinate system in value[0] element
+ float value[AXES] = {cm.gm.coord_system};
+ mp_queue_command(_exec_offset, value, value); // second vector is not used
return STAT_OK;
}
+/// G92.1
stat_t cm_reset_origin_offsets() {
cm.gmx.origin_offset_enable = 0;
for (uint8_t axis = AXIS_X; axis < AXES; axis++)
cm.gmx.origin_offset[axis] = 0;
- float value[AXES] = { (float)cm.gm.coord_system,0,0,0,0,0 };
+ float value[AXES] = {cm.gm.coord_system};
mp_queue_command(_exec_offset, value, value);
return STAT_OK;
}
+/// G92.2
stat_t cm_suspend_origin_offsets() {
cm.gmx.origin_offset_enable = 0;
- float value[AXES] = { (float)cm.gm.coord_system,0,0,0,0,0 };
+ float value[AXES] = {cm.gm.coord_system};
mp_queue_command(_exec_offset, value, value);
return STAT_OK;
}
+/// G92.3
stat_t cm_resume_origin_offsets() {
cm.gmx.origin_offset_enable = 1;
- float value[AXES] = { (float)cm.gm.coord_system,0,0,0,0,0 };
+ float value[AXES] = {cm.gm.coord_system};
mp_queue_command(_exec_offset, value, value);
return STAT_OK;
}
-/*****************************
- * Free Space Motion (4.3.4) *
- *****************************/
+// Free Space Motion (4.3.4)
/// G0 linear rapid
stat_t cm_straight_traverse(float target[], float flags[]) {
if (status != STAT_OK) return cm_soft_alarm(status);
// prep and plan the move
- cm_set_work_offsets(&cm.gm); // capture the fully resolved offsets to the state
- cm_cycle_start(); // required for homing & other cycles
- mp_aline(&cm.gm); // send the move to the planner
+ cm_set_work_offsets(&cm.gm); // capture fully resolved offsets to the state
+ cm_cycle_start(); // required for homing & other cycles
+ mp_aline(&cm.gm); // send the move to the planner
cm_finalize_move();
return STAT_OK;
}
-/*
- * cm_set_g28_position() - G28.1
- * cm_goto_g28_position() - G28
- * cm_set_g30_position() - G30.1
- * cm_goto_g30_position() - G30
- */
+/// G28.1
stat_t cm_set_g28_position() {
copy_vector(cm.gmx.g28_position, cm.gmx.position);
return STAT_OK;
}
+/// G28
stat_t cm_goto_g28_position(float target[], float flags[]) {
cm_set_absolute_override(MODEL, true);
- cm_straight_traverse(target, flags); // move through intermediate point, or skip
- while (mp_get_planner_buffers_available() == 0); // make sure you have an available buffer
+ // move through intermediate point, or skip
+ cm_straight_traverse(target, flags);
+
+ // make sure you have an available buffer
+ while (!mp_get_planner_buffers_available());
+
+ // execute actual stored move
float f[] = {1, 1, 1, 1, 1, 1};
- return cm_straight_traverse(cm.gmx.g28_position, f); // execute actual stored move
+ return cm_straight_traverse(cm.gmx.g28_position, f);
}
+/// G30.1
stat_t cm_set_g30_position() {
copy_vector(cm.gmx.g30_position, cm.gmx.position);
return STAT_OK;
}
+/// G30
stat_t cm_goto_g30_position(float target[], float flags[]) {
cm_set_absolute_override(MODEL, true);
- cm_straight_traverse(target, flags); // move through intermediate point, or skip
- while (mp_get_planner_buffers_available() == 0); // make sure you have an available buffer
+ // move through intermediate point, or skip
+ cm_straight_traverse(target, flags);
+ // make sure you have an available buffer
+ while (!mp_get_planner_buffers_available());
float f[] = {1, 1, 1, 1, 1, 1};
- return cm_straight_traverse(cm.gmx.g30_position, f); // execute actual stored move
+ // execute actual stored move
+ return cm_straight_traverse(cm.gmx.g30_position, f);
}
-/********************************
- * Machining Attributes (4.3.5) *
- ********************************/
-/*
- * cm_set_feed_rate() - F parameter (affects MODEL only)
- *
- * Normalize feed rate to mm/min or to minutes if in inverse time mode
- */
+// Machining Attributes (4.3.5)
+
+/// F parameter (affects MODEL only)
+/// Normalize feed rate to mm/min or to minutes if in inverse time mode
stat_t cm_set_feed_rate(float feed_rate) {
if (cm.gm.feed_rate_mode == INVERSE_TIME_MODE)
- cm.gm.feed_rate = 1 / feed_rate; // normalize to minutes (active for this gcode block only)
+ // normalize to minutes (active for this gcode block only)
+ cm.gm.feed_rate = 1 / feed_rate;
+
else cm.gm.feed_rate = _to_millimeters(feed_rate);
return STAT_OK;
}
-/*
- * cm_set_feed_rate_mode() - G93, G94 (affects MODEL only)
+/* G93, G94 (affects MODEL only)
*
- * INVERSE_TIME_MODE = 0, // G93
- * UNITS_PER_MINUTE_MODE, // G94
- * UNITS_PER_REVOLUTION_MODE // G95 (unimplemented)
+ * INVERSE_TIME_MODE = 0, // G93
+ * UNITS_PER_MINUTE_MODE, // G94
+ * UNITS_PER_REVOLUTION_MODE // G95 (unimplemented)
*/
stat_t cm_set_feed_rate_mode(uint8_t mode) {
cm.gm.feed_rate_mode = mode;
}
-/*******************************
- * Machining Functions (4.3.6) *
- *******************************/
-/*
- * cm_arc_feed() - SEE arc.c(pp)
- */
+// Machining Functions (4.3.6) See arc.c
/// G4, P parameter (seconds)
/// G1
stat_t cm_straight_feed(float target[], float flags[]) {
// trap zero feed rate condition
- if ((cm.gm.feed_rate_mode != INVERSE_TIME_MODE) && (fp_ZERO(cm.gm.feed_rate)))
+ if (cm.gm.feed_rate_mode != INVERSE_TIME_MODE && fp_ZERO(cm.gm.feed_rate))
return STAT_GCODE_FEEDRATE_NOT_SPECIFIED;
cm.gm.motion_mode = MOTION_MODE_STRAIGHT_FEED;
}
-/*****************************
- * Spindle Functions (4.3.7) *
- *****************************/
-// see spindle.c, spindle.h
+// Spindle Functions (4.3.7) see spindle.c, spindle.h
-/**************************
- * Tool Functions (4.3.8) *
- **************************/
-/*
- * cm_select_tool() - T parameter
- * _exec_select_tool() - execution callback
- *
- * cm_change_tool() - M6 (This might become a complete tool change cycle)
- * _exec_change_tool() - execution callback
+/* Tool Functions (4.3.8)
*
* Note: These functions don't actually do anything for now, and there's a bug
- * where T and M in different blocks don;t work correctly
+ * where T and M in different blocks don't work correctly
*/
+
+/// T parameter
stat_t cm_select_tool(uint8_t tool_select) {
- float value[AXES] = {(float)tool_select, 0, 0, 0, 0, 0};
+ float value[AXES] = {tool_select};
mp_queue_command(_exec_select_tool, value, value);
return STAT_OK;
}
}
+/// M6 This might become a complete tool change cycle
stat_t cm_change_tool(uint8_t tool_change) {
- float value[AXES] = {(float)cm.gm.tool_select, 0, 0, 0, 0, 0};
+ float value[AXES] = {cm.gm.tool_select};
mp_queue_command(_exec_change_tool, value, value);
return STAT_OK;
}
}
-/***********************************
- * Miscellaneous Functions (4.3.9) *
- ***********************************/
-/*
- * cm_mist_coolant_control() - M7
- * cm_flood_coolant_control() - M8, M9
- */
+// Miscellaneous Functions (4.3.9)
+/// M7
stat_t cm_mist_coolant_control(uint8_t mist_coolant) {
float value[AXES] = {(float)mist_coolant, 0, 0, 0, 0, 0};
mp_queue_command(_exec_mist_coolant_control, value, value);
static void _exec_mist_coolant_control(float *value, float *flag) {
cm.gm.mist_coolant = (uint8_t)value[0];
- if (cm.gm.mist_coolant == true) gpio_set_bit_on(MIST_COOLANT_BIT);
+ if (cm.gm.mist_coolant) gpio_set_bit_on(MIST_COOLANT_BIT);
else gpio_set_bit_off(MIST_COOLANT_BIT);
}
+/// M8, M9
stat_t cm_flood_coolant_control(uint8_t flood_coolant) {
- float value[AXES] = {(float)flood_coolant, 0, 0, 0, 0, 0};
+ float value[AXES] = {flood_coolant};
mp_queue_command(_exec_flood_coolant_control, value, value);
return STAT_OK;
}
static void _exec_flood_coolant_control(float *value, float *flag) {
cm.gm.flood_coolant = (uint8_t)value[0];
- if (cm.gm.flood_coolant == true) gpio_set_bit_on(FLOOD_COOLANT_BIT);
+ if (cm.gm.flood_coolant) gpio_set_bit_on(FLOOD_COOLANT_BIT);
else {
gpio_set_bit_off(FLOOD_COOLANT_BIT);
- float vect[] = { 0,0,0,0,0,0 }; // turn off mist coolant
- _exec_mist_coolant_control(vect, vect); // M9 special function
+ float vect[] = {}; // turn off mist coolant
+ _exec_mist_coolant_control(vect, vect); // M9 special function
}
}
-/*
- * cm_override_enables() - M48, M49
- * cm_feed_rate_override_enable() - M50
- * cm_feed_rate_override_factor() - M50.1
- * cm_traverse_override_enable() - M50.2
- * cm_traverse_override_factor() - M50.3
- * cm_spindle_override_enable() - M51
- * cm_spindle_override_factor() - M51.1
- *
- * Override enables are kind of a mess in Gcode. This is an attempt to sort them out.
- * See http://www.linuxcnc.org/docs/2.4/html/gcode_main.html#sec:M50:-Feed-Override
+/* Override enables are kind of a mess in Gcode. This is an attempt to sort
+ * them out. See
+ * http://www.linuxcnc.org/docs/2.4/html/gcode_main.html#sec:M50:-Feed-Override
*/
-stat_t cm_override_enables(uint8_t flag) { // M48, M49
+
+/// M48, M49
+stat_t cm_override_enables(uint8_t flag) {
cm.gmx.feed_rate_override_enable = flag;
cm.gmx.traverse_override_enable = flag;
cm.gmx.spindle_override_enable = flag;
}
-stat_t cm_feed_rate_override_enable(uint8_t flag) { // M50
+/// M50
+stat_t cm_feed_rate_override_enable(uint8_t flag) {
if (fp_TRUE(cm.gf.parameter) && fp_ZERO(cm.gn.parameter))
cm.gmx.feed_rate_override_enable = false;
else cm.gmx.feed_rate_override_enable = true;
}
-stat_t cm_feed_rate_override_factor(uint8_t flag) { // M50.1
+/// M50.1
+stat_t cm_feed_rate_override_factor(uint8_t flag) {
cm.gmx.feed_rate_override_enable = flag;
cm.gmx.feed_rate_override_factor = cm.gn.parameter;
return STAT_OK;
}
-stat_t cm_traverse_override_enable(uint8_t flag) { // M50.2
+/// M50.2
+stat_t cm_traverse_override_enable(uint8_t flag) {
if (fp_TRUE(cm.gf.parameter) && fp_ZERO(cm.gn.parameter))
cm.gmx.traverse_override_enable = false;
else cm.gmx.traverse_override_enable = true;
}
-stat_t cm_traverse_override_factor(uint8_t flag) { // M51
+/// M51
+stat_t cm_traverse_override_factor(uint8_t flag) {
cm.gmx.traverse_override_enable = flag;
cm.gmx.traverse_override_factor = cm.gn.parameter;
return STAT_OK;
}
-stat_t cm_spindle_override_enable(uint8_t flag) { // M51.1
+/// M51.1
+stat_t cm_spindle_override_enable(uint8_t flag) {
if (fp_TRUE(cm.gf.parameter) && fp_ZERO(cm.gn.parameter))
cm.gmx.spindle_override_enable = false;
else cm.gmx.spindle_override_enable = true;
}
-stat_t cm_spindle_override_factor(uint8_t flag) { // M50.1
+/// M50.1
+stat_t cm_spindle_override_factor(uint8_t flag) {
cm.gmx.spindle_override_enable = flag;
cm.gmx.spindle_override_factor = cm.gn.parameter;
}
-void cm_message(char *message) {
- printf(message);
-}
+void cm_message(char *message) {printf(message);}
-/******************************
- * Program Functions (4.3.10) *
- ******************************/
-/*
+/* Program Functions (4.3.10)
+ *
* This group implements stop, start, end, and hold.
* It is extended beyond the NIST spec to handle various situations.
*
- * _exec_program_finalize()
- * cm_cycle_start() (no Gcode) Do a cycle start right now
- * cm_cycle_end() (no Gcode) Do a cycle end right now
- * cm_feedhold() (no Gcode) Initiate a feedhold right now
- * cm_program_stop() (M0, M60) Queue a program stop
- * cm_optional_program_stop() (M1)
- * cm_program_end() (M2, M30)
- * cm_machine_ready() puts machine into a READY state
- *
* cm_program_stop and cm_optional_program_stop are synchronous Gcode
* commands that are received through the interpreter. They cause all motion
* to stop at the end of the current command, including spindle motion.
* cm_program_end is a stop that also resets the machine to initial state
*/
-/*
- * cm_request_feedhold()
- * cm_request_queue_flush()
- * cm_request_cycle_start()
- * cm_feedhold_sequencing_callback() - process feedholds, cycle starts & queue flushes
- * cm_flush_planner() - Flush planner queue and correct model positions
- *
- * Feedholds, queue flushes and cycles starts are all related. The request functions set
- * flags for these. The sequencing callback interprets the flags according to the
- * following rules:
- *
- * A feedhold request received during motion should be honored
- * A feedhold request received during a feedhold should be ignored and reset
- * A feedhold request received during a motion stop should be ignored and reset
- *
- * A queue flush request received during motion should be ignored but not reset
- * A queue flush request received during a feedhold should be deferred until
- * the feedhold enters a HOLD state (i.e. until deceleration is complete)
- * A queue flush request received during a motion stop should be honored
- *
- * A cycle start request received during motion should be ignored and reset
- * A cycle start request received during a feedhold should be deferred until
- * the feedhold enters a HOLD state (i.e. until deceleration is complete)
- * If a queue flush request is also present the queue flush should be done first
- * A cycle start request received during a motion stop should be honored and
- * should start to run anything in the planner queue
+/* Feedholds, queue flushes and cycles starts are all related. The request
+ * functions set flags for these. The sequencing callback interprets the flags
+ * according to the following rules:
+ *
+ * A feedhold request received during motion should be honored
+ * A feedhold request received during a feedhold should be ignored and reset
+ * A feedhold request received during a motion stop should be ignored and
+ * reset
+ *
+ * A queue flush request received during motion should be ignored but not
+ * reset
+ * A queue flush request received during a feedhold should be deferred until
+ * the feedhold enters a HOLD state (i.e. until deceleration is complete)
+ * A queue flush request received during a motion stop should be honored
+ *
+ * A cycle start request received during motion should be ignored and reset
+ * A cycle start request received during a feedhold should be deferred until
+ * the feedhold enters a HOLD state (i.e. until deceleration is complete)
+ * If a queue flush request is also present the queue flush should be done
+ * first
+ * A cycle start request received during a motion stop should be honored and
+ * should start to run anything in the planner queue
*/
+
+/// Initiate a feedhold right now
void cm_request_feedhold() {cm.feedhold_requested = true;}
void cm_request_queue_flush() {cm.queue_flush_requested = true;}
void cm_request_cycle_start() {cm.cycle_start_requested = true;}
+/// Process feedholds, cycle starts & queue flushes
stat_t cm_feedhold_sequencing_callback() {
- if (cm.feedhold_requested == true) {
- if ((cm.motion_state == MOTION_RUN) && (cm.hold_state == FEEDHOLD_OFF)) {
+ if (cm.feedhold_requested) {
+ if (cm.motion_state == MOTION_RUN && cm.hold_state == FEEDHOLD_OFF) {
cm_set_motion_state(MOTION_HOLD);
cm.hold_state = FEEDHOLD_SYNC; // invokes hold from aline execution
}
cm.feedhold_requested = false;
}
- if (cm.queue_flush_requested == true) {
- if (((cm.motion_state == MOTION_STOP) ||
- ((cm.motion_state == MOTION_HOLD) && (cm.hold_state == FEEDHOLD_HOLD))) &&
+ if (cm.queue_flush_requested) {
+ if ((cm.motion_state == MOTION_STOP ||
+ (cm.motion_state == MOTION_HOLD && cm.hold_state == FEEDHOLD_HOLD)) &&
!cm_get_runtime_busy()) {
cm.queue_flush_requested = false;
cm_queue_flush();
}
}
- bool feedhold_processing = // added feedhold processing lockout from omco fork
+ bool feedhold_processing = // added feedhold processing lockout from omco fork
cm.hold_state == FEEDHOLD_SYNC ||
cm.hold_state == FEEDHOLD_PLAN ||
cm.hold_state == FEEDHOLD_DECEL;
- if ((cm.cycle_start_requested == true) && (cm.queue_flush_requested == false) && !feedhold_processing) {
+ if (cm.cycle_start_requested && !cm.queue_flush_requested &&
+ !feedhold_processing) {
cm.cycle_start_requested = false;
cm.hold_state = FEEDHOLD_END_HOLD;
cm_cycle_start();
stat_t cm_queue_flush() {
- if (cm_get_runtime_busy() == true) return STAT_COMMAND_NOT_ACCEPTED;
+ if (cm_get_runtime_busy()) return STAT_COMMAND_NOT_ACCEPTED;
usart_rx_flush(); // flush serial queues
mp_flush_planner(); // flush planner queue
// Note: The following uses low-level mp calls for absolute position.
// It could also use cm_get_absolute_position(RUNTIME, axis);
for (uint8_t axis = AXIS_X; axis < AXES; axis++)
- cm_set_position(axis, mp_get_runtime_absolute_position(axis)); // set mm from mr
+ // set mm from mr
+ cm_set_position(axis, mp_get_runtime_absolute_position(axis));
- float value[AXES] = {(float)MACHINE_PROGRAM_STOP, 0,0,0,0,0};
+ float value[AXES] = {MACHINE_PROGRAM_STOP};
_exec_program_finalize(value, value); // finalize now, not later
return STAT_OK;
}
-/*
- * Program and cycle state functions
- *
- * _exec_program_finalize() - helper
- * cm_cycle_start()
- * cm_cycle_end()
- * cm_program_stop() - M0
- * cm_optional_program_stop() - M1
- * cm_program_end() - M2, M30
+/* Program and cycle state functions
*
* cm_program_end() implements M2 and M30
* The END behaviors are defined by NIST 3.6.1 are:
- * 1. Axis offsets are set to zero (like G92.2) and origin offsets are set to the default (like G54)
+ * 1. Axis offsets are set to zero (like G92.2) and origin offsets are set
+ * to the default (like G54)
* 2. Selected plane is set to CANON_PLANE_XY (like G17)
* 3. Distance mode is set to MODE_ABSOLUTE (like G90)
* 4. Feed rate mode is set to UNITS_PER_MINUTE (like G94)
* 9. Coolant is turned off (like M9)
*
* cm_program_end() implments things slightly differently:
- * 1. Axis offsets are set to G92.1 CANCEL offsets (instead of using G92.2 SUSPEND Offsets)
+ * 1. Axis offsets are set to G92.1 CANCEL offsets
+ * (instead of using G92.2 SUSPEND Offsets)
* Set default coordinate system (uses $gco, not G54)
- * 2. Selected plane is set to default plane ($gpl) (instead of setting it to G54)
+ * 2. Selected plane is set to default plane ($gpl)
+ * (instead of setting it to G54)
* 3. Distance mode is set to MODE_ABSOLUTE (like G90)
* 4. Feed rate mode is set to UNITS_PER_MINUTE (like G94)
* 5. Not implemented
cm.machine_state = (uint8_t)value[0];
cm_set_motion_state(MOTION_STOP);
if (cm.cycle_state == CYCLE_MACHINING)
- cm.cycle_state = CYCLE_OFF; // don't end cycle if homing, probing, etc.
+ cm.cycle_state = CYCLE_OFF; // don't end cycle if homing, probing, etc.
- cm.hold_state = FEEDHOLD_OFF; // end feedhold (if in feed hold)
- cm.cycle_start_requested = false; // cancel any pending cycle start request
- mp_zero_segment_velocity(); // for reporting purposes
+ cm.hold_state = FEEDHOLD_OFF; // end feedhold (if in feed hold)
+ cm.cycle_start_requested = false; // cancel any pending cycle start request
+ mp_zero_segment_velocity(); // for reporting purposes
// perform the following resets if it's a program END
if (cm.machine_state == MACHINE_PROGRAM_END) {
- cm_reset_origin_offsets(); // G92.1 - we do G91.1 instead of G92.2
- cm_set_coord_system(cm.coord_system); // reset to default coordinate system
- cm_select_plane(cm.select_plane); // reset to default arc plane
+ cm_reset_origin_offsets(); // G92.1 - we do G91.1 instead of G92.2
+ cm_set_coord_system(cm.coord_system); // reset to default coordinate system
+ cm_select_plane(cm.select_plane); // reset to default arc plane
cm_set_distance_mode(cm.distance_mode);
cm_spindle_control(SPINDLE_OFF); // M5
cm_flood_coolant_control(false); // M9
}
+/// Do a cycle start right now
void cm_cycle_start() {
cm.machine_state = MACHINE_CYCLE;
}
+/// Do a cycle end right now
void cm_cycle_end() {
if (cm.cycle_state != CYCLE_OFF) {
- float value[AXES] = {(float)MACHINE_PROGRAM_STOP, 0,0,0,0,0};
+ float value[AXES] = {MACHINE_PROGRAM_STOP};
_exec_program_finalize(value, value);
}
}
+
+/// M0 Queue a program stop
void cm_program_stop() {
- float value[AXES] = {(float)MACHINE_PROGRAM_STOP, 0,0,0,0,0};
+ float value[AXES] = {MACHINE_PROGRAM_STOP};
mp_queue_command(_exec_program_finalize, value, value);
}
+/// M1
void cm_optional_program_stop() {
- float value[AXES] = {(float)MACHINE_PROGRAM_STOP, 0,0,0,0,0};
+ float value[AXES] = {MACHINE_PROGRAM_STOP};
mp_queue_command(_exec_program_finalize, value, value);
}
+/// M2, M30
void cm_program_end() {
- float value[AXES] = {(float)MACHINE_PROGRAM_END, 0,0,0,0,0};
+ float value[AXES] = {MACHINE_PROGRAM_END};
mp_queue_command(_exec_program_finalize, value, value);
}
/// return ASCII char for axis given the axis number
char cm_get_axis_char(const int8_t axis) {
char axis_char[] = "XYZABC";
- if ((axis < 0) || (axis > AXES)) return ' ';
+ if (axis < 0 || axis > AXES) return ' ';
return axis_char[axis];
}
-/**** Jerk functions
- * cm_get_axis_jerk() - returns jerk for an axis
- * cm_set_axis_jerk() - sets the jerk for an axis, including recirpcal and cached values
+/* Jerk functions
*
- * cm_set_xjm() - set jerk max value
- * cm_set_xjh() - set jerk halt value (used by homing and other stops)
+ * Jerk values can be rather large, often in the billions. This makes
+ * for some pretty big numbers for people to deal with. Jerk values
+ * are stored in the system in truncated format; values are divided by
+ * 1,000,000 then reconstituted before use.
*
- * Jerk values can be rather large, often in the billions. This makes for some pretty big
- * numbers for people to deal with. Jerk values are stored in the system in truncated format;
- * values are divided by 1,000,000 then reconstituted before use.
- *
- * The set_xjm() nad set_xjh() functions will accept either truncated or untruncated jerk
- * numbers as input. If the number is > 1,000,000 it is divided by 1,000,000 before storing.
- * Numbers are accepted in either millimeter or inch mode and converted to millimeter mode.
- *
- * The axis_jerk() functions expect the jerk in divided-by 1,000,000 form
+ * The axis_jerk() functions expect the jerk in divided-by 1,000,000 form
*/
+
+/// returns jerk for an axis
float cm_get_axis_jerk(uint8_t axis) {
return cm.a[axis].jerk_max;
}
+/// sets the jerk for an axis, including recirpcal and cached values
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);
*
* This code is a loose implementation of Kramer, Proctor and Messina's
* canonical machining functions as described in the NIST RS274/NGC v3
- */
-/* 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.
+ * 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.
+ * 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.
*/
#pragma once
#include <stdint.h>
-#define MODEL (GCodeState_t *)&cm.gm // absolute pointer from canonical machine gm model
-#define PLANNER (GCodeState_t *)&bf->gm // relative to buffer *bf is currently pointing to
-#define RUNTIME (GCodeState_t *)&mr.gm // absolute pointer from runtime mm struct
-#define ACTIVE_MODEL cm.am // active model pointer is maintained by state management
+/// absolute pointer from canonical machine gm model
+#define MODEL (GCodeState_t *)&cm.gm
+/// relative to buffer *bf is currently pointing to
+#define PLANNER (GCodeState_t *)&bf->gm
+/// absolute pointer from runtime mm struct
+#define RUNTIME (GCodeState_t *)&mr.gm
+/// active model pointer is maintained by state management
+#define ACTIVE_MODEL cm.am
-#define _to_millimeters(a) ((cm.gm.units_mode == INCHES) ? (a * MM_PER_INCH) : a)
+#define _to_millimeters(a) (cm.gm.units_mode == INCHES ? (a) * MM_PER_INCH : a)
-#define DISABLE_SOFT_LIMIT (-1000000)
+#define DISABLE_SOFT_LIMIT -1000000
-/*****************************************************************************
- * GCODE MODEL - The following GCodeModel/GCodeInput structs are used:
+/* Gcode model - The following GCodeModel/GCodeInput structs are used:
*
- * - gm is the core Gcode model state. It keeps the internal gcode state model in
- * normalized, canonical form. All values are unit converted (to mm) and in the
- * machine coordinate system (absolute coordinate system). Gm is owned by the
- * canonical machine layer and should be accessed only through cm_ routines.
+ * - gm is the core Gcode model state. It keeps the internal gcode
+ * state model in normalized, canonical form. All values are unit
+ * converted (to mm) and in the machine coordinate system
+ * (absolute coordinate system). Gm is owned by the canonical
+ * machine layer and should be accessed only through cm_ routines.
*
- * The gm core struct is copied and passed as context to the runtime where it is
- * used for planning, replanning, and reporting.
+ * The gm core struct is copied and passed as context to the
+ * runtime where it is used for planning, replanning, and
+ * reporting.
*
- * - gmx is the extended gcode model variables that are only used by the canonical
- * machine and do not need to be passed further down.
+ * - gmx is the extended gcode model variables that are only used by
+ * the canonical machine and do not need to be passed further
+ * down.
*
- * - gn is used by the gcode interpreter and is re-initialized for each
- * gcode block.It accepts data in the new gcode block in the formats
- * present in the block (pre-normalized forms). During initialization
- * some state elements are necessarily restored from gm.
+ * - gn is used by the gcode interpreter and is re-initialized for
+ * each gcode block.It accepts data in the new gcode block in the
+ * formats present in the block (pre-normalized forms). During
+ * initialization some state elements are necessarily restored
+ * from gm.
*
- * - gf is used by the gcode parser interpreter to hold flags for any data
- * that has changed in gn during the parse. cm.gf.target[] values are also used
- * by the canonical machine during set_target().
+ * - gf is used by the gcode parser interpreter to hold flags for any
+ * data that has changed in gn during the parse. cm.gf.target[]
+ * values are also used by the canonical machine during
+ * set_target().
*
- * - cfg (config struct in config.h) is also used heavily and contains some
- * values that might be considered to be Gcode model values. The distinction
- * is that all values in the config are persisted and restored, whereas the
- * gm structs are transient. So cfg has the G54 - G59 offsets, but gm has the
- * G92 offsets. cfg has the power-on / reset gcode default values, but gm has
- * the operating state for the values (which may have changed).
+ * - cfg (config struct in config.h) is also used heavily and contains
+ * some values that might be considered to be Gcode model
+ * values. The distinction is that all values in the config are
+ * persisted and restored, whereas the gm structs are
+ * transient. So cfg has the G54 - G59 offsets, but gm has the G92
+ * offsets. cfg has the power-on / reset gcode default values, but
+ * gm has the operating state for the values (which may have
+ * changed).
*/
-typedef struct GCodeState { // Gcode model state - used by model, planning and runtime
- uint32_t linenum; // Gcode block line number
- uint8_t motion_mode; // Group1: G0, G1, G2, G3, G38.2, G80, G81,
- // G82, G83 G84, G85, G86, G87, G88, G89
- float target[AXES]; // XYZABC where the move should go
- float work_offset[AXES]; // offset from the work coordinate system (for reporting only)
-
- float move_time; // optimal time for move given axis constraints
- float minimum_time; // minimum time possible for move given axis constraints
- float feed_rate; // F - normalized to millimeters/minute or in inverse time mode
- float spindle_speed; // in RPM
- float parameter; // P - parameter used for dwell time in seconds, G10 coord select...
-
- uint8_t feed_rate_mode; // See cmFeedRateMode for settings
- uint8_t select_plane; // G17,G18,G19 - values to set plane to
- uint8_t units_mode; // G20,G21 - 0=inches (G20), 1 = mm (G21)
- uint8_t coord_system; // G54-G59 - select coordinate system 1-9
- uint8_t absolute_override; // G53 TRUE = move using machine coordinates - this block only (G53)
- uint8_t path_control; // G61... EXACT_PATH, EXACT_STOP, CONTINUOUS
- uint8_t distance_mode; // G91 0=use absolute coords(G90), 1=incremental movement
- uint8_t arc_distance_mode; // G91.1 0=use absolute coords(G90), 1=incremental movement
- uint8_t tool; // M6 tool change - moves "tool_select" to "tool"
- uint8_t tool_select; // T value - T sets this value
- uint8_t mist_coolant; // TRUE = mist on (M7), FALSE = off (M9)
- uint8_t flood_coolant; // TRUE = flood on (M8), FALSE = off (M9)
- uint8_t spindle_mode; // 0=OFF (M5), 1=CW (M3), 2=CCW (M4)
+/// Gcode model state - used by model, planning and runtime
+typedef struct GCodeState {
+ uint32_t linenum; // Gcode block line number
+ uint8_t motion_mode; // Group1: G0, G1, G2, G3, G38.2, G80, G81,
+ // G82, G83 G84, G85, G86, G87, G88, G89
+ float target[AXES]; // XYZABC where the move should go
+ /// offset from the work coordinate system (for reporting only)
+ float work_offset[AXES];
+
+ float move_time; // optimal time for move given axis constraints
+ /// minimum time possible for move given axis constraints
+ float minimum_time;
+ /// F - normalized to millimeters/minute or in inverse time mode
+ float feed_rate;
+
+ float spindle_speed; // in RPM
+ /// P - parameter used for dwell time in seconds, G10 coord select...
+ float parameter;
+
+ uint8_t feed_rate_mode; // See cmFeedRateMode for settings
+ uint8_t select_plane; // G17,G18,G19 - values to set plane to
+ uint8_t units_mode; // G20,G21 - 0=inches (G20), 1 = mm (G21)
+ uint8_t coord_system; // G54-G59 - select coordinate system 1-9
+ /// G53 TRUE = move using machine coordinates - this block only (G53)
+ uint8_t absolute_override;
+ uint8_t path_control; // G61... EXACT_PATH, EXACT_STOP, CONTINUOUS
+ /// G91 0=use absolute coords(G90), 1=incremental movement
+ uint8_t distance_mode;
+ /// G91.1 0=use absolute coords(G90), 1=incremental movement
+ uint8_t arc_distance_mode;
+ uint8_t tool; // M6 tool change - moves "tool_select" to "tool"
+ uint8_t tool_select; // T value - T sets this value
+ uint8_t mist_coolant; // TRUE = mist on (M7), FALSE = off (M9)
+ uint8_t flood_coolant; // TRUE = flood on (M8), FALSE = off (M9)
+ uint8_t spindle_mode; // 0=OFF (M5), 1=CW (M3), 2=CCW (M4)
} GCodeState_t;
-typedef struct GCodeStateExtended { // Gcode dynamic state extensions - used by model and arcs
- uint8_t next_action; // handles G modal group 1 moves & non-modals
+/// Gcode dynamic state extensions - used by model and arcs
+typedef struct GCodeStateExtended {
+ /// handles G modal group 1 moves & non-modals
+ uint8_t next_action;
uint8_t program_flow; // used only by the gcode_parser
- float position[AXES]; // XYZABC model position (Note: not used in gn or gf)
- float origin_offset[AXES]; // XYZABC G92 offsets (Note: not used in gn or gf)
- float g28_position[AXES]; // XYZABC stored machine position for G28
- float g30_position[AXES]; // XYZABC stored machine position for G30
+ float position[AXES]; // model position (not used in gn or gf)
+ float origin_offset[AXES]; // G92 offsets (not used in gn or gf)
+ float g28_position[AXES]; // stored machine position for G28
+ float g30_position[AXES]; // stored machine position for G30
- float feed_rate_override_factor; // 1.0000 x F feed rate. Go up or down from there
- float traverse_override_factor; // 1.0000 x traverse rate. Go down from there
+ float feed_rate_override_factor; // 1.0000 x F feed rate.
+ float traverse_override_factor; // 1.0000 x traverse rate.
uint8_t feed_rate_override_enable; // TRUE = overrides enabled (M48), F=(M49)
uint8_t traverse_override_enable; // TRUE = traverse override enabled
uint8_t l_word; // L word - used by G10s
- uint8_t origin_offset_enable; // G92 offsets enabled/disabled. 0=disabled, 1=enabled
- uint8_t block_delete_switch; // set true to enable block deletes (true is default)
+ uint8_t origin_offset_enable; // G92 offsets enabled/disabled.
+ uint8_t block_delete_switch; // true enables block deletes (true)
- float spindle_override_factor; // 1.0000 x S spindle speed. Go up or down from there
+ float spindle_override_factor; // 1.0000 x S spindle speed.
uint8_t spindle_override_enable; // TRUE = override enabled
// unimplemented gcode parameters
- // float cutter_radius; // D - cutter radius compensation (0 is off)
- // float cutter_length; // H - cutter length compensation (0 is off)
+ // float cutter_radius; // D - cutter radius compensation (0 is off)
+ // float cutter_length; // H - cutter length compensation (0 is off)
} GCodeStateX_t;
-typedef struct GCodeInput { // Gcode model inputs - meaning depends on context
- uint8_t next_action; // handles G modal group 1 moves & non-modals
- uint8_t motion_mode; // Group1: G0, G1, G2, G3, G38.2, G80, G81,
- // G82, G83 G84, G85, G86, G87, G88, G89
+/// Gcode model inputs - meaning depends on context
+typedef struct GCodeInput {
+ /// handles G modal group 1 moves & non-modals
+ uint8_t next_action;
+ /// G0, G1, G2, G3, G38.2, G80, G81, G82, G83 G84, G85, G86, G87, G88 or G89
+ uint8_t motion_mode;
uint8_t program_flow; // used only by the gcode_parser
uint32_t linenum; // N word or autoincrement in the model
float target[AXES]; // XYZABC where the move should go
float feed_rate; // F - normalized to millimeters/minute
- float feed_rate_override_factor; // 1.0000 x F feed rate. Go up or down from there
- float traverse_override_factor; // 1.0000 x traverse rate. Go down from there
+ float feed_rate_override_factor; // 1.0000 x F feed rate.
+ float traverse_override_factor; // 1.0000 x traverse rate.
uint8_t feed_rate_mode; // See cmFeedRateMode for settings
uint8_t feed_rate_override_enable; // TRUE = overrides enabled (M48), F=(M49)
uint8_t traverse_override_enable; // TRUE = traverse override enabled
- uint8_t override_enables; // enables for feed and spoindle (GN/GF only)
+ uint8_t override_enables; // feed and spindle enable (GN/GF only)
uint8_t l_word; // L word - used by G10s
uint8_t select_plane; // G17,G18,G19 - values to set plane to
uint8_t units_mode; // G20,G21 - 0=inches (G20), 1 = mm (G21)
uint8_t coord_system; // G54-G59 - select coordinate system 1-9
- uint8_t absolute_override; // G53 TRUE = move using machine coordinates - this block only (G53)
- uint8_t origin_offset_mode; // G92...TRUE=in origin offset mode
- uint8_t path_control; // G61... EXACT_PATH, EXACT_STOP, CONTINUOUS
- uint8_t distance_mode; // G91 0=use absolute coords(G90), 1=incremental movement
- uint8_t arc_distance_mode; // G91.1 0=use absolute coords(G90), 1=incremental movement
+ uint8_t absolute_override; // G53 TRUE = move in machine coordinates
+ uint8_t origin_offset_mode; // G92 TRUE = in origin offset mode
+ uint8_t path_control; // G61 EXACT_PATH, EXACT_STOP, CONTINUOUS
+ uint8_t distance_mode; // G91 0=absolute(G90), 1=incremental
+ uint8_t arc_distance_mode; // G91.1 0=absolute(G90), 1=incremental
- uint8_t tool; // Tool after T and M6 (tool_select and tool_change)
+ uint8_t tool; // Tool after T and M6
uint8_t tool_select; // T value - T sets this value
- uint8_t tool_change; // M6 tool change flag - moves "tool_select" to "tool"
+ uint8_t tool_change; // M6 tool change flag
uint8_t mist_coolant; // TRUE = mist on (M7), FALSE = off (M9)
uint8_t flood_coolant; // TRUE = flood on (M8), FALSE = off (M9)
uint8_t spindle_mode; // 0=OFF (M5), 1=CW (M3), 2=CCW (M4)
float spindle_speed; // in RPM
- float spindle_override_factor; // 1.0000 x S spindle speed. Go up or down from there
+ float spindle_override_factor; // 1.0000 x S spindle speed.
uint8_t spindle_override_enable; // TRUE = override enabled
- float parameter; // P - parameter used for dwell time in seconds, G10 coord select...
+ float parameter; // P - dwell time in sec, G10 coord select
float arc_radius; // R - radius value in arc radius mode
float arc_offset[3]; // IJK - used by arc commands
// unimplemented gcode parameters
- // float cutter_radius; // D - cutter radius compensation (0 is off)
- // float cutter_length; // H - cutter length compensation (0 is off)
+ // float cutter_radius; // D - cutter radius compensation (0 is off)
+ // float cutter_length; // H - cutter length compensation (0 is off)
} GCodeInput_t;
typedef struct cmAxis {
- uint8_t axis_mode; // see tgAxisMode in gcode.h
- float feedrate_max; // max velocity in mm/min or deg/min
- float velocity_max; // max velocity in mm/min or deg/min
- float travel_max; // max work envelope for soft limits
- float travel_min; // min work envelope for soft limits
- float jerk_max; // max jerk (Jm) in mm/min^3 divided by 1 million
- float jerk_homing; // homing jerk (Jh) in mm/min^3 divided by 1 million
- float recip_jerk; // stored reciprocal of current jerk value - has the million in it
- float junction_dev; // aka cornering delta
- float radius; // radius in mm for rotary axis modes
- float search_velocity; // homing search velocity
- float latch_velocity; // homing latch velocity
- float latch_backoff; // backoff from switches prior to homing latch movement
- float zero_backoff; // backoff from switches for machine zero
+ uint8_t axis_mode; // see tgAxisMode in gcode.h
+ float feedrate_max; // max velocity in mm/min or deg/min
+ float velocity_max; // max velocity in mm/min or deg/min
+ float travel_max; // max work envelope for soft limits
+ float travel_min; // min work envelope for soft limits
+ float jerk_max; // max jerk (Jm) in mm/min^3 divided by 1 million
+ float jerk_homing; // homing jerk (Jh) in mm/min^3 divided by 1 million
+ float recip_jerk; // reciprocal of current jerk value - with million
+ float junction_dev; // aka cornering delta
+ float radius; // radius in mm for rotary axis modes
+ float search_velocity; // homing search velocity
+ float latch_velocity; // homing latch velocity
+ float latch_backoff; // backoff from switches prior to homing latch movement
+ float zero_backoff; // backoff from switches for machine zero
} cfgAxis_t;
typedef struct cmSingleton { // struct to manage cm globals and cycles
// Public
// system group settings
- float junction_acceleration; // centripetal acceleration max for cornering
+ float junction_acceleration; // max cornering centripetal acceleration
float chordal_tolerance; // arc chordal accuracy setting in mm
uint8_t soft_limit_enable;
// hidden system settings
float min_segment_len; // line drawing resolution in mm
float arc_segment_len; // arc drawing resolution in mm
- float estd_segment_usec; // approximate segment time in microseconds
+ float estd_segment_usec; // approximate segment time in msec
// gcode power-on default settings - defaults are not the same as the gm state
uint8_t coord_system; // G10 active coordinate system default
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
+ // persistent coordinate offsets: absolute (G53) + G54,G55,G56,G57,G58,G59
+ float offset[COORDS + 1][AXES];
// settings for axes X,Y,Z,A B,C
cfgAxis_t a[AXES];
// Private
- uint8_t combined_state; // stat: combination of states for display purposes
- uint8_t machine_state; // macs: machine/cycle/motion is the actual machine state
+ // stat: combination of states for display purposes
+ uint8_t combined_state;
+ // macs: machine/cycle/motion is the actual machine state
+ uint8_t machine_state;
uint8_t cycle_state; // cycs
uint8_t motion_state; // momo
uint8_t hold_state; // hold: feedhold sub-state machine
uint8_t g28_flag; // true = complete a G28 move
uint8_t g30_flag; // true = complete a G30 move
uint8_t feedhold_requested; // feedhold character has been received
- uint8_t queue_flush_requested; // queue flush character has been received
- uint8_t cycle_start_requested; // cycle start character has been received (flag to end feedhold)
- struct GCodeState *am; // active Gcode model is maintained by state management
+ /// queue flush character has been received
+ uint8_t queue_flush_requested;
+ /// cycle start character has been received (flag to end feedhold)
+ uint8_t cycle_start_requested;
+ struct GCodeState *am; // active model
// Model states
GCodeState_t gm; // core gcode model state
extern cmSingleton_t cm; // canonical machine controller singleton
-/*****************************************************************************
- * MACHINE STATE MODEL
+/* Machine state model
*
- * The following main variables track canonical machine state and state transitions.
+ * The following main variables track canonical machine state and state
+ * transitions.
* - cm.machine_state - overall state of machine and program execution
* - cm.cycle_state - what cycle the machine is executing (or none)
* - cm.motion_state - state of movement
- */
-/* Allowed states and combined states
*
- * MACHINE STATE CYCLE STATE MOTION_STATE COMBINED_STATE (FYI)
- * ------------- ------------ ------------- --------------------
- * MACHINE_UNINIT na na (U)
- * MACHINE_READY CYCLE_OFF MOTION_STOP (ROS) RESET-OFF-STOP
- * MACHINE_PROG_STOP CYCLE_OFF MOTION_STOP (SOS) STOP-OFF-STOP
- * MACHINE_PROG_END CYCLE_OFF MOTION_STOP (EOS) END-OFF-STOP
+ * Allowed states and combined states:
+ *
+ * MACHINE STATE CYCLE STATE MOTION_STATE COMBINED_STATE (FYI)
+ * ------------- ------------ ------------- --------------------
+ * MACHINE_UNINIT na na (U)
+ * MACHINE_READY CYCLE_OFF MOTION_STOP (ROS) RESET-OFF-STOP
+ * MACHINE_PROG_STOP CYCLE_OFF MOTION_STOP (SOS) STOP-OFF-STOP
+ * MACHINE_PROG_END CYCLE_OFF MOTION_STOP (EOS) END-OFF-STOP
*
- * MACHINE_CYCLE CYCLE_STARTED MOTION_STOP (CSS) CYCLE-START-STOP
- * MACHINE_CYCLE CYCLE_STARTED MOTION_RUN (CSR) CYCLE-START-RUN
- * MACHINE_CYCLE CYCLE_STARTED MOTION_HOLD (CSH) CYCLE-START-HOLD
- * MACHINE_CYCLE CYCLE_STARTED MOTION_END_HOLD (CSE) CYCLE-START-END_HOLD
+ * MACHINE_CYCLE CYCLE_STARTED MOTION_STOP (CSS) CYCLE-START-STOP
+ * MACHINE_CYCLE CYCLE_STARTED MOTION_RUN (CSR) CYCLE-START-RUN
+ * MACHINE_CYCLE CYCLE_STARTED MOTION_HOLD (CSH) CYCLE-START-HOLD
+ * MACHINE_CYCLE CYCLE_STARTED MOTION_END_HOLD (CSE) CYCLE-START-END_HOLD
*
- * MACHINE_CYCLE CYCLE_HOMING MOTION_STOP (CHS) CYCLE-HOMING-STOP
- * MACHINE_CYCLE CYCLE_HOMING MOTION_RUN (CHR) CYCLE-HOMING-RUN
- * MACHINE_CYCLE CYCLE_HOMING MOTION_HOLD (CHH) CYCLE-HOMING-HOLD
- * MACHINE_CYCLE CYCLE_HOMING MOTION_END_HOLD (CHE) CYCLE-HOMING-END_HOLD
+ * MACHINE_CYCLE CYCLE_HOMING MOTION_STOP (CHS) CYCLE-HOMING-STOP
+ * MACHINE_CYCLE CYCLE_HOMING MOTION_RUN (CHR) CYCLE-HOMING-RUN
+ * MACHINE_CYCLE CYCLE_HOMING MOTION_HOLD (CHH) CYCLE-HOMING-HOLD
+ * MACHINE_CYCLE CYCLE_HOMING MOTION_END_HOLD (CHE) CYCLE-HOMING-END_HOLD
*/
-// *** Note: check config printout strings align with all the state variables
-
-// ### LAYER 8 CRITICAL REGION ###
-// ### DO NOT CHANGE THESE ENUMERATIONS WITHOUT COMMUNITY INPUT ###
-enum cmCombinedState { // check alignment with messages in config.c / msg_stat strings
- COMBINED_INITIALIZING = 0, // [0] machine is initializing
- COMBINED_READY, // [1] machine is ready for use. Also used to force STOP state for null moves
- COMBINED_ALARM, // [2] machine in soft alarm state
- COMBINED_PROGRAM_STOP, // [3] program stop or no more blocks
- COMBINED_PROGRAM_END, // [4] program end
- COMBINED_RUN, // [5] motion is running
- COMBINED_HOLD, // [6] motion is holding
- COMBINED_PROBE, // [7] probe cycle active
- COMBINED_CYCLE, // [8] machine is running (cycling)
- COMBINED_HOMING, // [9] homing is treated as a cycle
- COMBINED_SHUTDOWN, // [10] machine in hard alarm state (shutdown)
+
+/// check alignment with messages in config.c / msg_stat strings
+enum cmCombinedState {
+ COMBINED_INITIALIZING, // machine is initializing
+ COMBINED_READY, // machine is ready for use. Also null move STOP state
+ COMBINED_ALARM, // machine in soft alarm state
+ COMBINED_PROGRAM_STOP, // program stop or no more blocks
+ COMBINED_PROGRAM_END, // program end
+ COMBINED_RUN, // motion is running
+ COMBINED_HOLD, // motion is holding
+ COMBINED_PROBE, // probe cycle active
+ COMBINED_CYCLE, // machine is running (cycling)
+ COMBINED_HOMING, // homing is treated as a cycle
+ COMBINED_SHUTDOWN, // machine in hard alarm state (shutdown)
};
-//### END CRITICAL REGION ###
enum cmMachineState {
- MACHINE_INITIALIZING = 0, // machine is initializing
- MACHINE_READY, // machine is ready for use
- MACHINE_ALARM, // machine in soft alarm state
- MACHINE_PROGRAM_STOP, // program stop or no more blocks
- MACHINE_PROGRAM_END, // program end
- MACHINE_CYCLE, // machine is running (cycling)
- MACHINE_SHUTDOWN, // machine in hard alarm state (shutdown)
+ MACHINE_INITIALIZING, // machine is initializing
+ MACHINE_READY, // machine is ready for use
+ MACHINE_ALARM, // machine in soft alarm state
+ MACHINE_PROGRAM_STOP, // program stop or no more blocks
+ MACHINE_PROGRAM_END, // program end
+ MACHINE_CYCLE, // machine is running (cycling)
+ MACHINE_SHUTDOWN, // machine in hard alarm state (shutdown)
};
enum cmCycleState {
- CYCLE_OFF = 0, // machine is idle
- CYCLE_MACHINING, // in normal machining cycle
- CYCLE_PROBE, // in probe cycle
- CYCLE_HOMING, // homing is treated as a specialized cycle
+ CYCLE_OFF, // machine is idle
+ CYCLE_MACHINING, // in normal machining cycle
+ CYCLE_PROBE, // in probe cycle
+ CYCLE_HOMING, // homing is treated as a specialized cycle
};
enum cmMotionState {
- MOTION_STOP = 0, // motion has stopped
- MOTION_RUN, // machine is in motion
- MOTION_HOLD // feedhold in progress
+ MOTION_STOP, // motion has stopped
+ MOTION_RUN, // machine is in motion
+ MOTION_HOLD // feedhold in progress
};
-enum cmFeedholdState { // feedhold_state machine
- FEEDHOLD_OFF = 0, // no feedhold in effect
- FEEDHOLD_SYNC, // start hold - sync to latest aline segment
- FEEDHOLD_PLAN, // replan blocks for feedhold
- FEEDHOLD_DECEL, // decelerate to hold point
- FEEDHOLD_HOLD, // holding
- FEEDHOLD_END_HOLD // end hold (transient state to OFF)
+enum cmFeedholdState { // feedhold_state machine
+ FEEDHOLD_OFF, // no feedhold in effect
+ FEEDHOLD_SYNC, // start hold - sync to latest aline segment
+ FEEDHOLD_PLAN, // replan blocks for feedhold
+ FEEDHOLD_DECEL, // decelerate to hold point
+ FEEDHOLD_HOLD, // holding
+ FEEDHOLD_END_HOLD // end hold (transient state to OFF)
};
-enum cmHomingState { // applies to cm.homing_state
- HOMING_NOT_HOMED = 0, // machine is not homed (0=false)
- HOMING_HOMED = 1, // machine is homed (1=true)
- HOMING_WAITING // machine waiting to be homed
+enum cmHomingState { // applies to cm.homing_state
+ HOMING_NOT_HOMED, // machine is not homed (0=false)
+ HOMING_HOMED, // machine is homed (1=true)
+ HOMING_WAITING // machine waiting to be homed
};
-enum cmProbeState { // applies to cm.probe_state
- PROBE_FAILED = 0, // probe reached endpoint without triggering
- PROBE_SUCCEEDED = 1, // probe was triggered, cm.probe_results has position
- PROBE_WAITING // probe is waiting to be started
+enum cmProbeState { // applies to cm.probe_state
+ PROBE_FAILED, // probe reached endpoint without triggering
+ PROBE_SUCCEEDED, // probe was triggered, cm.probe_results has position
+ PROBE_WAITING // probe is waiting to be started
};
* used by the current block, and may carry non-modal commands, whereas
* MotionMode persists across blocks (as G modal group 1)
*/
-enum cmNextAction { // these are in order to optimized CASE statement
- NEXT_ACTION_DEFAULT = 0, // Must be zero (invokes motion modes)
+/// these are in order to optimized CASE statement
+enum cmNextAction {
+ NEXT_ACTION_DEFAULT, // Must be zero (invokes motion modes)
NEXT_ACTION_SEARCH_HOME, // G28.2 homing cycle
NEXT_ACTION_SET_ABSOLUTE_ORIGIN, // G28.3 origin set
- NEXT_ACTION_HOMING_NO_SET, // G28.4 homing cycle with no coordinate setting
+ NEXT_ACTION_HOMING_NO_SET, // G28.4 homing cycle no coord setting
NEXT_ACTION_SET_G28_POSITION, // G28.1 set position in abs coordinates
NEXT_ACTION_GOTO_G28_POSITION, // G28 go to machine position
NEXT_ACTION_SET_G30_POSITION, // G30.1
enum cmMotionMode { // G Modal Group 1
- MOTION_MODE_STRAIGHT_TRAVERSE = 0, // G0 - straight traverse
+ MOTION_MODE_STRAIGHT_TRAVERSE, // G0 - straight traverse
MOTION_MODE_STRAIGHT_FEED, // G1 - straight feed
MOTION_MODE_CW_ARC, // G2 - clockwise arc feed
MOTION_MODE_CCW_ARC, // G3 - counter-clockwise arc feed
};
-enum cmModalGroup { // Used for detecting gcode errors. See NIST section 3.4
- MODAL_GROUP_G0 = 0, // {G10,G28,G28.1,G92} non-modal axis commands (note 1)
- MODAL_GROUP_G1, // {G0,G1,G2,G3,G80} motion
- MODAL_GROUP_G2, // {G17,G18,G19} plane selection
- MODAL_GROUP_G3, // {G90,G91} distance mode
- MODAL_GROUP_G5, // {G93,G94} feed rate mode
- MODAL_GROUP_G6, // {G20,G21} units
- MODAL_GROUP_G7, // {G40,G41,G42} cutter radius compensation
- MODAL_GROUP_G8, // {G43,G49} tool length offset
- MODAL_GROUP_G9, // {G98,G99} return mode in canned cycles
- MODAL_GROUP_G12, // {G54,G55,G56,G57,G58,G59} coordinate system selection
- MODAL_GROUP_G13, // {G61,G61.1,G64} path control mode
- MODAL_GROUP_M4, // {M0,M1,M2,M30,M60} stopping
- MODAL_GROUP_M6, // {M6} tool change
- MODAL_GROUP_M7, // {M3,M4,M5} spindle turning
- MODAL_GROUP_M8, // {M7,M8,M9} coolant (M7 & M8 may be active together)
- MODAL_GROUP_M9 // {M48,M49} speed/feed override switches
+enum cmModalGroup { // Used for detecting gcode errors. See NIST section 3.4
+ MODAL_GROUP_G0, // {G10,G28,G28.1,G92} non-modal axis commands
+ MODAL_GROUP_G1, // {G0,G1,G2,G3,G80} motion
+ MODAL_GROUP_G2, // {G17,G18,G19} plane selection
+ MODAL_GROUP_G3, // {G90,G91} distance mode
+ MODAL_GROUP_G5, // {G93,G94} feed rate mode
+ MODAL_GROUP_G6, // {G20,G21} units
+ MODAL_GROUP_G7, // {G40,G41,G42} cutter radius compensation
+ MODAL_GROUP_G8, // {G43,G49} tool length offset
+ MODAL_GROUP_G9, // {G98,G99} return mode in canned cycles
+ MODAL_GROUP_G12, // {G54,G55,G56,G57,G58,G59} coordinate system selection
+ MODAL_GROUP_G13, // {G61,G61.1,G64} path control mode
+ MODAL_GROUP_M4, // {M0,M1,M2,M30,M60} stopping
+ MODAL_GROUP_M6, // {M6} tool change
+ MODAL_GROUP_M7, // {M3,M4,M5} spindle turning
+ MODAL_GROUP_M8, // {M7,M8,M9} coolant
+ MODAL_GROUP_M9 // {M48,M49} speed/feed override switches
};
#define MODAL_GROUP_COUNT (MODAL_GROUP_M9 + 1)
-// Note 1: Our G0 omits G4,G30,G53,G92.1,G92.2,G92.3 as these have no axis components to error check
+// Note 1: Our G0 omits G4,G30,G53,G92.1,G92.2,G92.3 as these have no axis
+// components to error check
enum cmCanonicalPlane { // canonical plane - translates to:
// axis_0 axis_1 axis_2
- CANON_PLANE_XY = 0, // G17 X Y Z
+ CANON_PLANE_XY, // G17 X Y Z
CANON_PLANE_XZ, // G18 X Z Y
CANON_PLANE_YZ // G19 Y Z X
};
enum cmUnitsMode {
- INCHES = 0, // G20
- MILLIMETERS, // G21
- DEGREES // ABC axes (this value used for displays only)
+ INCHES, // G20
+ MILLIMETERS, // G21
+ DEGREES // ABC axes (this value used for displays only)
};
enum cmCoordSystem {
- ABSOLUTE_COORDS = 0, // machine coordinate system
+ ABSOLUTE_COORDS, // machine coordinate system
G54, // G54 coordinate system
G55, // G55 coordinate system
G56, // G56 coordinate system
#define COORD_SYSTEM_MAX G59 // set this manually to the last one
-enum cmPathControlMode { // G Modal Group 13
- PATH_EXACT_PATH = 0, // G61 - hits corners but does not stop if it does not need to.
+/// G Modal Group 13
+enum cmPathControlMode {
+ /// G61 - hits corners but does not stop if it does not need to.
+ PATH_EXACT_PATH,
PATH_EXACT_STOP, // G61.1 - stops at all corners
PATH_CONTINUOUS // G64 and typically the default mode
};
enum cmDistanceMode {
- ABSOLUTE_MODE = 0, // G90
+ ABSOLUTE_MODE, // G90
INCREMENTAL_MODE // G91
};
enum cmFeedRateMode {
- INVERSE_TIME_MODE = 0, // G93
+ INVERSE_TIME_MODE, // G93
UNITS_PER_MINUTE_MODE, // G94
UNITS_PER_REVOLUTION_MODE // G95 (unimplemented)
};
enum cmOriginOffset {
- ORIGIN_OFFSET_SET = 0, // G92 - set origin offsets
- ORIGIN_OFFSET_CANCEL, // G92.1 - zero out origin offsets
- ORIGIN_OFFSET_SUSPEND, // G92.2 - do not apply offsets, but preserve the values
- ORIGIN_OFFSET_RESUME // G92.3 - resume application of the suspended offsets
+ ORIGIN_OFFSET_SET, // G92 - set origin offsets
+ ORIGIN_OFFSET_CANCEL, // G92.1 - zero out origin offsets
+ ORIGIN_OFFSET_SUSPEND, // G92.2 - do not apply offsets, but preserve values
+ ORIGIN_OFFSET_RESUME // G92.3 - resume application of the suspended offsets
};
enum cmProgramFlow {
- PROGRAM_STOP = 0,
+ PROGRAM_STOP,
PROGRAM_END
};
-enum cmSpindleState { // spindle state settings (See hardware.h for bit settings)
- SPINDLE_OFF = 0,
+/// spindle state settings (See hardware.h for bit settings)
+enum cmSpindleState {
+ SPINDLE_OFF,
SPINDLE_CW,
SPINDLE_CCW
};
-enum cmCoolantState { // mist and flood coolant states
- COOLANT_OFF = 0, // all coolant off
- COOLANT_ON, // request coolant on or indicates both coolants are on
- COOLANT_MIST, // indicates mist coolant on
- COOLANT_FLOOD // indicates flood coolant on
+/// mist and flood coolant states
+enum cmCoolantState {
+ COOLANT_OFF, // all coolant off
+ COOLANT_ON, // request coolant on or indicate both coolants are on
+ COOLANT_MIST, // indicates mist coolant on
+ COOLANT_FLOOD // indicates flood coolant on
};
-enum cmDirection { // used for spindle and arc dir
- DIRECTION_CW = 0,
+/// used for spindle and arc dir
+enum cmDirection {
+ DIRECTION_CW,
DIRECTION_CCW
};
-enum cmAxisMode { // axis modes (ordered: see _cm_get_feed_time())
- AXIS_DISABLED = 0, // kill axis
- AXIS_STANDARD, // axis in coordinated motion w/standard behaviors
- AXIS_INHIBITED, // axis is computed but not activated
- AXIS_RADIUS // rotary axis calibrated to circumference
+
+/// axis modes (ordered: see _cm_get_feed_time())
+enum cmAxisMode {
+ AXIS_DISABLED, // kill axis
+ AXIS_STANDARD, // axis in coordinated motion w/standard behaviors
+ AXIS_INHIBITED, // axis is computed but not activated
+ AXIS_RADIUS // rotary axis calibrated to circumference
}; // ordering must be preserved.
#define AXIS_MODE_MAX_LINEAR AXIS_INHIBITED
void cm_set_spindle_mode(GCodeState_t *gcode_state, uint8_t spindle_mode);
void cm_set_spindle_speed_parameter(GCodeState_t *gcode_state, float speed);
void cm_set_tool_number(GCodeState_t *gcode_state, uint8_t tool);
-void cm_set_absolute_override(GCodeState_t *gcode_state, uint8_t absolute_override);
+void cm_set_absolute_override(GCodeState_t *gcode_state,
+ uint8_t absolute_override);
void cm_set_model_linenum(uint32_t linenum);
// Coordinate systems and offsets
void cm_set_model_target(float target[], float flag[]);
stat_t cm_test_soft_limits(float target[]);
-// Canonical machining functions (loosely) defined by NIST [organized by NIST Gcode doc]
+// Canonical machining functions (loosely) defined by NIST [organized by NIST
+// Gcode doc]
// Initialization and termination (4.3.2)
void canonical_machine_init();
-stat_t cm_hard_alarm(stat_t status); // enter hard alarm state. returns same status code
-stat_t cm_soft_alarm(stat_t status); // enter soft alarm state. returns same status code
+/// enter hard alarm state. returns same status code
+stat_t cm_hard_alarm(stat_t status);
+/// enter soft alarm state. returns same status code
+stat_t cm_soft_alarm(stat_t status);
stat_t cm_clear();
// Representation (4.3.3)
-stat_t cm_select_plane(uint8_t plane); // G17, G18, G19
-stat_t cm_set_units_mode(uint8_t mode); // G20, G21
-stat_t cm_set_distance_mode(uint8_t mode); // G90, G91
-stat_t cm_set_coord_offsets(uint8_t coord_system, float offset[], float flag[]); // G10 L2
+stat_t cm_select_plane(uint8_t plane);
+stat_t cm_set_units_mode(uint8_t mode);
+stat_t cm_set_distance_mode(uint8_t mode);
+stat_t cm_set_coord_offsets(uint8_t coord_system, float offset[], float flag[]);
-void cm_set_position(uint8_t axis, float position); // set absolute position - single axis
-stat_t cm_set_absolute_origin(float origin[], float flag[]); // G28.3
-void cm_set_axis_origin(uint8_t axis, const float position); // G28.3 planner callback
+void cm_set_position(uint8_t axis, float position);
+stat_t cm_set_absolute_origin(float origin[], float flag[]);
+void cm_set_axis_origin(uint8_t axis, const float position);
-stat_t cm_set_coord_system(uint8_t coord_system); // G54 - G59
-stat_t cm_set_origin_offsets(float offset[], float flag[]); // G92
-stat_t cm_reset_origin_offsets(); // G92.1
-stat_t cm_suspend_origin_offsets(); // G92.2
-stat_t cm_resume_origin_offsets(); // G92.3
+stat_t cm_set_coord_system(uint8_t coord_system);
+stat_t cm_set_origin_offsets(float offset[], float flag[]);
+stat_t cm_reset_origin_offsets();
+stat_t cm_suspend_origin_offsets();
+stat_t cm_resume_origin_offsets();
// Free Space Motion (4.3.4)
-stat_t cm_straight_traverse(float target[], float flags[]); // G0
-stat_t cm_set_g28_position(); // G28.1
-stat_t cm_goto_g28_position(float target[], float flags[]); // G28
-stat_t cm_set_g30_position(); // G30.1
-stat_t cm_goto_g30_position(float target[], float flags[]); // G30
+stat_t cm_straight_traverse(float target[], float flags[]);
+stat_t cm_set_g28_position();
+stat_t cm_goto_g28_position(float target[], float flags[]);
+stat_t cm_set_g30_position();
+stat_t cm_goto_g30_position(float target[], float flags[]);
// Machining Attributes (4.3.5)
-stat_t cm_set_feed_rate(float feed_rate); // F parameter
-stat_t cm_set_feed_rate_mode(uint8_t mode); // G93, G94, (G95 unimplemented)
-stat_t cm_set_path_control(uint8_t mode); // G61, G61.1, G64
+stat_t cm_set_feed_rate(float feed_rate);
+stat_t cm_set_feed_rate_mode(uint8_t mode);
+stat_t cm_set_path_control(uint8_t mode);
// Machining Functions (4.3.6)
-stat_t cm_straight_feed(float target[], float flags[]); // G1
-stat_t cm_arc_feed(float target[], float flags[], // G2, G3
+stat_t cm_straight_feed(float target[], float flags[]);
+stat_t cm_arc_feed(float target[], float flags[],
float i, float j, float k,
float radius, uint8_t motion_mode);
-stat_t cm_dwell(float seconds); // G4, P parameter
+stat_t cm_dwell(float seconds);
// Spindle Functions (4.3.7)
// see spindle.h for spindle definitions - which would go right here
// Tool Functions (4.3.8)
-stat_t cm_select_tool(uint8_t tool); // T parameter
-stat_t cm_change_tool(uint8_t tool); // M6
+stat_t cm_select_tool(uint8_t tool);
+stat_t cm_change_tool(uint8_t tool);
// Miscellaneous Functions (4.3.9)
-stat_t cm_mist_coolant_control(uint8_t mist_coolant); // M7
-stat_t cm_flood_coolant_control(uint8_t flood_coolant); // M8, M9
+stat_t cm_mist_coolant_control(uint8_t mist_coolant);
+stat_t cm_flood_coolant_control(uint8_t flood_coolant);
-stat_t cm_override_enables(uint8_t flag); // M48, M49
-stat_t cm_feed_rate_override_enable(uint8_t flag); // M50
-stat_t cm_feed_rate_override_factor(uint8_t flag); // M50.1
-stat_t cm_traverse_override_enable(uint8_t flag); // M50.2
-stat_t cm_traverse_override_factor(uint8_t flag); // M50.3
-stat_t cm_spindle_override_enable(uint8_t flag); // M51
-stat_t cm_spindle_override_factor(uint8_t flag); // M51.1
+stat_t cm_override_enables(uint8_t flag);
+stat_t cm_feed_rate_override_enable(uint8_t flag);
+stat_t cm_feed_rate_override_factor(uint8_t flag);
+stat_t cm_traverse_override_enable(uint8_t flag);
+stat_t cm_traverse_override_factor(uint8_t flag);
+stat_t cm_spindle_override_enable(uint8_t flag);
+stat_t cm_spindle_override_factor(uint8_t flag);
-void cm_message(char *message); // msg to console (e.g. Gcode comments)
+void cm_message(char *message);
// Program Functions (4.3.10)
void cm_request_feedhold();
void cm_request_queue_flush();
void cm_request_cycle_start();
-stat_t cm_feedhold_sequencing_callback(); // process feedhold, cycle start and queue flush requests
-stat_t cm_queue_flush(); // flush serial and planner queues with coordinate resets
+stat_t cm_feedhold_sequencing_callback();
+stat_t cm_queue_flush();
-void cm_cycle_start(); // (no Gcode)
-void cm_cycle_end(); // (no Gcode)
-void cm_feedhold(); // (no Gcode)
-void cm_program_stop(); // M0
-void cm_optional_program_stop(); // M1
-void cm_program_end(); // M2
+void cm_cycle_start();
+void cm_cycle_end();
+void cm_feedhold();
+void cm_program_stop();
+void cm_optional_program_stop();
+void cm_program_end();
char cm_get_axis_char(const int8_t axis);
-/*--- Cycles ---*/
+// Cycles
// Homing cycles
-stat_t cm_homing_cycle_start(); // G28.2
-stat_t cm_homing_cycle_start_no_set(); // G28.4
-stat_t cm_homing_callback(); // G28.2/.4 main loop callback
+stat_t cm_homing_cycle_start();
+stat_t cm_homing_cycle_start_no_set();
+stat_t cm_homing_callback();
// Probe cycles
-stat_t cm_straight_probe(float target[], float flags[]); // G38.2
-stat_t cm_probe_callback(); // G38.2 main loop callback
+stat_t cm_straight_probe(float target[], float flags[]);
+stat_t cm_probe_callback();
#define PWMS 2 // number of supported PWM channels
// Motor settings
-#define STEP_CLOCK_FREQ 25000
-#define MOTOR_CURRENT 0.8 // 1.0 is full power
+#define STEP_CLOCK_FREQ 25000 // Hz
+#define MOTOR_CURRENT 0.8 // 1.0 is full power
#define MOTOR_MICROSTEPS 16
-
-/// 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 MOTOR_POWER_MODE MOTOR_ALWAYS_POWERED // See stepper.c
+#define MOTOR_IDLE_TIMEOUT 2.00 // secs, motor off after this time
#define MAX_VELOCITY(angle, travel, mstep) \
(0.98 * (angle) * (travel) * STEP_CLOCK_FREQ / (mstep) / 6.0)
#define M1_STEP_ANGLE 1.8
#define M1_TRAVEL_PER_REV 1.25
#define M1_MICROSTEPS MOTOR_MICROSTEPS
-#define M1_POLARITY 0 // 0=normal, 1=reversed
+#define M1_POLARITY MOTOR_POLARITY_NORMAL
#define M1_POWER_MODE MOTOR_POWER_MODE
#define M1_POWER_LEVEL MOTOR_POWER_LEVEL
#define M2_STEP_ANGLE 1.8
#define M2_TRAVEL_PER_REV 1.25
#define M2_MICROSTEPS MOTOR_MICROSTEPS
-#define M2_POLARITY 0
+#define M2_POLARITY MOTOR_POLARITY_NORMAL
#define M2_POWER_MODE MOTOR_POWER_MODE
#define M2_POWER_LEVEL MOTOR_POWER_LEVEL
#define M3_MOTOR_MAP AXIS_A
#define M3_STEP_ANGLE 1.8
-#define M3_TRAVEL_PER_REV 1.25
+#define M3_TRAVEL_PER_REV 360 // degrees per motor rev
#define M3_MICROSTEPS MOTOR_MICROSTEPS
-#define M3_POLARITY 0
+#define M3_POLARITY MOTOR_POLARITY_NORMAL
#define M3_POWER_MODE MOTOR_POWER_MODE
#define M3_POWER_LEVEL MOTOR_POWER_LEVEL
#define M4_MOTOR_MAP AXIS_Z
#define M4_STEP_ANGLE 1.8
-#define M4_TRAVEL_PER_REV 360 // degrees moved per motor rev
+#define M4_TRAVEL_PER_REV 1.25
#define M4_MICROSTEPS MOTOR_MICROSTEPS
-#define M4_POLARITY 0
+#define M4_POLARITY MOTOR_POLARITY_NORMAL
#define M4_POWER_MODE MOTOR_POWER_MODE
#define M4_POWER_LEVEL MOTOR_POWER_LEVEL
-#define M5_MOTOR_MAP AXIS_B
-#define M5_STEP_ANGLE 1.8
-#define M5_TRAVEL_PER_REV 360
-#define M5_MICROSTEPS MOTOR_MICROSTEPS
-#define M5_POLARITY 0
-#define M5_POWER_MODE MOTOR_POWER_MODE
-#define M5_POWER_LEVEL MOTOR_POWER_LEVEL
-
-#define M6_MOTOR_MAP AXIS_C
-#define M6_STEP_ANGLE 1.8
-#define M6_TRAVEL_PER_REV 360
-#define M6_MICROSTEPS MOTOR_MICROSTEPS
-#define M6_POLARITY 0
-#define M6_POWER_MODE MOTOR_POWER_MODE
-#define M6_POWER_LEVEL MOTOR_POWER_LEVEL
-
-
-// Switch settings
-/// one of: SW_TYPE_NORMALLY_OPEN, SW_TYPE_NORMALLY_CLOSED
+
+// Switch settings. See switch.h
#define SWITCH_TYPE SW_TYPE_NORMALLY_OPEN
-/// SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT
#define X_SWITCH_MODE_MIN SW_MODE_HOMING
#define X_SWITCH_MODE_MAX SW_MODE_DISABLED
#define Y_SWITCH_MODE_MIN SW_MODE_HOMING
#define C_SWITCH_MODE_MIN SW_MODE_HOMING
#define C_SWITCH_MODE_MAX SW_MODE_DISABLED
+
// Jog settings
#define JOG_ACCELERATION 50000 // mm/min^2
+
// Machine settings
#define CHORDAL_TOLERANCE 0.01 // chordal accuracy for arc drawing
#define SOFT_LIMIT_ENABLE 0 // 0 = off, 1 = on
MAX_VELOCITY(M1_STEP_ANGLE, M1_TRAVEL_PER_REV, MOTOR_MICROSTEPS)
#define FEEDRATE_MAX VELOCITY_MAX
-// see canonical_machine.h cmAxisMode for valid values
+// See canonical_machine.h cmAxisMode for valid values
#define X_AXIS_MODE AXIS_STANDARD
#define X_VELOCITY_MAX VELOCITY_MAX // G0 max velocity in mm/min
#define X_FEEDRATE_MAX FEEDRATE_MAX // G1 max feed rate in mm/min
// Gcode defaults
#define GCODE_DEFAULT_UNITS MILLIMETERS // MILLIMETERS or INCHES
-// CANON_PLANE_XY, CANON_PLANE_XZ, or CANON_PLANE_YZ
-#define GCODE_DEFAULT_PLANE CANON_PLANE_XY
+#define GCODE_DEFAULT_PLANE CANON_PLANE_XY // See canonical_machine.h
#define GCODE_DEFAULT_COORD_SYSTEM G54 // G54, G55, G56, G57, G58 or G59
#define GCODE_DEFAULT_PATH_CONTROL PATH_CONTINUOUS
#define GCODE_DEFAULT_DISTANCE_MODE ABSOLUTE_MODE
#define MILLISECONDS_PER_TICK 1 // MS for system tick (systick * N)
#define SYS_ID_LEN 12 // length of system ID string from sys_get_id()
-// Clock Crystal Config. Pick one:
-//#define __CLOCK_INTERNAL_32MHZ TRUE // use internal oscillator
-//#define __CLOCK_EXTERNAL_8MHZ TRUE // uses PLL to provide 32 MHz system clock
-#define __CLOCK_EXTERNAL_16MHZ TRUE // uses PLL to provide 32 MHz system clock
+// Clock Crystal Config. See clock.c
+#define __CLOCK_EXTERNAL_16MHZ // uses PLL to provide 32 MHz system clock
-// Motor, output bit & switch port assignments
-// These are not all the same, and must line up in multiple places in gpio.h
-// Sorry if this is confusing - it's a board routing issue
-#define PORT_MOTOR_1 PORTA // motors mapped to ports
+// Motors mapped to ports
+#define PORT_MOTOR_1 PORTA
#define PORT_MOTOR_2 PORTF
#define PORT_MOTOR_3 PORTE
#define PORT_MOTOR_4 PORTD
-#define PORT_SWITCH_X PORTA // Switch axes mapped to ports
+// Switch axes mapped to ports
+#define PORT_SWITCH_X PORTA
#define PORT_SWITCH_Y PORTF
#define PORT_SWITCH_Z PORTE
#define PORT_SWITCH_A PORTD
+// Axes mapped to output ports
#define PORT_OUT_X PORTA
#define PORT_OUT_Y PORTF
#define PORT_OUT_Z PORTE
#define PORT_OUT_A PORTD
-// These next four must be changed when the PORT_MOTOR_* definitions change!
-#define PORTCFG_VP0MAP_PORT_MOTOR_1_gc PORTCFG_VP02MAP_PORTA_gc
-#define PORTCFG_VP1MAP_PORT_MOTOR_2_gc PORTCFG_VP13MAP_PORTF_gc
-#define PORTCFG_VP2MAP_PORT_MOTOR_3_gc PORTCFG_VP02MAP_PORTE_gc
-#define PORTCFG_VP3MAP_PORT_MOTOR_4_gc PORTCFG_VP13MAP_PORTD_gc
-
-#define PORT_MOTOR_1_VPORT VPORT0
-#define PORT_MOTOR_2_VPORT VPORT1
-#define PORT_MOTOR_3_VPORT VPORT2
-#define PORT_MOTOR_4_VPORT VPORT3
-
/*
- * Port setup - Stepper / Switch Ports:
- * b0 (out) step (SET is step, CLR is rest)
- * b1 (out) direction (CLR = Clockwise)
- * b2 (out) motor enable (CLR = Enabled)
+ * Port setup - stepper / switch ports:
+ * b0 (out) step
+ * b1 (out) direction (low = clockwise)
+ * b2 (out) motor enable (low = enabled)
* b3 (out) chip select
* b4 (in) fault
- * b5 (out) output bit for GPIO port1
- * b6 (in) min limit switch on GPIO 2 *
- * b7 (in) max limit switch on GPIO 2 *
- * * motor controls and GPIO2 port mappings are not the same
+ * b5 (out) output bit for GPIO port 1
+ * b6 (in) min limit switch on GPIO 2
+ * b7 (in) max limit switch on GPIO 2
*/
#define MOTOR_PORT_DIR_gm 0x2f // pin dir settings
#define MIST_COOLANT_BIT 1 // coolant on/off (same as flood)
#define FLOOD_COOLANT_BIT 1 // coolant on/off (same as mist)
-#define SPINDLE_LED 0
-#define SPINDLE_DIR_LED 1
-#define SPINDLE_PWM_LED 2
-#define COOLANT_LED 3
-
-// Can use the spindle direction as an indicator LED
+// Can use the spindle direction as an indicator LED. See gpio.h
#define INDICATOR_LED SPINDLE_DIR_LED
-/*
- * Interrupt usage - TinyG uses a lot of them all over the place
+/* Interrupt usage:
*
* HI Stepper DDA pulse generation (set in stepper.h)
* HI Stepper load routine SW interrupt (set in stepper.h)
*/
// Timer assignments - see specific modules for details
-#define TIMER_DDA TCC0 // DDA timer (see stepper.h)
-#define TIMER_TMC2660 TCC1 // TMC2660 timer (see tmc2660.h)
-#define TIMER_PWM1 TCD1 // PWM timer #1 (see pwm.c)
-#define TIMER_PWM2 TCD1 // PWM timer #2 (see pwm.c)
+#define TIMER_DDA TCC0 // DDA timer (see stepper.h)
+#define TIMER_TMC2660 TCC1 // TMC2660 timer (see tmc2660.h)
+#define TIMER_PWM1 TCD1 // PWM timer #1 (see pwm.c)
+#define TIMER_PWM2 TCD1 // PWM timer #2 (see pwm.c)
#define TIMER_MOTOR1 TCE1
#define TIMER_MOTOR2 TCF0
#define TIMER_MOTOR3 TCE0
#define TIMER_MOTOR4 TCD0
// Timer setup for stepper and dwells
-#define FREQUENCY_DDA STEP_CLOCK_FREQ // DDA frequency in hz.
-#define STEP_TIMER_DISABLE 0 // turn timer off
-#define STEP_TIMER_ENABLE 1 // turn timer clock on
-#define STEP_TIMER_WGMODE 0 // normal mode (count to TOP and rollover)
+#define STEP_TIMER_DISABLE 0 // timer clock off
+#define STEP_TIMER_ENABLE 1 // timer clock on
+#define STEP_TIMER_WGMODE 0 // normal mode (count to TOP & rollover)
#define TIMER_DDA_ISR_vect TCC0_OVF_vect
-#define TIMER_DDA_INTLVL 3 // Timer overflow HI
+#define TIMER_DDA_INTLVL 3 // timer overflow HI
+
+// PWM settings
#define PWM1_CTRLB (3 | TC1_CCBEN_bm) // single slope PWM channel B
#define PWM1_ISR_vect TCD1_CCB_vect
#define PWM2_CTRLA_CLKSEL TC_CLKSEL_DIV1_gc
#include <stdio.h>
-struct hmHomingSingleton { // persistent homing runtime variables
+typedef stat_t (*homing_func_t)(int8_t axis);
+
+
+/// persistent homing runtime variables
+struct hmHomingSingleton {
// controls for homing cycle
int8_t axis; // axis currently being homed
uint8_t min_mode; // mode for min switch for this axis
uint8_t max_mode; // mode for max switch for this axis
- int8_t homing_switch; // homing switch for current axis (index into switch flag table)
- int8_t limit_switch; // limit switch for current axis, or -1 if none
+ /// homing switch for current axis (index into switch flag table)
+ int8_t homing_switch;
+ int8_t limit_switch; // limit switch for current axis or -1 if none
uint8_t homing_closed; // 0=open, 1=closed
uint8_t limit_closed; // 0=open, 1=closed
- uint8_t set_coordinates; // G28.4 flag. true = set coords to zero at the end of homing cycle
- stat_t (*func)(int8_t axis); // binding for callback function state machine
+ /// G28.4 flag. true = set coords to zero at the end of homing cycle
+ uint8_t set_coordinates;
+ homing_func_t func; // binding for callback function state machine
// per-axis parameters
- float direction; // set to 1 for positive (max), -1 for negative (to min);
+ /// set to 1 for positive (max), -1 for negative (to min);
+ float direction;
float search_travel; // signed distance to travel in search
float search_velocity; // search speed as positive number
float latch_velocity; // latch speed as positive number
- float latch_backoff; // max distance to back off switch during latch phase
- float zero_backoff; // distance to back off switch before setting zero
- float max_clear_backoff; // maximum distance of switch clearing backoffs before erring out
+ /// max distance to back off switch during latch phase
+ float latch_backoff;
+ /// distance to back off switch before setting zero
+ float zero_backoff;
+ /// maximum distance of switch clearing backoffs before erring out
+ float max_clear_backoff;
// state saved from gcode model
uint8_t saved_units_mode; // G20,G21 global setting
static struct hmHomingSingleton hm;
-static stat_t _set_homing_func(stat_t (*func)(int8_t axis));
+static stat_t _set_homing_func(homing_func_t func);
static stat_t _homing_axis_start(int8_t axis);
static stat_t _homing_axis_clear(int8_t axis);
static stat_t _homing_axis_search(int8_t axis);
static stat_t _homing_finalize_exit(int8_t axis);
static int8_t _get_next_axis(int8_t axis);
-/***********************************************************************************
- **** G28.2 Homing Cycle ***********************************************************
- ***********************************************************************************/
-/*****************************************************************************
- * cm_homing_cycle_start() - G28.2 homing cycle using limit switches
- *
- * Homing works from a G28.2 according to the following writeup:
- * https://github.com/synthetos/TinyG/wiki/TinyG-Homing-(version-0.95-and-above)
+// G28.2 homing cycle
+
+/* Homing works from a G28.2 according to the following writeup:
+ * https://github.com/synthetos/TinyG/wiki/TinyG-Homing-(version-0.95-and-above)
*
- * --- How does this work? ---
+ * How does this work?
*
- * Homing is invoked using a G28.2 command with 1 or more axes specified in the
- * command: e.g. g28.2 x0 y0 z0 (FYI: the number after each axis is irrelevant)
+ * Homing is invoked using a G28.2 command with 1 or more axes specified in the
+ * command: e.g. g28.2 x0 y0 z0 (FYI: the number after each axis is irrelevant)
*
- * Homing is always run in the following order - for each enabled axis:
- * Z,X,Y,A Note: B and C cannot be homed
+ * Homing is always run in the following order - for each enabled axis:
+ * Z,X,Y,A Note: B and C cannot be homed
*
- * At the start of a homing cycle those switches configured for homing
- * (or for homing and limits) are treated as homing switches (they are modal).
+ * At the start of a homing cycle those switches configured for homing
+ * (or for homing and limits) are treated as homing switches (they are modal).
*
- * After initialization the following sequence is run for each axis to be homed:
+ * After initialization the following sequence is run for each axis to be homed:
*
- * 0. If a homing or limit switch is closed on invocation, clear off the switch
- * 1. Drive towards the homing switch at search velocity until switch is hit
- * 2. Drive away from the homing switch at latch velocity until switch opens
- * 3. Back off switch by the zero backoff distance and set zero for that axis
+ * 0. If a homing or limit switch is closed on invocation, clear the switch
+ * 1. Drive towards the homing switch at search velocity until switch is hit
+ * 2. Drive away from the homing switch at latch velocity until switch opens
+ * 3. Back off switch by the zero backoff distance and set zero for that axis
*
- * Homing works as a state machine that is driven by registering a callback
- * function at hm.func() for the next state to be run. Once the axis is
- * initialized each callback basically does two things (1) start the move
- * for the current function, and (2) register the next state with hm.func().
- * When a move is started it will either be interrupted if the homing switch
- * changes state, This will cause the move to stop with a feedhold. The other
- * thing that can happen is the move will run to its full length if no switch
- * change is detected (hit or open),
+ * Homing works as a state machine that is driven by registering a callback
+ * function at hm.func() for the next state to be run. Once the axis is
+ * initialized each callback basically does two things (1) start the move
+ * for the current function, and (2) register the next state with hm.func().
+ * When a move is started it will either be interrupted if the homing switch
+ * changes state, This will cause the move to stop with a feedhold. The other
+ * thing that can happen is the move will run to its full length if no switch
+ * change is detected (hit or open),
*
- * Once all moves for an axis are complete the next axis in the sequence is homed
+ * Once all moves for an axis are complete the next axis in the sequence is
+ * homed.
*
- * When a homing cycle is initiated the homing state is set to HOMING_NOT_HOMED
- * When homing completes successfully this is set to HOMING_HOMED, otherwise it
- * remains HOMING_NOT_HOMED.
+ * When a homing cycle is initiated the homing state is set to HOMING_NOT_HOMED
+ * When homing completes successfully this is set to HOMING_HOMED, otherwise it
+ * remains HOMING_NOT_HOMED.
*/
-/* --- 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.
+ * 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
- * 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.
+ * Another Note: When coding a cycle (like this one) 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.
*/
+
+/// G28.2 homing cycle using limit switches
stat_t cm_homing_cycle_start() {
// save relevant non-axis parameters from Gcode model
hm.saved_units_mode = cm_get_units_mode(ACTIVE_MODEL);
// set working values
cm_set_units_mode(MILLIMETERS);
cm_set_distance_mode(INCREMENTAL_MODE);
- cm_set_coord_system(ABSOLUTE_COORDS); // homing is done in machine coordinates
+ cm_set_coord_system(ABSOLUTE_COORDS); // in machine coordinates
cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE);
hm.set_coordinates = true;
stat_t cm_homing_cycle_start_no_set() {
cm_homing_cycle_start();
- hm.set_coordinates = false; // set flag to not update position variables at end of cycle
+ hm.set_coordinates = false; // don't update position variables at end of cycle
return STAT_OK;
}
-/* Homing axis moves - these execute in sequence for each axis
- * cm_homing_callback() - main loop callback for running the homing cycle
- * _set_homing_func() - a convenience for setting the next dispatch vector and exiting
- * _trigger_feedhold() - callback from switch closure to trigger feedhold
- * (convenience for casting)
- * _bind_switch_settings() - setup switch for homing operation
- * _restore_switch_settings() - return switch to normal operation
- * _homing_axis_start() - get next axis, initialize variables, call the clear
- * _homing_axis_clear() - initiate a clear to move off a switch that is thrown at the start
- * _homing_axis_search() - fast search for switch, closes switch
- * _homing_axis_latch() - slow reverse until switch opens again
- * _homing_axis_final() - backoff from latch location to zero position
- * _homing_axis_move() - helper that actually executes the above moves
- */
+/// Main loop callback for running the homing cycle
stat_t cm_homing_callback() {
- if (cm.cycle_state != CYCLE_HOMING) return STAT_NOOP; // exit if not in a homing cycle
- if (cm_get_runtime_busy() == true) return STAT_EAGAIN; // sync to planner move ends
- return hm.func(hm.axis); // execute the current homing move
+ if (cm.cycle_state != CYCLE_HOMING)
+ return STAT_NOOP; // exit if not in a homing cycle
+ if (cm_get_runtime_busy()) return STAT_EAGAIN; // sync to planner move ends
+ return hm.func(hm.axis); // execute the current homing move
}
-static stat_t _set_homing_func(stat_t (*func)(int8_t axis)) {
+/// A convenience for setting the next dispatch vector and exiting
+static stat_t _set_homing_func(homing_func_t func) {
hm.func = func;
return STAT_EAGAIN;
}
+/// Get next axis, initialize variables, call the clear
static stat_t _homing_axis_start(int8_t axis) {
// get the first or next axis
- if ((axis = _get_next_axis(axis)) < 0) { // axes are done or error
+ if ((axis = _get_next_axis(axis)) < 0) { // axes are done or error
if (axis == -1) { // -1 is done
cm.homing_state = HOMING_HOMED;
return _set_homing_func(_homing_finalize_exit);
return _homing_error_exit(-2, STAT_HOMING_ERROR_BAD_OR_NO_AXIS);
}
- // clear the homed flag for axis so we'll be able to move w/o triggering soft limits
+ // clear the homed flag for axis to move w/o triggering soft limits
cm.homed[axis] = false;
// trap axis mis-configurations
// calculate and test travel distance
float travel_distance =
- fabs(cm.a[axis].travel_max - cm.a[axis].travel_min) + cm.a[axis].latch_backoff;
+ fabs(cm.a[axis].travel_max - cm.a[axis].travel_min) +
+ cm.a[axis].latch_backoff;
if (fp_ZERO(travel_distance))
return _homing_error_exit(axis, STAT_HOMING_ERROR_TRAVEL_MIN_MAX_IDENTICAL);
hm.max_mode = get_switch_mode(MAX_SWITCH(axis));
// one or the other must be homing
- if (((hm.min_mode & SW_HOMING_BIT) ^ (hm.max_mode & SW_HOMING_BIT)) == 0)
- return _homing_error_exit(axis, STAT_HOMING_ERROR_SWITCH_MISCONFIGURATION); // axis cannot be homed
+ if (!((hm.min_mode & SW_HOMING_BIT) ^ (hm.max_mode & SW_HOMING_BIT)))
+ // axis cannot be homed
+ return _homing_error_exit(axis, STAT_HOMING_ERROR_SWITCH_MISCONFIGURATION);
- hm.axis = axis; // persist the axis
- hm.search_velocity = fabs(cm.a[axis].search_velocity); // search velocity is always positive
- hm.latch_velocity = fabs(cm.a[axis].latch_velocity); // latch velocity is always positive
+ hm.axis = axis; // persist the axis
+ // search velocity is always positive
+ hm.search_velocity = fabs(cm.a[axis].search_velocity);
+ // latch velocity is always positive
+ hm.latch_velocity = fabs(cm.a[axis].latch_velocity);
// setup parameters homing to the minimum switch
if (hm.min_mode & SW_HOMING_BIT) {
- hm.homing_switch = MIN_SWITCH(axis); // the min is the homing switch
- hm.limit_switch = MAX_SWITCH(axis); // the max would be the limit switch
- hm.search_travel = -travel_distance; // search travels in negative direction
- hm.latch_backoff = cm.a[axis].latch_backoff; // latch travels in positive direction
+ hm.homing_switch = MIN_SWITCH(axis); // min is the homing switch
+ hm.limit_switch = MAX_SWITCH(axis); // max would be limit switch
+ hm.search_travel = -travel_distance; // in negative direction
+ hm.latch_backoff = cm.a[axis].latch_backoff; // in positive direction
hm.zero_backoff = cm.a[axis].zero_backoff;
// setup parameters for positive travel (homing to the maximum switch)
} else {
- hm.homing_switch = MAX_SWITCH(axis); // the max is the homing switch
- hm.limit_switch = MIN_SWITCH(axis); // the min would be the limit switch
- hm.search_travel = travel_distance; // search travels in positive direction
- hm.latch_backoff = -cm.a[axis].latch_backoff; // latch travels in negative direction
+ hm.homing_switch = MAX_SWITCH(axis); // max is homing switch
+ hm.limit_switch = MIN_SWITCH(axis); // min would be limit switch
+ hm.search_travel = travel_distance; // in positive direction
+ hm.latch_backoff = -cm.a[axis].latch_backoff; // in negative direction
hm.zero_backoff = -cm.a[axis].zero_backoff;
}
// if homing is disabled for the axis then skip to the next axis
uint8_t sw_mode = get_switch_mode(hm.homing_switch);
- if ((sw_mode != SW_MODE_HOMING) && (sw_mode != SW_MODE_HOMING_LIMIT))
+ if (sw_mode != SW_MODE_HOMING && sw_mode != SW_MODE_HOMING_LIMIT)
return _set_homing_func(_homing_axis_start);
// disable the limit switch parameter if there is no limit switch
- if (get_switch_mode(hm.limit_switch) == SW_MODE_DISABLED) hm.limit_switch = -1;
+ if (get_switch_mode(hm.limit_switch) == SW_MODE_DISABLED)
+ hm.limit_switch = -1;
- // hm.saved_jerk = cm.a[axis].jerk_max; // save the max jerk value
- hm.saved_jerk = cm_get_axis_jerk(axis); // save the max jerk value
+ hm.saved_jerk = cm_get_axis_jerk(axis); // save the max jerk value
- return _set_homing_func(_homing_axis_clear); // start the clear
+ return _set_homing_func(_homing_axis_clear); // start the clear
}
-// Handle an initial switch closure by backing off the closed switch
-// NOTE: Relies on independent switches per axis (not shared)
-static stat_t _homing_axis_clear(int8_t axis) { // first clear move
+/// Initiate a clear to move off a switch that is thrown at the start
+static stat_t _homing_axis_clear(int8_t axis) {
+ // Handle an initial switch closure by backing off the closed switch
+ // NOTE: Relies on independent switches per axis (not shared)
+
if (sw.state[hm.homing_switch] == SW_CLOSED)
_homing_axis_move(axis, hm.latch_backoff, hm.search_velocity);
+
else if (sw.state[hm.limit_switch] == SW_CLOSED)
_homing_axis_move(axis, -hm.latch_backoff, hm.search_velocity);
}
-static stat_t _homing_axis_search(int8_t axis) { // start the search
- cm_set_axis_jerk(axis, cm.a[axis].jerk_homing); // use the homing jerk for search onward
+/// Fast search for switch, closes switch
+static stat_t _homing_axis_search(int8_t axis) {
+ // use the homing jerk for search onward
+ cm_set_axis_jerk(axis, cm.a[axis].jerk_homing);
_homing_axis_move(axis, hm.search_travel, hm.search_velocity);
return _set_homing_func(_homing_axis_latch);
}
-static stat_t _homing_axis_latch(int8_t axis) { // latch to switch open
+/// Slow reverse until switch opens again
+static stat_t _homing_axis_latch(int8_t axis) {
// verify assumption that we arrived here because of homing switch closure
// rather than user-initiated feedhold or other disruption
if (sw.state[hm.homing_switch] != SW_CLOSED)
}
-static stat_t _homing_axis_zero_backoff(int8_t axis) { // backoff to zero position
+/// backoff to zero position
+static stat_t _homing_axis_zero_backoff(int8_t axis) {
_homing_axis_move(axis, hm.zero_backoff, hm.search_velocity);
return _set_homing_func(_homing_axis_set_zero);
}
-static stat_t _homing_axis_set_zero(int8_t axis) { // set zero and finish up
- if (hm.set_coordinates != false) {
+/// set zero and finish up
+static stat_t _homing_axis_set_zero(int8_t axis) {
+ if (hm.set_coordinates) {
cm_set_position(axis, 0);
cm.homed[axis] = true;
- } else
- // do not set axis if in G28.4 cycle
+ } else // do not set axis if in G28.4 cycle
cm_set_position(axis, cm_get_work_position(RUNTIME, axis));
- cm_set_axis_jerk(axis, hm.saved_jerk); // restore the max jerk value
+ cm_set_axis_jerk(axis, hm.saved_jerk); // restore the max jerk value
return _set_homing_func(_homing_axis_start);
}
+/// helper that actually executes the above moves
static stat_t _homing_axis_move(int8_t axis, float target, float velocity) {
- float vect[] = {0,0,0,0,0,0};
- float flags[] = {false, false, false, false, false, false};
+ float vect[] = {};
+ float flags[] = {};
vect[axis] = target;
flags[axis] = true;
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();
ritorno(cm_straight_feed(vect, flags));
/// End homing cycle in progress
static stat_t _homing_abort(int8_t axis) {
- cm_set_axis_jerk(axis, hm.saved_jerk); // restore the max jerk value
+ cm_set_axis_jerk(axis, hm.saved_jerk); // restore the max jerk value
_homing_finalize_exit(axis);
report_request();
- return STAT_HOMING_CYCLE_FAILED; // homing state remains HOMING_NOT_HOMED
+ return STAT_HOMING_CYCLE_FAILED; // homing state remains HOMING_NOT_HOMED
}
static stat_t _homing_error_exit(int8_t axis, stat_t status) {
// Generate the warning message.
- if (axis == -2) fprintf_P(stderr, PSTR("Homing error - Bad or no axis(es) specified"));
+ if (axis == -2)
+ fprintf_P(stderr, PSTR("Homing error - Bad or no axis(es) specified"));
else fprintf_P(stderr, PSTR("Homing error - %c axis settings misconfigured"),
cm_get_axis_char(axis));
_homing_finalize_exit(axis);
- return STAT_HOMING_CYCLE_FAILED; // homing state remains HOMING_NOT_HOMED
+ return STAT_HOMING_CYCLE_FAILED; // homing state remains HOMING_NOT_HOMED
}
-/// Helper to finalize homing
-static stat_t _homing_finalize_exit(int8_t axis) { // third part of return to home
- mp_flush_planner(); // should be stopped, but in case of switch closure
+/// Helper to finalize homing, third part of return to home
+static stat_t _homing_finalize_exit(int8_t axis) {
+ mp_flush_planner(); // should be stopped, but in case of switch closure
- cm_set_coord_system(hm.saved_coord_system); // restore to work coordinate system
+ // restore to work coordinate system
+ cm_set_coord_system(hm.saved_coord_system);
cm_set_units_mode(hm.saved_units_mode);
cm_set_distance_mode(hm.saved_distance_mode);
cm_set_feed_rate_mode(hm.saved_feed_rate_mode);
}
-/*
- * Return next axis in sequence based on axis in arg
+/* Return next axis in sequence based on axis in arg
*
- * Accepts "axis" arg as the current axis; or -1 to retrieve the first axis
- * Returns next axis based on "axis" argument and if that axis is flagged for homing in the gf struct
- * Returns -1 when all axes have been processed
- * Returns -2 if no axes are specified (Gcode calling error)
- * Homes Z first, then the rest in sequence
+ * Accepts "axis" arg as the current axis; or -1 to retrieve the first axis
+ * Returns next axis based on "axis" argument and if that axis is flagged for
+ * homing in the gf struct
+ * Returns -1 when all axes have been processed
+ * Returns -2 if no axes are specified (Gcode calling error)
+ * Homes Z first, then the rest in sequence
*
- * Isolating this function facilitates implementing more complex and
- * user-specified axis homing orders
+ * Isolating this function facilitates implementing more complex and
+ * user-specified axis homing orders
*/
static int8_t _get_next_axis(int8_t axis) {
#if (HOMING_AXES <= 4)
- if (axis == -1) { // inelegant brute force solution
+ switch (axis) {
+ case -1: // inelegant brute force solution
if (fp_TRUE(cm.gf.target[AXIS_Z])) return AXIS_Z;
if (fp_TRUE(cm.gf.target[AXIS_X])) return AXIS_X;
if (fp_TRUE(cm.gf.target[AXIS_Y])) return AXIS_Y;
if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A;
return -2; // error
- } else if (axis == AXIS_Z) {
+ case AXIS_Z:
if (fp_TRUE(cm.gf.target[AXIS_X])) return AXIS_X;
if (fp_TRUE(cm.gf.target[AXIS_Y])) return AXIS_Y;
if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A;
+ break;
- } else if (axis == AXIS_X) {
+ case AXIS_X:
if (fp_TRUE(cm.gf.target[AXIS_Y])) return AXIS_Y;
if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A;
+ break;
- } else if (axis == AXIS_Y) {
+ case AXIS_Y:
if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A;
+ break;
}
return -1; // done
#else
- if (axis == -1) {
+ switch (axis) {
+ case -1:
if (fp_TRUE(cm.gf.target[AXIS_Z])) return AXIS_Z;
if (fp_TRUE(cm.gf.target[AXIS_X])) return AXIS_X;
if (fp_TRUE(cm.gf.target[AXIS_Y])) return AXIS_Y;
if (fp_TRUE(cm.gf.target[AXIS_C])) return AXIS_C;
return -2; // error
- } else if (axis == AXIS_Z) {
+ case AXIS_Z:
if (fp_TRUE(cm.gf.target[AXIS_X])) return AXIS_X;
if (fp_TRUE(cm.gf.target[AXIS_Y])) return AXIS_Y;
if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A;
if (fp_TRUE(cm.gf.target[AXIS_B])) return AXIS_B;
if (fp_TRUE(cm.gf.target[AXIS_C])) return AXIS_C;
+ break;
- } else if (axis == AXIS_X) {
+ case AXIS_X:
if (fp_TRUE(cm.gf.target[AXIS_Y])) return AXIS_Y;
if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A;
if (fp_TRUE(cm.gf.target[AXIS_B])) return AXIS_B;
if (fp_TRUE(cm.gf.target[AXIS_C])) return AXIS_C;
+ break;
- } else if (axis == AXIS_Y) {
+ case AXIS_Y:
if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A;
if (fp_TRUE(cm.gf.target[AXIS_B])) return AXIS_B;
if (fp_TRUE(cm.gf.target[AXIS_C])) return AXIS_C;
+ break;
- } else if (axis == AXIS_A) {
+ case AXIS_A:
if (fp_TRUE(cm.gf.target[AXIS_B])) return AXIS_B;
if (fp_TRUE(cm.gf.target[AXIS_C])) return AXIS_C;
+ break;
- } else if (axis == AXIS_B) {
+ case AXIS_B:
if (fp_TRUE(cm.gf.target[AXIS_C])) return AXIS_C;
+ break;
}
return -1; // done
-/*
- * cycle_probing.c - probing cycle extension to canonical_machine.c
+/* cycle_probing.c - probing cycle extension to canonical_machine.c
* Part of TinyG project
*
* Copyright (c) 2010 - 2015 Alden S Hart, Jr.
*
- * 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 <stdbool.h>
#include <stdio.h>
-/**** Probe singleton structure ****/
#define MINIMUM_PROBE_TRAVEL 0.254
-struct pbProbingSingleton { // persistent probing runtime variables
- stat_t (*func)(); // binding for callback function state machine
+
+struct pbProbingSingleton { // persistent probing runtime variables
+ stat_t (*func)(); // binding for callback function state machine
// switch configuration
- uint8_t probe_switch; // which switch should we check?
- uint8_t saved_switch_type; // saved switch type NO/NC
- uint8_t saved_switch_mode; // save the probe switch's original settings
+ uint8_t probe_switch; // which switch should we check?
+ uint8_t saved_switch_type; // saved switch type NO/NC
+ uint8_t saved_switch_mode; // save the probe switch's original settings
// state saved from gcode model
- uint8_t saved_distance_mode; // G90,G91 global setting
- uint8_t saved_coord_system; // G54 - G59 setting
- float saved_jerk[AXES]; // saved and restored for each axis
+ uint8_t saved_distance_mode; // G90,G91 global setting
+ uint8_t saved_coord_system; // G54 - G59 setting
+ float saved_jerk[AXES]; // saved and restored for each axis
// probe destination
float start_position[AXES];
float target[AXES];
float flags[AXES];
};
+
static struct pbProbingSingleton pb;
-/**** NOTE: global prototypes and other .h info is located in canonical_machine.h ****/
static stat_t _probing_init();
static stat_t _probing_start();
static stat_t _probing_error_exit(int8_t axis);
-/**** HELPERS ***************************************************************************
- * _set_pb_func() - a convenience for setting the next dispatch vector and exiting
- */
-
-uint8_t _set_pb_func(uint8_t (*func)())
-{
+/// A convenience for setting the next dispatch vector and exiting
+uint8_t _set_pb_func(uint8_t (*func)()) {
pb.func = func;
return STAT_EAGAIN;
}
-/****************************************************************************************
- * cm_probing_cycle_start() - G38.2 homing cycle using limit switches
- * cm_probing_callback() - main loop callback for running the homing cycle
- *
- * --- Some further details ---
- *
- * All cm_probe_cycle_start does is prevent any new commands from queueing to the
- * planner so that the planner can move to a sop and report MACHINE_PROGRAM_STOP.
- * OK, it also queues the function that's called once motion has stopped.
+
+/* All cm_probe_cycle_start does is prevent any new commands from
+ * queueing to the planner so that the planner can move to a sop and
+ * report MACHINE_PROGRAM_STOP. OK, it also queues the function
+ * that's called once motion has stopped.
*
- * Note: When coding a cycle (like this one) you get to perform one queued move per
- * entry into the continuation, then you must exit.
+ * 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
- * 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_get_runtime_busy() is about.
+ * Another Note: When coding a cycle (like this one) 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_get_runtime_busy() is
+ * about.
*/
-uint8_t cm_straight_probe(float target[], float flags[])
-{
+
+/// G38.2 homing cycle using limit switches
+uint8_t cm_straight_probe(float target[], float flags[]) {
// trap zero feed rate condition
- if ((cm.gm.feed_rate_mode != INVERSE_TIME_MODE) && (fp_ZERO(cm.gm.feed_rate))) {
+ if (cm.gm.feed_rate_mode != INVERSE_TIME_MODE && fp_ZERO(cm.gm.feed_rate))
return STAT_GCODE_FEEDRATE_NOT_SPECIFIED;
- }
// trap no axes specified
- if (fp_NOT_ZERO(flags[AXIS_X]) && fp_NOT_ZERO(flags[AXIS_Y]) && fp_NOT_ZERO(flags[AXIS_Z]))
+ if (fp_NOT_ZERO(flags[AXIS_X]) && fp_NOT_ZERO(flags[AXIS_Y]) &&
+ fp_NOT_ZERO(flags[AXIS_Z]))
return STAT_GCODE_AXIS_IS_MISSING;
// set probe move endpoint
- copy_vector(pb.target, target); // set probe move endpoint
+ copy_vector(pb.target, target); // set probe move endpoint
copy_vector(pb.flags, flags); // set axes involved on the move
- clear_vector(cm.probe_results); // clear the old probe position.
+ clear_vector(cm.probe_results); // clear the old probe position.
// NOTE: relying on probe_result will not detect a probe to 0,0,0.
- cm.probe_state = PROBE_WAITING; // wait until planner queue empties before completing initialization
+ // wait until planner queue empties before completing initialization
+ cm.probe_state = PROBE_WAITING;
pb.func = _probing_init; // bind probing initialization function
return STAT_OK;
}
-uint8_t cm_probe_callback()
-{
- if ((cm.cycle_state != CYCLE_PROBE) && (cm.probe_state != PROBE_WAITING)) {
- return STAT_NOOP; // exit if not in a probe cycle or waiting for one
- }
- if (cm_get_runtime_busy() == true) { return STAT_EAGAIN;} // sync to planner move ends
- return pb.func(); // execute the current homing move
+
+/// main loop callback for running the homing cycle
+uint8_t cm_probe_callback() {
+ if (cm.cycle_state != CYCLE_PROBE && cm.probe_state != PROBE_WAITING)
+ return STAT_NOOP; // exit if not in a probe cycle or waiting for one
+
+ if (cm_get_runtime_busy()) return STAT_EAGAIN; // sync to planner move ends
+ return pb.func(); // execute the current homing move
}
-/*
- * _probing_init() - G38.2 homing cycle using limit switches
+/* G38.2 homing cycle using limit switches
*
- * These initializations are required before starting the probing cycle.
- * They must be done after the planner has exhasted all current CYCLE moves as
- * they affect the runtime (specifically the switch modes). Side effects would
- * include limit switches initiating probe actions instead of just killing movement
+ * These initializations are required before starting the probing cycle.
+ * They must be done after the planner has exhasted all current CYCLE moves as
+ * they affect the runtime (specifically the switch modes). Side effects would
+ * include limit switches initiating probe actions instead of just killing
+ * movement
*/
-
-static uint8_t _probing_init()
-{
+static uint8_t _probing_init() {
// so optimistic... ;)
// NOTE: it is *not* an error condition for the probe not to trigger.
- // it is an error for the limit or homing switches to fire, or for some other configuration error.
+ // it is an error for the limit or homing switches to fire, or for some other
+ // configuration error.
cm.probe_state = PROBE_FAILED;
cm.cycle_state = CYCLE_PROBE;
- // initialize the axes - save the jerk settings & switch to the jerk_homing settings
- for( uint8_t axis=0; axis<AXES; axis++ ) {
- pb.saved_jerk[axis] = cm_get_axis_jerk(axis); // save the max jerk value
- cm_set_axis_jerk(axis, cm.a[axis].jerk_homing); // use the homing jerk for probe
+ // initialize the axes - save the jerk settings & switch to the jerk_homing
+ // settings
+ for (uint8_t axis = 0; axis < AXES; axis++) {
+ pb.saved_jerk[axis] = cm_get_axis_jerk(axis); // save the max jerk value
+ cm_set_axis_jerk(axis, cm.a[axis].jerk_homing); // use homing jerk for probe
pb.start_position[axis] = cm_get_absolute_position(ACTIVE_MODEL, axis);
}
// error if the probe target is too close to the current position
- if (get_axis_vector_length(pb.start_position, pb.target) < MINIMUM_PROBE_TRAVEL)
+ if (get_axis_vector_length(pb.start_position, pb.target) <
+ MINIMUM_PROBE_TRAVEL)
_probing_error_exit(-2);
// error if the probe target requires a move along the A/B/C axes
- for ( uint8_t axis=AXIS_A; axis<AXES; axis++ ) {
+ for (uint8_t axis = 0; axis < AXES; axis++)
if (fp_NE(pb.start_position[axis], pb.target[axis]))
_probing_error_exit(axis);
- }
// initialize the probe switch
// FIXME: we should be able to use the homing switch at this point too,
// Can't because switch mode is global and our probe is NO, not NC.
- pb.probe_switch = SW_MIN_Z; // FIXME: hardcoded...
+ pb.probe_switch = SW_MIN_Z; // FIXME: hardcoded...
pb.saved_switch_mode = sw.mode[pb.probe_switch];
sw.mode[pb.probe_switch] = SW_MODE_HOMING;
- pb.saved_switch_type = sw.switch_type; // save the switch type for recovery later.
- sw.switch_type = SW_TYPE_NORMALLY_OPEN; // contact probes are NO switches... usually
- switch_init(); // re-init to pick up new switch settings
+ // save the switch type for recovery later.
+ pb.saved_switch_type = sw.switch_type;
+ // contact probes are NO switches... usually
+ sw.switch_type = SW_TYPE_NORMALLY_OPEN;
+ // re-init to pick up new switch settings
+ switch_init();
// probe in absolute machine coords
- pb.saved_coord_system = cm_get_coord_system(ACTIVE_MODEL); //cm.gm.coord_system;
- pb.saved_distance_mode = cm_get_distance_mode(ACTIVE_MODEL); //cm.gm.distance_mode;
+ pb.saved_coord_system = cm_get_coord_system(ACTIVE_MODEL);
+ pb.saved_distance_mode = cm_get_distance_mode(ACTIVE_MODEL);
cm_set_distance_mode(ABSOLUTE_MODE);
cm_set_coord_system(ABSOLUTE_COORDS);
cm_spindle_control(SPINDLE_OFF);
- return _set_pb_func(_probing_start); // start the move
+
+ // start the move
+ return _set_pb_func(_probing_start);
}
-/*
- * _probing_start()
- */
-static stat_t _probing_start()
-{
+static stat_t _probing_start() {
// initial probe state, don't probe if we're already contacted!
int8_t probe = sw.state[pb.probe_switch];
return _set_pb_func(_probing_finish);
}
-/*
- * _probing_finish()
- */
-static stat_t _probing_finish()
-{
+static stat_t _probing_finish() {
int8_t probe = sw.state[pb.probe_switch];
cm.probe_state = (probe==SW_CLOSED) ? PROBE_SUCCEEDED : PROBE_FAILED;
- for( uint8_t axis=0; axis<AXES; axis++ ) {
- // if we got here because of a feed hold we need to keep the model position correct
+ for (uint8_t axis = 0; axis < AXES; axis++) {
+ // if we got here because of a feed hold keep the model position correct
cm_set_position(axis, mp_get_runtime_work_position(axis));
// store the probe results
return _set_pb_func(_probing_finalize_exit);
}
-/*
- * _probe_restore_settings()
- * _probing_finalize_exit()
- * _probing_error_exit()
- */
static void _probe_restore_settings() {
- mp_flush_planner(); // we should be stopped now, but in case of switch closure
+ // we should be stopped now, but in case of switch closure
+ mp_flush_planner();
sw.switch_type = pb.saved_switch_type;
sw.mode[pb.probe_switch] = pb.saved_switch_mode;
- switch_init(); // re-init to pick up changes
+ switch_init(); // re-init to pick up changes
// restore axis jerk
for (uint8_t axis = 0; axis < AXES; axis++ )
static stat_t _probing_error_exit(int8_t axis) {
// Generate the warning message.
- if (axis == -2) fprintf_P(stderr, PSTR("Probing error - invalid probe destination"));
- else fprintf_P(stderr, PSTR("Probing error - %c axis cannot move during probing"),
+ if (axis == -2)
+ fprintf_P(stderr, PSTR("Probing error - invalid probe destination"));
+ else fprintf_P(stderr,
+ PSTR("Probing error - %c axis cannot move during probing"),
cm_get_axis_char(axis));
// clean up and exit
_probe_restore_settings();
+
return STAT_PROBE_CYCLE_FAILED;
}
else { *block_delete_flag = false; }
// normalize the command block & find the comment (if any)
- for (; *wr != 0; rd++)
- if (*rd == 0) *wr = 0;
+ for (; *wr; rd++)
+ if (!*rd) *wr = 0;
else if (*rd == '(' || *rd == ';') {*wr = 0; *com = rd + 1;}
else if (isalnum((char)*rd) || strchr("-.", *rd)) // all valid characters
*wr++ = (char)toupper((char)*rd);
// Perform Octal stripping - remove invalid leading zeros in number strings
rd = str;
- while (*rd != 0) {
+ while (*rd) {
if (*rd == '.') break; // don't strip past a decimal point
if (!isdigit(*rd) && *(rd + 1) == '0' && isdigit(*(rd + 2))) {
wr = rd + 1;
- while (*wr != 0) {*wr = *(wr + 1); wr++;} // copy forward w/overwrite
+ while (*wr) {*wr = *(wr + 1); wr++;} // copy forward w/overwrite
continue;
}
}
// process comments and messages
- if (**com != 0) {
+ if (**com) {
rd = *com;
while (isspace(*rd)) rd++; // skip any leading spaces before "msg"
tolower(*(rd + 2)) == 'g')
*msg = rd + 3;
- for (; *rd != 0; rd++)
+ for (; *rd; rd++)
// 0 terminate on trailing parenthesis, if any
if (*rd == ')') *rd = 0;
}
* octal. G0X... is not interpreted as hexadecimal. This is trapped.
*/
static stat_t _get_next_gcode_word(char **pstr, char *letter, float *value) {
- if (**pstr == 0) return STAT_COMPLETE; // no more words to process
+ if (!**pstr) return STAT_COMPLETE; // no more words to process
// get letter part
if (!isupper(**pstr)) return STAT_INVALID_OR_MALFORMED_COMMAND;
}
-/*
- * Execute parsed block
+/* Execute parsed block
*
* Conditionally (based on whether a flag is set in gf) call the canonical
* machining functions in order of execution as per RS274NGC_3 table 8
cm_set_absolute_override(MODEL, false);
// do the program stops and ends : M0, M1, M2, M30, M60
- if (cm.gf.program_flow == true) {
+ if (cm.gf.program_flow) {
if (cm.gn.program_flow == PROGRAM_STOP) cm_program_stop();
else cm_program_end();
}
*
* There are 2 GPIO ports:
*
- * gpio1 Located on 5x2 header next to the PDI programming plugs (on v7's)
+ * gpio1 Located on 5x2 header next to the PDI programming plugs
* Four (4) output bits capable of driving 3.3v or 5v logic
*
* Note: On v6 and earlier boards there are also 4 inputs:
* **** These bits CANNOT be used as 5v inputs ****
*/
-#include "controller.h"
-#include "hardware.h"
#include "gpio.h"
-
-#include <avr/interrupt.h>
-
-
-void indicator_led_set() {
- gpio_led_on(INDICATOR_LED);
-}
-
-
-void indicator_led_clear() {
- gpio_led_off(INDICATOR_LED);
-}
-
-
-void indicator_led_toggle() {
- gpio_led_toggle(INDICATOR_LED);
-}
-
-
-void gpio_led_on(uint8_t led) {
- gpio_set_bit_on(8 >> led);
-}
-
-
-void gpio_led_off(uint8_t led) {
- gpio_set_bit_off(8 >> led);
-}
+#include "hardware.h"
-void gpio_led_toggle(uint8_t led) {
- gpio_set_bit_toggle(8 >> led);
-}
+void indicator_led_set() {gpio_led_on(INDICATOR_LED);}
+void indicator_led_clear() {gpio_led_off(INDICATOR_LED);}
+void indicator_led_toggle() {gpio_led_toggle(INDICATOR_LED);}
+void gpio_led_on(uint8_t led) {gpio_set_bit_on(8 >> led);}
+void gpio_led_off(uint8_t led) {gpio_set_bit_off(8 >> led);}
+void gpio_led_toggle(uint8_t led) {gpio_set_bit_toggle(8 >> led);}
uint8_t gpio_read_bit(uint8_t b) {
#pragma once
-
#include <stdint.h>
+
+enum {SPINDLE_LED, SPINDLE_DIR_LED, SPINDLE_PWM_LED, COOLANT_LED};
+
void indicator_led_set();
void indicator_led_clear();
void indicator_led_toggle();
#include <stdbool.h>
+hwSingleton_t hw;
+
+
/// Bind XMEGA ports to hardware
static void _port_bindings() {
hw.st_port[0] = &PORT_MOTOR_1;
}
-/*
- * Get a human readable signature
+/* Get a human readable device signature
*
* Produce a unique deviceID based on the factory calibration data.
+ *
* Format is: 123456-ABC
*
* The number part is a direct readout of the 6 digit lot number
/// software hard reset using the watchdog timer
void hw_hard_reset() {
wdt_enable(WDTO_15MS);
- while (true); // loops for about 15ms then resets
+ while (true) continue; // loops for about 15ms then resets
}
#pragma once
-
#include "status.h"
#include "config.h"
PORT_t *sw_port[MOTORS]; // bindings for switch ports (GPIO2)
PORT_t *out_port[MOTORS]; // bindings for output ports (GPIO1)
} hwSingleton_t;
-hwSingleton_t hw;
+extern hwSingleton_t hw;
+
void hardware_init();
void hw_get_id(char *id);
*/
/* This module actually contains some parts that belong ion the
- * canonical machine, * and other parts that belong at the motion planner
+ * canonical machine, and other parts that belong at the motion planner
* level, but the whole thing is * treated as if it were part of the
* motion planner.
*/
void cm_arc_init() {}
-/*
- * Canonical machine entry point for arc
+/* Canonical machine entry point for arc
*
* Generates an arc by queuing line segments to the move buffer. The arc is
* approximated by generating a large number of tiny, linear arc_segments.
*/
-stat_t cm_arc_feed(float target[], float flags[], // arc endpoints
- float i, float j, float k, // raw arc offsets
- float radius, // non-zero radius implies radius mode
- uint8_t motion_mode) { // defined motion mode
+stat_t cm_arc_feed(float target[], float flags[], // arc endpoints
+ float i, float j, float k, // raw arc offsets
+ float radius, // non-zero radius implies radius mode
+ uint8_t motion_mode) { // defined motion mode
// Set axis plane and trap arc specification errors
// trap missing feed rate
- if ((cm.gm.feed_rate_mode != INVERSE_TIME_MODE) && (fp_ZERO(cm.gm.feed_rate)))
+ if (cm.gm.feed_rate_mode != INVERSE_TIME_MODE && fp_ZERO(cm.gm.feed_rate))
return STAT_GCODE_FEEDRATE_NOT_SPECIFIED;
// 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
+ bool radius_f = fp_NOT_ZERO(cm.gf.arc_radius); // set true if radius arc
+ // radius value must be + and > minimum radius
+ if (radius_f && cm.gn.arc_radius < MIN_ARC_RADIUS)
return STAT_ARC_RADIUS_OUT_OF_TOLERANCE;
// setup some flags
- bool target_x = fp_NOT_ZERO(flags[AXIS_X]); // set true if X axis has been specified
+ bool target_x = fp_NOT_ZERO(flags[AXIS_X]); // is X axis specified
bool target_y = fp_NOT_ZERO(flags[AXIS_Y]);
bool target_z = fp_NOT_ZERO(flags[AXIS_Z]);
- bool offset_i = fp_NOT_ZERO(cm.gf.arc_offset[0]); // set true if offset I has been specified
- bool offset_j = fp_NOT_ZERO(cm.gf.arc_offset[1]); // J
- bool offset_k = fp_NOT_ZERO(cm.gf.arc_offset[2]); // K
+ bool offset_i = fp_NOT_ZERO(cm.gf.arc_offset[0]); // is offset I specified
+ bool offset_j = fp_NOT_ZERO(cm.gf.arc_offset[1]); // J
+ bool offset_k = fp_NOT_ZERO(cm.gf.arc_offset[2]); // K
- // Set the arc plane for the current G17/G18/G19 setting and test arc specification
- // Plane axis 0 and 1 are the arc plane, the linear axis is normal to the arc plane.
- if (cm.gm.select_plane == CANON_PLANE_XY) { // G17 - the vast majority of arcs are in the G17 (XY) plane
+ // Set the arc plane for the current G17/G18/G19 setting and test arc
+ // specification Plane axis 0 and 1 are the arc plane, the linear axis is
+ // normal to the arc plane.
+ // G17 - the vast majority of arcs are in the G17 (XY) plane
+ if (cm.gm.select_plane == CANON_PLANE_XY) {
arc.plane_axis_0 = AXIS_X;
arc.plane_axis_1 = AXIS_Y;
arc.linear_axis = AXIS_Z;
arc.linear_axis = AXIS_X;
if (radius_f) {
- if (!(target_y || target_z)) return STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE;
+ if (!target_y && !target_z)
+ return STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE;
} else if (offset_i) return STAT_ARC_SPECIFICATION_ERROR;
}
- // set values in the Gcode model state & copy it (linenum was already captured)
+ // set values in the Gcode model state & copy it (linenum was already
+ // captured)
cm_set_model_target(target, flags);
// in radius mode it's an error for start == end
// now get down to the rest of the work setting up the arc for execution
cm.gm.motion_mode = motion_mode;
- cm_set_work_offsets(&cm.gm); // capture the fully resolved offsets to gm
- memcpy(&arc.gm, &cm.gm, sizeof(GCodeState_t)); // copy GCode context to arc singleton
- // some will be overwritten to run segments
- copy_vector(arc.position, cm.gmx.position); // set initial arc position from gcode model
+ cm_set_work_offsets(&cm.gm); // resolved offsets to gm
+ memcpy(&arc.gm, &cm.gm, sizeof(GCodeState_t)); // context to arc singleton
+
+ copy_vector(arc.position, cm.gmx.position); // arc pos from gcode model
arc.radius = _to_millimeters(radius); // set arc radius or zero
- arc.offset[0] = _to_millimeters(i); // copy offsets with conversion to canonical form (mm)
+ arc.offset[0] = _to_millimeters(i); // offsets canonical form (mm)
arc.offset[1] = _to_millimeters(j);
arc.offset[2] = _to_millimeters(k);
- arc.rotations = floor(fabs(cm.gn.parameter)); // P must be a positive integer - force it if not
+ arc.rotations = floor(fabs(cm.gn.parameter)); // P must be positive integer
// determine if this is a full circle arc. Evaluates true if no target is set
- arc.full_circle = (fp_ZERO(flags[arc.plane_axis_0]) & fp_ZERO(flags[arc.plane_axis_1]));
+ arc.full_circle =
+ fp_ZERO(flags[arc.plane_axis_0]) & fp_ZERO(flags[arc.plane_axis_1]);
// compute arc runtime values
ritorno(_compute_arc());
if (fp_ZERO(arc.length)) return STAT_MINIMUM_LENGTH_MOVE;
cm_cycle_start(); // if not already started
- arc.run_state = MOVE_RUN; // enable arc to be run from the callback
+ arc.run_state = MOVE_RUN; // enable arc run from the callback
cm_finalize_move();
return STAT_OK;
}
-/*
- * Generate an arc
+/* Generate an arc
*
* Called from the controller main loop. Each time it's called it queues
* as many arc segments (lines) as it can before it blocks, then returns.
*/
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;
+ if (mp_get_planner_buffers_available() < PLANNER_BUFFER_HEADROOM)
+ return STAT_EAGAIN;
arc.theta += arc.arc_segment_theta;
arc.gm.target[arc.plane_axis_0] = arc.center_0 + sin(arc.theta) * arc.radius;
arc.gm.target[arc.plane_axis_1] = arc.center_1 + cos(arc.theta) * arc.radius;
arc.gm.target[arc.linear_axis] += arc.arc_segment_linear_travel;
mp_aline(&arc.gm); // run the line
- copy_vector(arc.position, arc.gm.target); // update arc current position
+ copy_vector(arc.position, arc.gm.target); // update arc current pos
if (--arc.arc_segment_count > 0) return STAT_EAGAIN;
}
-/*
- * Compute arc from I and J (arc center point)
+/* Compute arc from I and J (arc center point)
*
- * The theta calculation sets up an clockwise or counterclockwise arc from the current
- * position to the target position around the center designated by the offset vector.
- * All theta-values measured in radians of deviance from the positive y-axis.
+ * The theta calculation sets up an clockwise or counterclockwise arc
+ * from the current position to the target position around the center
+ * designated by the offset vector. All theta-values measured in
+ * radians of deviance from the positive y-axis.
*
- * | <- theta == 0
- * * * *
- * * *
- * * *
- * * O ----T <- theta_end (e.g. 90 degrees: theta_end == PI/2)
- * * /
- * C <- theta_start (e.g. -145 degrees: theta_start == -PI*(3/4))
+ * | <- theta == 0
+ * * * *
+ * * *
+ * * *
+ * * O ----T <- theta_end (e.g. 90 degrees: theta_end == PI/2)
+ * * /
+ * C <- theta_start (e.g. -145 degrees: theta_start == -PI*(3/4))
*
* Parts of this routine were originally sourced from the grbl project.
*/
static stat_t _compute_arc() {
// Compute radius. A non-zero radius value indicates a radius arc
- if (fp_NOT_ZERO(arc.radius)) _compute_arc_offsets_from_radius(); // indicates a radius arc
+ if (fp_NOT_ZERO(arc.radius))
+ _compute_arc_offsets_from_radius(); // indicates a radius arc
else // compute start radius
- arc.radius = hypotf(-arc.offset[arc.plane_axis_0], -arc.offset[arc.plane_axis_1]);
+ arc.radius =
+ hypotf(-arc.offset[arc.plane_axis_0], -arc.offset[arc.plane_axis_1]);
// Test arc specification for correctness according to:
// http://linuxcnc.org/docs/html/gcode/gcode.html#sec:G2-G3-Arc
- // "It is an error if: when the arc is projected on the selected plane, the distance from
- // the current point to the center differs from the distance from the end point to the
- // center by more than (.05 inch/.5 mm) OR ((.0005 inch/.005mm) AND .1% of radius)."
+ // "It is an error if: when the arc is projected on the selected
+ // plane, the distance from the current point to the center differs
+ // from the distance from the end point to the center by more than
+ // (.05 inch/.5 mm) OR ((.0005 inch/.005mm) AND .1% of radius)."
// Compute end radius from the center of circle (offsets) to target endpoint
- float end_0 = arc.gm.target[arc.plane_axis_0] - arc.position[arc.plane_axis_0] - arc.offset[arc.plane_axis_0];
- float end_1 = arc.gm.target[arc.plane_axis_1] - arc.position[arc.plane_axis_1] - arc.offset[arc.plane_axis_1];
- float err = fabs(hypotf(end_0, end_1) - arc.radius); // end radius - start radius
-
- if ((err > ARC_RADIUS_ERROR_MAX) ||
- ((err < ARC_RADIUS_ERROR_MIN) && (err > arc.radius * ARC_RADIUS_TOLERANCE)))
+ float end_0 = arc.gm.target[arc.plane_axis_0] -
+ arc.position[arc.plane_axis_0] - arc.offset[arc.plane_axis_0];
+ float end_1 = arc.gm.target[arc.plane_axis_1] -
+ arc.position[arc.plane_axis_1] - arc.offset[arc.plane_axis_1];
+ // end radius - start radius
+ float err = fabs(hypotf(end_0, end_1) - arc.radius);
+
+ if (err > ARC_RADIUS_ERROR_MAX ||
+ (err < ARC_RADIUS_ERROR_MIN && err > arc.radius * ARC_RADIUS_TOLERANCE))
return STAT_ARC_SPECIFICATION_ERROR;
// Calculate the theta (angle) of the current point (position)
- // arc.theta is angular starting point for the arc (also needed later for calculating center point)
- arc.theta = atan2(-arc.offset[arc.plane_axis_0], -arc.offset[arc.plane_axis_1]);
+ // arc.theta is angular starting point for the arc (also needed later for
+ // calculating center point)
+ arc.theta = atan2(-arc.offset[arc.plane_axis_0],
+ -arc.offset[arc.plane_axis_1]);
- // g18_correction is used to invert G18 XZ plane arcs for proper CW orientation
+ // g18_correction is used to invert G18 XZ plane arcs for proper CW
+ // orientation
float g18_correction = (cm.gm.select_plane == CANON_PLANE_XZ) ? -1 : 1;
- if (arc.full_circle) { // if full circle you can skip the stuff in the else clause
- arc.angular_travel = 0; // angular travel always starts as zero for full circles
- if (fp_ZERO(arc.rotations)) arc.rotations = 1.0; // handle the valid case of a full circle arc w/P=0
+ if (arc.full_circle) {
+ // angular travel always starts as zero for full circles
+ arc.angular_travel = 0;
+ // handle the valid case of a full circle arc w/P=0
+ if (fp_ZERO(arc.rotations)) arc.rotations = 1.0;
- } else { // ... it's not a full circle
+ } else {
arc.theta_end = atan2(end_0, end_1);
// Compute the angular travel
if (fp_EQ(arc.theta_end, arc.theta))
- arc.angular_travel = 0; // very large radii arcs can have zero angular travel (thanks PartKam)
+ // very large radii arcs can have zero angular travel (thanks PartKam)
+ arc.angular_travel = 0;
else {
- if (arc.theta_end < arc.theta) // make the difference positive so we have clockwise travel
+ // make the difference positive so we have clockwise travel
+ if (arc.theta_end < arc.theta)
arc.theta_end += 2 * M_PI * g18_correction;
- arc.angular_travel = arc.theta_end - arc.theta; // compute positive angular travel
+ // compute positive angular travel
+ arc.angular_travel = arc.theta_end - arc.theta;
- if (cm.gm.motion_mode == MOTION_MODE_CCW_ARC) // reverse travel direction if it's CCW arc
+ // reverse travel direction if it's CCW arc
+ if (cm.gm.motion_mode == MOTION_MODE_CCW_ARC)
arc.angular_travel -= 2 * M_PI * g18_correction;
}
}
arc.angular_travel += (2*M_PI * arc.rotations * g18_correction);
else arc.angular_travel -= (2*M_PI * arc.rotations * g18_correction);
- // Calculate travel in the depth axis of the helix and compute the time it should take to perform the move
- // arc.length is the total mm of travel of the helix (or just a planar arc)
- arc.linear_travel = arc.gm.target[arc.linear_axis] - arc.position[arc.linear_axis];
+ // Calculate travel in the depth axis of the helix and compute the time it
+ // should take to perform the move arc.length is the total mm of travel of
+ // the helix (or just a planar arc)
+ arc.linear_travel =
+ arc.gm.target[arc.linear_axis] - arc.position[arc.linear_axis];
arc.planar_travel = arc.angular_travel * arc.radius;
- arc.length = hypotf(arc.planar_travel, arc.linear_travel); // hypot is insensitive to +/- signs
- _estimate_arc_time(); // get an estimate of execution time to inform arc_segment calculation
+ // hypot is insensitive to +/- signs
+ arc.length = hypotf(arc.planar_travel, arc.linear_travel);
+ // get an estimate of execution time to inform arc_segment calculation
+ _estimate_arc_time();
// Find the minimum number of arc_segments that meets these constraints...
float arc_segments_for_chordal_accuracy =
- arc.length / sqrt(4*cm.chordal_tolerance * (2 * arc.radius - cm.chordal_tolerance));
+ arc.length / sqrt(4 * cm.chordal_tolerance *
+ (2 * arc.radius - cm.chordal_tolerance));
float arc_segments_for_minimum_distance = arc.length / cm.arc_segment_len;
float arc_segments_for_minimum_time =
arc.arc_time * MICROSECONDS_PER_MINUTE / MIN_ARC_SEGMENT_USEC;
arc_segments_for_minimum_distance,
arc_segments_for_minimum_time));
- arc.arc_segments = max(arc.arc_segments, 1); //...but is at least 1 arc_segment
- arc.gm.move_time = arc.arc_time / arc.arc_segments; // gcode state struct gets arc_segment_time, not arc time
+ arc.arc_segments = max(arc.arc_segments, 1); // at least 1 arc_segment
+ // gcode state struct gets arc_segment_time, not arc time
+ arc.gm.move_time = arc.arc_time / arc.arc_segments;
arc.arc_segment_count = (int32_t)arc.arc_segments;
arc.arc_segment_theta = arc.angular_travel / arc.arc_segments;
arc.arc_segment_linear_travel = arc.linear_travel / arc.arc_segments;
arc.center_0 = arc.position[arc.plane_axis_0] - sin(arc.theta) * arc.radius;
arc.center_1 = arc.position[arc.plane_axis_1] - cos(arc.theta) * arc.radius;
- arc.gm.target[arc.linear_axis] = arc.position[arc.linear_axis]; // initialize the linear target
+ // initialize the linear target
+ arc.gm.target[arc.linear_axis] = arc.position[arc.linear_axis];
return STAT_OK;
}
-/*
- * Compute arc center (offset) from radius.
+/* Compute arc center (offset) from radius.
*
- * Needs to calculate the center of the circle that has the designated radius and
- * passes through both the current position and the target position
+ * Needs to calculate the center of the circle that has the designated radius
+ * and passes through both the current position and the target position
*
- * This method calculates the following set of equations where:
- * ` [x,y] is the vector from current to target position,
- * d == magnitude of that vector,
- * h == hypotenuse of the triangle formed by the radius of the circle,
- * the distance to the center of the travel vector.
+ * This method calculates the following set of equations where:
*
- * A vector perpendicular to the travel vector [-y,x] is scaled to the length
- * of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2]
- * to form the new point [i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the
- * center of the arc.
+ * [x,y] is the vector from current to target position,
+ * d == magnitude of that vector,
+ * h == hypotenuse of the triangle formed by the radius of the circle,
+ * the distance to the center of the travel vector.
+ *
+ * A vector perpendicular to the travel vector [-y,x] is scaled to the length
+ * of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2]
+ * to form the new point [i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the
+ * center of the arc.
*
* d^2 == x^2 + y^2
* h^2 == r^2 - (d/2)^2
* r - designated radius
* h - distance from center of CT to O
*
- * Expanding the equations:
+ * Expanding the equations:
+ *
* d -> sqrt(x^2 + y^2)
* h -> sqrt(4 * r^2 - x^2 - y^2)/2
* i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
* j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2
*
- * Which can be written:
+ * Which can be written:
+ *
* i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
* j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2
*
- * Which we for size and speed reasons optimize to:
+ * Which we for size and speed reasons optimize to:
+ *
* h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2)
* i = (x - (y * h_x2_div_d))/2
* j = (y + (x * h_x2_div_d))/2
*
- * ----Computing clockwise vs counter-clockwise motion ----
+ * Computing clockwise vs counter-clockwise motion
*
- * The counter clockwise circle lies to the left of the target direction.
- * When offset is positive the left hand circle will be generated -
- * when it is negative the right hand circle is generated.
+ * The counter clockwise circle lies to the left of the target direction.
+ * When offset is positive the left hand circle will be generated -
+ * when it is negative the right hand circle is generated.
*
* T <-- Target position
*
// If the distance between endpoints is greater than the arc diameter, disc
// will be negative indicating that the arc is offset into the complex plane
// beyond the reach of any real CNC. However, numerical errors can flip the
- // sign of disc as it approaches zero (which happens as the arc angle approaches
- // 180 degrees). To avoid mishandling these arcs we use the closest real
- // solution (which will be 0 when disc <= 0). This risks obscuring g-code errors
- // where the radius is actually too small (they will be treated as half circles),
- // but ensures that all valid arcs end up reasonably close to their intended
- // paths regardless of any numerical issues.
+ // sign of disc as it approaches zero (which happens as the arc angle
+ // approaches 180 degrees). To avoid mishandling these arcs we use the
+ // closest real solution (which will be 0 when disc <= 0). This risks
+ // obscuring g-code errors where the radius is actually too small (they will
+ // be treated as half circles), but ensures that all valid arcs end up
+ // reasonably close to their intended paths regardless of any numerical
+ // issues.
float disc = 4 * square(arc.radius) - (square(x) + square(y));
- // h_x2_div_d == -(h * 2 / d)
float h_x2_div_d = (disc > 0) ? -sqrt(disc) / hypotf(x, y) : 0;
- // Invert the sign of h_x2_div_d if circle is counter clockwise (see header notes)
+ // Invert the sign of h_x2_div_d if circle is counter clockwise (see header
+ // notes)
if (cm.gm.motion_mode == MOTION_MODE_CCW_ARC) h_x2_div_d = -h_x2_div_d;
// Negative R is g-code-alese for "I want a circle with more than 180 degrees
}
-/*
- * Returns a naiive estimate of arc execution time to inform segment calculation.
- * The arc time is computed not to exceed the time taken in the slowest dimension
- * in the arc plane or in linear travel. Maximum feed rates are compared in each
- * dimension, but the comparison assumes that the arc will have at least one segment
- * where the unit vector is 1 in that dimension. This is not true for any arbitrary arc,
- * with the result that the time returned may be less than optimal.
+/* Returns a naiive estimate of arc execution time to inform segment
+ * calculation. The arc time is computed not to exceed the time taken
+ * in the slowest dimension in the arc plane or in linear
+ * travel. Maximum feed rates are compared in each dimension, but the
+ * comparison assumes that the arc will have at least one segment
+ * where the unit vector is 1 in that dimension. This is not true for
+ * any arbitrary arc, with the result that the time returned may be
+ * less than optimal.
*/
static void _estimate_arc_time() {
// Determine move time at requested feed rate
if (cm.gm.feed_rate_mode == INVERSE_TIME_MODE) {
- arc.arc_time = cm.gm.feed_rate; // inverse feed rate has been normalized to minutes
- cm.gm.feed_rate = 0; // reset feed rate so next block requires an explicit feed rate setting
+ // inverse feed rate has been normalized to minutes
+ arc.arc_time = cm.gm.feed_rate;
+ // reset feed rate so next block requires an explicit feed rate setting
+ cm.gm.feed_rate = 0;
cm.gm.feed_rate_mode = UNITS_PER_MINUTE_MODE;
} else arc.arc_time = arc.length / cm.gm.feed_rate;
// Downgrade the time if there is a rate-limiting axis
- arc.arc_time = max(arc.arc_time, arc.planar_travel/cm.a[arc.plane_axis_0].feedrate_max);
- arc.arc_time = max(arc.arc_time, arc.planar_travel/cm.a[arc.plane_axis_1].feedrate_max);
-
- if (fabs(arc.linear_travel) > 0)
- arc.arc_time = max(arc.arc_time, fabs(arc.linear_travel/cm.a[arc.linear_axis].feedrate_max));
+ arc.arc_time =
+ max(arc.arc_time, arc.planar_travel/cm.a[arc.plane_axis_0].feedrate_max);
+ arc.arc_time =
+ max(arc.arc_time, arc.planar_travel/cm.a[arc.plane_axis_1].feedrate_max);
+
+ if (0 < fabs(arc.linear_travel))
+ arc.arc_time =
+ max(arc.arc_time,
+ fabs(arc.linear_travel/cm.a[arc.linear_axis].feedrate_max));
}
*/
mpBuf_t *mp_get_run_buffer() {
// CASE: fresh buffer; becomes running if queued or pending
- if ((mb.r->buffer_state == MP_BUFFER_QUEUED) ||
- (mb.r->buffer_state == MP_BUFFER_PENDING))
+ if (mb.r->buffer_state == MP_BUFFER_QUEUED ||
+ mb.r->buffer_state == MP_BUFFER_PENDING)
mb.r->buffer_state = MP_BUFFER_RUNNING;
// CASE: asking for the same run buffer for the Nth time
/// Queue a synchronous Mcode, program control, or other command
-void mp_queue_command(void(*cm_exec)(float[], float[]), float *value,
- float *flag) {
+void mp_queue_command(cm_exec_t cm_exec, float *value, float *flag) {
mpBuf_t *bf;
// Never supposed to fail as buffer availability was checked upstream in the
*
* 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_segment() - helper for running a segment
- *
* Returns:
* STAT_OK move is done
* STAT_EAGAIN move is not finished - has more segments to run
*
* The equations that govern the acceleration and deceleration ramps are:
*
- * Period 1 V = Vi + Jm*(T^2)/2
- * Period 2 V = Vh + As*T - Jm*(T^2)/2
- * Period 3 V = Vi - Jm*(T^2)/2
- * Period 4 V = Vh + As*T + Jm*(T^2)/2
+ * Period 1 V = Vi + Jm*(T^2)/2
+ * Period 2 V = Vh + As*T - Jm*(T^2)/2
+ * Period 3 V = Vi - Jm*(T^2)/2
+ * Period 4 V = Vh + As*T + Jm*(T^2)/2
*
* These routines play some games with the acceleration and move timing
* to make sure this actually all works out. move_time is the actual time of
* or just remains _OFF
*
* mr.move_state transitions on first call from _OFF to one of _HEAD, _BODY,
- * _TAIL
- * Within each section state may be
- * _NEW - trigger initialization
- * _RUN1 - run the first part
- * _RUN2 - run the second part
- *
- * Note: For a direct math implementation see build 357.xx or earlier.
- * Builds 358 onward have only forward difference code.
+ * _TAIL. Within each section state may be:
+ *
+ * _NEW - trigger initialization
+ * _RUN1 - run the first part
+ * _RUN2 - run the second part
*/
stat_t mp_exec_aline(mpBuf_t *bf) {
if (bf->move_state == MOVE_OFF) return STAT_NOOP;
return status;
}
+/// Helper for cruise section
+/// The body is broken into little segments even though it is a
+/// straight line so that feedholds can happen in the middle of a line
+/// with a minimum of latency
+static stat_t _exec_aline_body() {
+ if (mr.section_state == SECTION_NEW) {
+ if (fp_ZERO(mr.body_length)) {
+ mr.section = SECTION_TAIL;
+ return _exec_aline_tail(); // skip ahead to tail periods
+ }
+
+ mr.gm.move_time = mr.body_length / mr.cruise_velocity;
+ mr.segments = ceil(uSec(mr.gm.move_time) / NOM_SEGMENT_USEC);
+ mr.segment_time = mr.gm.move_time / mr.segments;
+ mr.segment_velocity = mr.cruise_velocity;
+ mr.segment_count = (uint32_t)mr.segments;
+
+ if (mr.segment_time < MIN_SEGMENT_TIME)
+ return STAT_MINIMUM_TIME_MOVE; // exit without advancing position
+
+ mr.section = SECTION_BODY;
+ // uses PERIOD_2 so last segment detection works
+ mr.section_state = SECTION_2nd_HALF;
+ }
+
+ if (mr.section_state == SECTION_2nd_HALF) // straight part (period 3)
+ if (_exec_aline_segment() == STAT_OK) { // OK means this section is done
+ if (fp_ZERO(mr.tail_length)) return STAT_OK; // ends the move
+ mr.section = SECTION_TAIL;
+ mr.section_state = SECTION_NEW;
+ }
+
+ return STAT_EAGAIN;
+}
+
+#ifdef __JERK_EXEC
+/// Helper for acceleration section
+static stat_t _exec_aline_head() {
+ if (mr.section_state == SECTION_NEW) { // initialize the move singleton (mr)
+ if (fp_ZERO(mr.head_length)) {
+ mr.section = SECTION_BODY;
+ return _exec_aline_body(); // skip ahead to the body generator
+ }
+
+ mr.midpoint_velocity = (mr.entry_velocity + mr.cruise_velocity) / 2;
+ // time for entire accel region
+ mr.gm.move_time = mr.head_length / mr.midpoint_velocity;
+ // # of segments in *each half*
+ mr.segments = ceil(uSec(mr.gm.move_time) / (2 * NOM_SEGMENT_USEC));
+ mr.segment_time = mr.gm.move_time / (2 * mr.segments);
+ mr.accel_time =
+ 2 * sqrt((mr.cruise_velocity - mr.entry_velocity) / mr.jerk);
+ mr.midpoint_acceleration =
+ 2 * (mr.cruise_velocity - mr.entry_velocity) / mr.accel_time;
+ // time to advance for each segment
+ mr.segment_accel_time = mr.accel_time / (2 * mr.segments);
+ // elapsed time starting point (offset)
+ mr.elapsed_accel_time = mr.segment_accel_time / 2;
+ mr.segment_count = (uint32_t)mr.segments;
+
+ if (mr.segment_time < MIN_SEGMENT_TIME)
+ return STAT_MINIMUM_TIME_MOVE; // exit without advancing position
+
+ mr.section = SECTION_HEAD;
+ mr.section_state = SECTION_1st_HALF;
+ }
+
+ // First half (concave part of accel curve)
+ if (mr.section_state == SECTION_1st_HALF) {
+ mr.segment_velocity =
+ mr.entry_velocity + (square(mr.elapsed_accel_time) * mr.jerk_div2);
+
+ if (_exec_aline_segment() == STAT_OK) { // set up for second half
+ mr.segment_count = (uint32_t)mr.segments;
+ mr.section_state = SECTION_2nd_HALF;
+ // start time from midpoint of segment
+ mr.elapsed_accel_time = mr.segment_accel_time / 2;
+ }
+
+ return STAT_EAGAIN;
+ }
+
+ // Second half (convex part of accel curve)
+ if (mr.section_state == SECTION_2nd_HALF) {
+ mr.segment_velocity = mr.midpoint_velocity +
+ (mr.elapsed_accel_time * mr.midpoint_acceleration) -
+ (square(mr.elapsed_accel_time) * mr.jerk_div2);
+
+ if (_exec_aline_segment() == STAT_OK) { // OK means this section is done
+ if (fp_ZERO(mr.body_length) && fp_ZERO(mr.tail_length))
+ return STAT_OK; // ends the move
+
+ mr.section = SECTION_BODY;
+ mr.section_state = SECTION_NEW;
+ }
+ }
+
+ return STAT_EAGAIN;
+}
+
+
+/// helper for deceleration section
+static stat_t _exec_aline_tail() {
+ if (mr.section_state == SECTION_NEW) { // INITIALIZATION
+ if (fp_ZERO(mr.tail_length)) return STAT_OK; // end the move
+
+ mr.midpoint_velocity = (mr.cruise_velocity + mr.exit_velocity) / 2;
+ mr.gm.move_time = mr.tail_length / mr.midpoint_velocity;
+ // # of segments in *each half*
+ mr.segments = ceil(uSec(mr.gm.move_time) / (2 * NOM_SEGMENT_USEC));
+ // time to advance for each segment
+ mr.segment_time = mr.gm.move_time / (2 * mr.segments);
+ mr.accel_time = 2 * sqrt((mr.cruise_velocity - mr.exit_velocity) / mr.jerk);
+ mr.midpoint_acceleration =
+ 2 * (mr.cruise_velocity - mr.exit_velocity) / mr.accel_time;
+ // time to advance for each segment
+ mr.segment_accel_time = mr.accel_time / (2 * mr.segments);
+ // compute time from midpoint of segment
+ mr.elapsed_accel_time = mr.segment_accel_time / 2;
+ mr.segment_count = (uint32_t)mr.segments;
+ if (mr.segment_time < MIN_SEGMENT_TIME)
+ return STAT_MINIMUM_TIME_MOVE; // exit /wo advancing position
+ mr.section = SECTION_TAIL;
+ mr.section_state = SECTION_1st_HALF;
+ }
+
+ // FIRST HALF - convex part (period 4)
+ if (mr.section_state == SECTION_1st_HALF) {
+ mr.segment_velocity =
+ mr.cruise_velocity - (square(mr.elapsed_accel_time) * mr.jerk_div2);
+
+ if (_exec_aline_segment() == STAT_OK) { // set up for second half
+ mr.segment_count = (uint32_t)mr.segments;
+ mr.section_state = SECTION_2nd_HALF;
+ // start time from midpoint of segment
+ mr.elapsed_accel_time = mr.segment_accel_time / 2;
+ }
+
+ return STAT_EAGAIN;
+ }
+
+ // SECOND HALF - concave part (period 5)
+ if (mr.section_state == SECTION_2nd_HALF) {
+ mr.segment_velocity = mr.midpoint_velocity -
+ (mr.elapsed_accel_time * mr.midpoint_acceleration) +
+ (square(mr.elapsed_accel_time) * mr.jerk_div2);
+ return _exec_aline_segment(); // ends the move or continues EAGAIN
+ }
+
+ return STAT_EAGAIN; // should never get here
+}
+
+
+#else // __JERK_EXEC
/* Forward difference math explained:
*
* We are using a quintic (fifth-degree) Bezier polynomial for the
*
* The Bezier curve takes the form:
*
- * V(t) = P_0 * B_0(t) + P_1 * B_1(t) + P_2 * B_2(t) + P_3 * B_3(t) +
- * P_4 * B_4(t) + P_5 * B_5(t)
+ * V(t) = P_0 * B_0(t) + P_1 * B_1(t) + P_2 * B_2(t) + P_3 * B_3(t) +
+ * P_4 * B_4(t) + P_5 * B_5(t)
*
* Where 0 <= t <= 1, and V(t) is the velocity. P_0 through P_5 are
* the control points, and B_0(t) through B_5(t) are the Bernstein
* basis as follows:
*
- * B_0(t) = (1-t)^5 = -t^5 + 5t^4 - 10t^3 + 10t^2 - 5t + 1
- * B_1(t) = 5(1-t)^4 * t = 5t^5 - 20t^4 + 30t^3 - 20t^2 + 5t
- * B_2(t) = 10(1-t)^3 * t^2 = -10t^5 + 30t^4 - 30t^3 + 10t^2
- * B_3(t) = 10(1-t)^2 * t^3 = 10t^5 - 20t^4 + 10t^3
- * B_4(t) = 5(1-t) * t^4 = -5t^5 + 5t^4
- * B_5(t) = t^5 = t^5
- * ^ ^ ^ ^ ^ ^
- * | | | | | |
- * A B C D E F
- *
- * We use forward-differencing to calculate each position through the curve.
+ * B_0(t) = (1 - t)^5 = -t^5 + 5t^4 - 10t^3 + 10t^2 - 5t + 1
+ * B_1(t) = 5(1 - t)^4 * t = 5t^5 - 20t^4 + 30t^3 - 20t^2 + 5t
+ * B_2(t) = 10(1 - t)^3 * t^2 = -10t^5 + 30t^4 - 30t^3 + 10t^2
+ * B_3(t) = 10(1 - t)^2 * t^3 = 10t^5 - 20t^4 + 10t^3
+ * B_4(t) = 5(1 - t) * t^4 = -5t^5 + 5t^4
+ * B_5(t) = t^5 = t^5
+ *
+ * ^ ^ ^ ^ ^ ^
+ * A B C D E F
+ *
+ * We use forward-differencing to calculate each position through the curve.
* This requires a formula of the form:
*
- * V_f(t) = A*t^5 + B*t^4 + C*t^3 + D*t^2 + E*t + F
+ * V_f(t) = A * t^5 + B * t^4 + C * t^3 + D * t^2 + E * t + F
*
* Looking at the above B_0(t) through B_5(t) expanded forms, if we
* take the coefficients of t^5 through t of the Bezier form of V(t),
* we can determine that:
*
- * A = -P_0 + 5*P_1 - 10*P_2 + 10*P_3 - 5*P_4 + P_5
- * B = 5*P_0 - 20*P_1 + 30*P_2 - 20*P_3 + 5*P_4
- * C = -10*P_0 + 30*P_1 - 30*P_2 + 10*P_3
- * D = 10*P_0 - 20*P_1 + 10*P_2
- * E = - 5*P_0 + 5*P_1
- * F = P_0
+ * A = -P_0 + 5 * P_1 - 10 * P_2 + 10 * P_3 - 5 * P_4 + P_5
+ * B = 5 * P_0 - 20 * P_1 + 30 * P_2 - 20 * P_3 + 5 * P_4
+ * C = -10 * P_0 + 30 * P_1 - 30 * P_2 + 10 * P_3
+ * D = 10 * P_0 - 20 * P_1 + 10 * P_2
+ * E = - 5 * P_0 + 5 * P_1
+ * F = P_0
*
* Now, since we will (currently) *always* want the initial
* acceleration and jerk values to be 0, We set P_i = P_0 = P_1 =
* P_2 (initial velocity), and P_t = P_3 = P_4 = P_5 (target
* velocity), which, after simplification, resolves to:
*
- * A = - 6*P_i + 6*P_t
- * B = 15*P_i - 15*P_t
- * C = -10*P_i + 10*P_t
+ * A = - 6 * P_i + 6 * P_t
+ * B = 15 * P_i - 15 * P_t
+ * C = -10 * P_i + 10 * P_t
* D = 0
* E = 0
* F = P_i
* step-by-step here, but it gives the resulting formulas:
*
* a = A, b = B, c = C, d = D, e = E, f = F
- * F_5(t+h)-F_5(t) = (5ah)t^4 + (10ah^2 + 4bh)t^3 +
+ *
+ * F_5(t + h) - F_5(t) = (5ah)t^4 + (10ah^2 + 4bh)t^3 +
* (10ah^3 + 6bh^2 + 3ch)t^2 + (5ah^4 + 4bh^3 + 3ch^2 + 2dh)t + ah^5 +
* bh^4 + ch^3 + dh^2 + eh
*
* d = 5ah^4 + 4bh^3 + 3ch^2 + 2dh
*
* (After substitution, simplification, and rearranging):
- * F_4(t+h)-F_4(t) = (20ah^2)t^3 + (60ah^3 + 12bh^2)t^2 +
+ *
+ * F_4(t + h) - F_4(t) = (20ah^2)t^3 + (60ah^3 + 12bh^2)t^2 +
* (70ah^4 + 24bh^3 + 6ch^2)t + 30ah^5 + 14bh^4 + 6ch^3 + 2dh^2
*
- * a = (20ah^2)
- * b = (60ah^3 + 12bh^2)
- * c = (70ah^4 + 24bh^3 + 6ch^2)
+ * a = 20ah^2
+ * b = 60ah^3 + 12bh^2
+ * c = 70ah^4 + 24bh^3 + 6ch^2
*
* (After substitution, simplification, and rearranging):
+ *
* F_3(t+h)-F_3(t) = (60ah^3)t^2 + (180ah^4 + 24bh^3)t + 150ah^5 +
* 36bh^4 + 6ch^3
*
* (You get the picture...)
- * F_2(t+h)-F_2(t) = (120ah^4)t + 240ah^5 + 24bh^4
- * F_1(t+h)-F_1(t) = 120ah^5
+ *
+ * F_2(t + h) - F_2(t) = (120ah^4)t + 240ah^5 + 24bh^4
+ * F_1(t + h) - F_1(t) = 120ah^5
*
* Normally, we could then assign t = 0, use the A-F values from
* above, and get out initial F_* values. However, for the sake of
* the initial V be be at t = h/2 and iterate I-1 times. So, the
* resulting F_* values are (steps not shown):
*
- * F_5 = (121Ah^5)/16 + 5Bh^4 + (13Ch^3)/4 + 2Dh^2 + Eh
- * F_4 = (165Ah^5)/2 + 29Bh^4 + 9Ch^3 + 2Dh^2
+ * F_5 = 121Ah^5 / 16 + 5Bh^4 + 13Ch^3 / 4 + 2Dh^2 + Eh
+ * F_4 = 165Ah^5 / 2 + 29Bh^4 + 9Ch^3 + 2Dh^2
* F_3 = 255Ah^5 + 48Bh^4 + 6Ch^3
* F_2 = 300Ah^5 + 24Bh^4
* F_1 = 120Ah^5
*
* Note that with our current control points, D and E are actually 0.
*/
-#ifndef __JERK_EXEC
-
static void _init_forward_diffs(float Vi, float Vt) {
float A = -6.0 * Vi + 6.0 * Vt;
float B = 15.0 * Vi - 15.0 * Vt;
float half_Ah_5 = C * half_h * half_h * half_h * half_h * half_h;
mr.segment_velocity = half_Ah_5 + half_Bh_4 + half_Ch_3 + Vi;
}
-#endif
-
-
-#ifdef __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)) {
- mr.section = SECTION_BODY;
- return _exec_aline_body(); // skip ahead to the body generator
- }
-
- mr.midpoint_velocity = (mr.entry_velocity + mr.cruise_velocity) / 2;
- // time for entire accel region
- mr.gm.move_time = mr.head_length / mr.midpoint_velocity;
- // # of segments in *each half*
- mr.segments = ceil(uSec(mr.gm.move_time) / (2 * NOM_SEGMENT_USEC));
- mr.segment_time = mr.gm.move_time / (2 * mr.segments);
- mr.accel_time =
- 2 * sqrt((mr.cruise_velocity - mr.entry_velocity) / mr.jerk);
- mr.midpoint_acceleration =
- 2 * (mr.cruise_velocity - mr.entry_velocity) / mr.accel_time;
- // time to advance for each segment
- mr.segment_accel_time = mr.accel_time / (2 * mr.segments);
- // elapsed time starting point (offset)
- mr.elapsed_accel_time = mr.segment_accel_time / 2;
- mr.segment_count = (uint32_t)mr.segments;
-
- if (mr.segment_time < MIN_SEGMENT_TIME)
- return STAT_MINIMUM_TIME_MOVE; // exit without advancing position
-
- mr.section = SECTION_HEAD;
- mr.section_state = SECTION_1st_HALF;
- }
-
- // FIRST HALF (concave part of accel curve)
- if (mr.section_state == SECTION_1st_HALF) {
- mr.segment_velocity =
- mr.entry_velocity + (square(mr.elapsed_accel_time) * mr.jerk_div2);
-
- if (_exec_aline_segment() == STAT_OK) { // set up for second half
- mr.segment_count = (uint32_t)mr.segments;
- mr.section_state = SECTION_2nd_HALF;
- // start time from midpoint of segment
- mr.elapsed_accel_time = mr.segment_accel_time / 2;
- }
-
- return STAT_EAGAIN;
- }
-
- // SECOND HAF (convex part of accel curve)
- if (mr.section_state == SECTION_2nd_HALF) {
- mr.segment_velocity = mr.midpoint_velocity +
- (mr.elapsed_accel_time * mr.midpoint_acceleration) -
- (square(mr.elapsed_accel_time) * mr.jerk_div2);
-
- if (_exec_aline_segment() == STAT_OK) { // OK means this section is done
- if ((fp_ZERO(mr.body_length)) && (fp_ZERO(mr.tail_length)))
- return STAT_OK; // ends the move
-
- mr.section = SECTION_BODY;
- mr.section_state = SECTION_NEW;
- }
- }
-
- return STAT_EAGAIN;
-}
-#else // __ JERK_EXEC
+/// Helper for acceleration section
static stat_t _exec_aline_head() {
if (mr.section_state == SECTION_NEW) { // initialize the move singleton (mr)
if (fp_ZERO(mr.head_length)) {
// For forward differencing we should have one segment in
// SECTION_1st_HALF However, if it returns from that as STAT_OK,
// then there was only one segment in this section.
- // FIRST HALF (concave part of accel curve)
+ // First half (concave part of accel curve)
if (mr.section_state == SECTION_1st_HALF) {
if (_exec_aline_segment() == STAT_OK) { // set up for second half
mr.section = SECTION_BODY;
return STAT_EAGAIN;
}
- // SECOND HALF (convex part of accel curve)
+ // Second half (convex part of accel curve)
if (mr.section_state == SECTION_2nd_HALF) {
#ifndef __KAHAN
mr.segment_velocity += mr.forward_diff_5;
#endif
if (_exec_aline_segment() == STAT_OK) { // set up for body
- if ((fp_ZERO(mr.body_length)) && (fp_ZERO(mr.tail_length)))
+ if (fp_ZERO(mr.body_length) && fp_ZERO(mr.tail_length))
return STAT_OK; // ends the move
mr.section = SECTION_BODY;
return STAT_EAGAIN;
}
-#endif // __ JERK_EXEC
-/// The body is broken into little segments even though it is a
-/// straight line so that feedholds can happen in the middle of a line
-/// with a minimum of latency
-static stat_t _exec_aline_body() {
- if (mr.section_state == SECTION_NEW) {
- if (fp_ZERO(mr.body_length)) {
- mr.section = SECTION_TAIL;
- return _exec_aline_tail(); // skip ahead to tail periods
- }
-
- mr.gm.move_time = mr.body_length / mr.cruise_velocity;
- mr.segments = ceil(uSec(mr.gm.move_time) / NOM_SEGMENT_USEC);
- mr.segment_time = mr.gm.move_time / mr.segments;
- mr.segment_velocity = mr.cruise_velocity;
- mr.segment_count = (uint32_t)mr.segments;
-
- if (mr.segment_time < MIN_SEGMENT_TIME)
- return STAT_MINIMUM_TIME_MOVE; // exit without advancing position
-
- mr.section = SECTION_BODY;
- // uses PERIOD_2 so last segment detection works
- mr.section_state = SECTION_2nd_HALF;
- }
-
- if (mr.section_state == SECTION_2nd_HALF) // straight part (period 3)
- if (_exec_aline_segment() == STAT_OK) { // OK means this section is done
- if (fp_ZERO(mr.tail_length)) return STAT_OK; // ends the move
- mr.section = SECTION_TAIL;
- mr.section_state = SECTION_NEW;
- }
-
- return STAT_EAGAIN;
-}
-
-
-#ifdef __JERK_EXEC
-static stat_t _exec_aline_tail() {
- if (mr.section_state == SECTION_NEW) { // INITIALIZATION
- if (fp_ZERO(mr.tail_length)) return STAT_OK; // end the move
-
- mr.midpoint_velocity = (mr.cruise_velocity + mr.exit_velocity) / 2;
- mr.gm.move_time = mr.tail_length / mr.midpoint_velocity;
- // # of segments in *each half*
- mr.segments = ceil(uSec(mr.gm.move_time) / (2 * NOM_SEGMENT_USEC));
- // time to advance for each segment
- mr.segment_time = mr.gm.move_time / (2 * mr.segments);
- mr.accel_time = 2 * sqrt((mr.cruise_velocity - mr.exit_velocity) / mr.jerk);
- mr.midpoint_acceleration =
- 2 * (mr.cruise_velocity - mr.exit_velocity) / mr.accel_time;
- // time to advance for each segment
- mr.segment_accel_time = mr.accel_time / (2 * mr.segments);
- // compute time from midpoint of segment
- mr.elapsed_accel_time = mr.segment_accel_time / 2;
- mr.segment_count = (uint32_t)mr.segments;
- if (mr.segment_time < MIN_SEGMENT_TIME)
- return STAT_MINIMUM_TIME_MOVE; // exit /wo advancing position
- mr.section = SECTION_TAIL;
- mr.section_state = SECTION_1st_HALF;
- }
-
- // FIRST HALF - convex part (period 4)
- if (mr.section_state == SECTION_1st_HALF) {
- mr.segment_velocity =
- mr.cruise_velocity - (square(mr.elapsed_accel_time) * mr.jerk_div2);
-
- if (_exec_aline_segment() == STAT_OK) { // set up for second half
- mr.segment_count = (uint32_t)mr.segments;
- mr.section_state = SECTION_2nd_HALF;
- // start time from midpoint of segment
- mr.elapsed_accel_time = mr.segment_accel_time / 2;
- }
-
- return STAT_EAGAIN;
- }
-
- // SECOND HALF - concave part (period 5)
- if (mr.section_state == SECTION_2nd_HALF) {
- mr.segment_velocity = mr.midpoint_velocity -
- (mr.elapsed_accel_time * mr.midpoint_acceleration) +
- (square(mr.elapsed_accel_time) * mr.jerk_div2);
- return _exec_aline_segment(); // ends the move or continues EAGAIN
- }
-
- return STAT_EAGAIN; // should never get here
-}
-
-
-#else // __JERK_EXEC -- run forward differencing math
+/// helper for deceleration section
static stat_t _exec_aline_tail() {
- if (mr.section_state == SECTION_NEW) { // INITIALIZATION
+ if (mr.section_state == SECTION_NEW) { // Initialization
if (fp_ZERO(mr.tail_length)) return STAT_OK; // end the move
// len/avg. velocity
mr.segment_time = mr.gm.move_time / mr.segments;
_init_forward_diffs(mr.cruise_velocity, mr.exit_velocity);
mr.segment_count = (uint32_t)mr.segments;
+
if (mr.segment_time < MIN_SEGMENT_TIME)
return STAT_MINIMUM_TIME_MOVE; // exit /wo advancing position
+
mr.section = SECTION_TAIL;
mr.section_state = SECTION_1st_HALF;
}
- // FIRST HALF - convex part (period 4)
+ // First half - convex part (period 4)
if (mr.section_state == SECTION_1st_HALF) {
if (_exec_aline_segment() == STAT_OK) {
// For forward differencing we should have one segment in
return STAT_EAGAIN;
}
- // SECOND HALF - concave part (period 5)
+ // Second half - concave part (period 5)
if (mr.section_state == SECTION_2nd_HALF) {
#ifndef __KAHAN
mr.segment_velocity += mr.forward_diff_5;
return STAT_EAGAIN;
}
-#endif // __JERK_EXEC
+#endif // !__JERK_EXEC
/* Segment runner helper
// 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)
copy_vector(mr.gm.target, mr.waypoint[mr.section]);
// works for Cartesian kinematics. Other kinematics may require
// transforming travel distance as opposed to simply subtracting
// steps.
-
for (i = 0; i < MOTORS; i++) {
// previous segment's position, delayed by 1 segment
mr.commanded_steps[i] = mr.position_steps[i];
* 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
+ * OTHER LIABILITY, WHETHER IN AN ACTIN OF CONTRACT, TORT OR
* OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE
* OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
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
+ if (!(bp = mp_get_run_buffer())) 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; // length left in mr buffer for deceleration
// Check if we are done
if (done) {
// Update machine position
- for (uint8_t axis = 0; axis < AXES; axis++) {
+ for (int motor = 0; motor < MOTORS; motor++) {
+ int axis = st_cfg.mot[motor].motor_map;
float steps = en_read_encoder(axis);
-
- for (int motor = 0; motor < MOTORS; motor++)
- if (st_cfg.mot[motor].motor_map == axis)
- cm_set_position(axis, steps / st_cfg.mot[motor].steps_per_unit);
+ cm_set_position(axis, steps / st_cfg.mot[motor].steps_per_unit);
}
// Release queue
* loading. See stepper.c for details.
*/
void ik_kinematics(const float travel[], float steps[]) {
- float joint[AXES];
-
// you can insert inverse kinematics transformations here
- // _inverse_kinematics(travel, joint);
-
- //...or just do a memcpy for Cartesian machines
- memcpy(joint, travel, sizeof(float) * AXES);
+ // float joint[AXES];
+ // _inverse_kinematics(travel, joint);
+ // ...or just use travel directly for Cartesian machines
// Map motors to axes and convert length units to steps
// 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;
-
- for (int i = 0; i < MOTORS; i++)
- if (st_cfg.mot[i].motor_map == axis)
- steps[i] = joint[axis] * st_cfg.mot[i].steps_per_unit;
+ for (int motor = 0; motor < MOTORS; motor++) {
+ int axis = st_cfg.mot[motor].motor_map;
+ if (cm.a[axis].axis_mode == AXIS_INHIBITED) steps[motor] = 0;
+ else steps[motor] = travel[axis] * st_cfg.mot[motor].steps_per_unit;
}
}
#include <math.h>
-static void _calc_move_times(GCodeState_t *gms, const float axis_length[],
- const float axis_square[]);
-static float _get_junction_vmax(const float a_unit[], const float b_unit[]);
+/* 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)
+ * 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:
+ * 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."
+ *
+ * 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.
+ *
+ * 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
+ * d Delta of sums (Dx*Ux+DY*UY)/Usum
+ */
+static float _get_junction_vmax(const float a_unit[], const float b_unit[]) {
+ float costheta =
+ -a_unit[AXIS_X] * b_unit[AXIS_X] -
+ a_unit[AXIS_Y] * b_unit[AXIS_Y] -
+ a_unit[AXIS_Z] * b_unit[AXIS_Z] -
+ a_unit[AXIS_A] * b_unit[AXIS_A] -
+ a_unit[AXIS_B] * b_unit[AXIS_B] -
+ a_unit[AXIS_C] * b_unit[AXIS_C];
+ if (costheta < -0.99) return 10000000; // straight line cases
+ if (costheta > 0.99) return 0; // reversal cases
-/// Correct velocity in last segment for reporting purposes
-void mp_zero_segment_velocity() {mr.segment_velocity = 0;}
+ // Fuse the junction deviations into a vector sum
+ float a_delta = square(a_unit[AXIS_X] * cm.a[AXIS_X].junction_dev);
+ a_delta += square(a_unit[AXIS_Y] * cm.a[AXIS_Y].junction_dev);
+ a_delta += square(a_unit[AXIS_Z] * cm.a[AXIS_Z].junction_dev);
+ a_delta += square(a_unit[AXIS_A] * cm.a[AXIS_A].junction_dev);
+ a_delta += square(a_unit[AXIS_B] * cm.a[AXIS_B].junction_dev);
+ a_delta += square(a_unit[AXIS_C] * cm.a[AXIS_C].junction_dev);
+ float b_delta = square(b_unit[AXIS_X] * cm.a[AXIS_X].junction_dev);
+ b_delta += square(b_unit[AXIS_Y] * cm.a[AXIS_Y].junction_dev);
+ b_delta += square(b_unit[AXIS_Z] * cm.a[AXIS_Z].junction_dev);
+ b_delta += square(b_unit[AXIS_A] * cm.a[AXIS_A].junction_dev);
+ b_delta += square(b_unit[AXIS_B] * cm.a[AXIS_B].junction_dev);
+ b_delta += square(b_unit[AXIS_C] * cm.a[AXIS_C].junction_dev);
-/// Returns current velocity (aggregate)
-float mp_get_runtime_velocity() {return mr.segment_velocity;}
+ float delta = (sqrt(a_delta) + sqrt(b_delta)) / 2;
+ float sintheta_over2 = sqrt((1 - costheta) / 2);
+ float radius = delta * sintheta_over2 / (1 - sintheta_over2);
+ float velocity = sqrt(radius * cm.junction_acceleration);
+ return velocity;
+}
-/// Returns current axis position in machine coordinates
-float mp_get_runtime_absolute_position(uint8_t axis) {return mr.position[axis];}
+/*
+ * 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.
+ *
+ * "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 following times are compared and the longest is returned:
+ * - G93 inverse time (if G93 is active)
+ * - time for coordinated move at requested feed rate
+ * - time that the slowest axis would require for the move
+ *
+ * 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.
+ *
+ * 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.
+ */
+static void _calc_move_times(GCodeState_t *gms, const float axis_length[],
+ const float axis_square[]) {
+ // gms = Gcode model state
+ float inv_time = 0; // inverse time if doing a feed in G93 mode
+ float xyz_time = 0; // linear coordinated move at requested feed
+ float abc_time = 0; // rotary coordinated move at requested feed
+ float max_time = 0; // time required for the rate-limiting axis
+ float tmp_time = 0; // used in computation
+ gms->minimum_time = 8675309; // arbitrarily large number
-/// Set offsets in the MR struct
-void mp_set_runtime_work_offset(float offset[]) {
- copy_vector(mr.gm.work_offset, offset);
-}
+ // compute times for feed motion
+ if (gms->motion_mode != MOTION_MODE_STRAIGHT_TRAVERSE) {
+ if (gms->feed_rate_mode == INVERSE_TIME_MODE) {
+ // feed rate was un-inverted to minutes by cm_set_feed_rate()
+ inv_time = gms->feed_rate;
+ gms->feed_rate_mode = UNITS_PER_MINUTE_MODE;
+ } else {
+ // compute length of linear move in millimeters. Feed rate is provided as
+ // mm/min
+ xyz_time = sqrt(axis_square[AXIS_X] + axis_square[AXIS_Y] +
+ axis_square[AXIS_Z]) / gms->feed_rate;
-/// Returns current axis position in work coordinates
-/// that were in effect at move planning time
-float mp_get_runtime_work_position(uint8_t axis) {
- return mr.position[axis] - mr.gm.work_offset[axis];
-}
+ // if no linear axes, compute length of multi-axis rotary move in degrees.
+ // Feed rate is provided as degrees/min
+ if (fp_ZERO(xyz_time))
+ abc_time = sqrt(axis_square[AXIS_A] + axis_square[AXIS_B] +
+ axis_square[AXIS_C]) / gms->feed_rate;
+ }
+ }
+
+ for (uint8_t axis = 0; axis < AXES; axis++) {
+ if (gms->motion_mode == MOTION_MODE_STRAIGHT_TRAVERSE)
+ tmp_time = fabs(axis_length[axis]) / cm.a[axis].velocity_max;
+
+ else // MOTION_MODE_STRAIGHT_FEED
+ tmp_time = fabs(axis_length[axis]) / cm.a[axis].feedrate_max;
+ max_time = max(max_time, tmp_time);
-/// Return TRUE if motion control busy (i.e. robot is moving)
-/// Use this function to sync to the queue. If you wait until it returns
-/// FALSE you know the queue is empty and the motors have stopped.
-uint8_t mp_get_runtime_busy() {
- return st_runtime_isbusy() || mr.move_state == MOVE_RUN;
+ if (tmp_time > 0) // collect minimum time if this axis is not zero
+ gms->minimum_time = min(gms->minimum_time, tmp_time);
+ }
+
+ gms->move_time = max4(inv_time, max_time, xyz_time, abc_time);
}
}
-/*
- * 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.
- *
- * "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 following times are compared and the longest is returned:
- * - G93 inverse time (if G93 is active)
- * - time for coordinated move at requested feed rate
- * - time that the slowest axis would require for the move
- *
- * 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.
- *
- * 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.
- */
-static void _calc_move_times(GCodeState_t *gms, const float axis_length[],
- const float axis_square[]) {
- // gms = Gcode model state
- float inv_time = 0; // inverse time if doing a feed in G93 mode
- float xyz_time = 0; // linear coordinated move at requested feed
- float abc_time = 0; // rotary coordinated move at requested feed
- float max_time = 0; // time required for the rate-limiting axis
- float tmp_time = 0; // used in computation
- gms->minimum_time = 8675309; // arbitrarily large number
-
- // compute times for feed motion
- if (gms->motion_mode != MOTION_MODE_STRAIGHT_TRAVERSE) {
- if (gms->feed_rate_mode == INVERSE_TIME_MODE) {
- // feed rate was un-inverted to minutes by cm_set_feed_rate()
- inv_time = gms->feed_rate;
- gms->feed_rate_mode = UNITS_PER_MINUTE_MODE;
-
- } else {
- // compute length of linear move in millimeters. Feed rate is provided as
- // mm/min
- xyz_time = sqrt(axis_square[AXIS_X] + axis_square[AXIS_Y] +
- axis_square[AXIS_Z]) / gms->feed_rate;
-
- // if no linear axes, compute length of multi-axis rotary move in degrees.
- // Feed rate is provided as degrees/min
- if (fp_ZERO(xyz_time))
- abc_time = sqrt(axis_square[AXIS_A] + axis_square[AXIS_B] +
- axis_square[AXIS_C]) / gms->feed_rate;
- }
- }
-
- for (uint8_t axis = 0; axis < AXES; axis++) {
- if (gms->motion_mode == MOTION_MODE_STRAIGHT_TRAVERSE)
- tmp_time = fabs(axis_length[axis]) / cm.a[axis].velocity_max;
-
- else // MOTION_MODE_STRAIGHT_FEED
- tmp_time = fabs(axis_length[axis]) / cm.a[axis].feedrate_max;
-
- max_time = max(max_time, tmp_time);
-
- if (tmp_time > 0) // collect minimum time if this axis is not zero
- gms->minimum_time = min(gms->minimum_time, tmp_time);
- }
-
- gms->move_time = max4(inv_time, max_time, xyz_time, abc_time);
-}
-
-
/* Plans the entire block list
*
* The block list is the circular buffer of planner buffers
// 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) {
+ if (bp->pv == bf || *mr_flag) {
bp->entry_velocity = bp->entry_vmax; // first block in the list
*mr_flag = false;
bp->exit_velocity = 0;
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)
- *
- * 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)
- * 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:
- * 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."
- *
- * 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.
- *
- * 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
- * d Delta of sums (Dx*Ux+DY*UY)/Usum
- */
-static float _get_junction_vmax(const float a_unit[], const float b_unit[]) {
- float costheta =
- -a_unit[AXIS_X] * b_unit[AXIS_X] -
- a_unit[AXIS_Y] * b_unit[AXIS_Y] -
- a_unit[AXIS_Z] * b_unit[AXIS_Z] -
- a_unit[AXIS_A] * b_unit[AXIS_A] -
- a_unit[AXIS_B] * b_unit[AXIS_B] -
- a_unit[AXIS_C] * b_unit[AXIS_C];
-
- if (costheta < -0.99) return 10000000; // straight line cases
- if (costheta > 0.99) return 0; // reversal cases
-
- // Fuse the junction deviations into a vector sum
- float a_delta = square(a_unit[AXIS_X] * cm.a[AXIS_X].junction_dev);
- a_delta += square(a_unit[AXIS_Y] * cm.a[AXIS_Y].junction_dev);
- a_delta += square(a_unit[AXIS_Z] * cm.a[AXIS_Z].junction_dev);
- a_delta += square(a_unit[AXIS_A] * cm.a[AXIS_A].junction_dev);
- a_delta += square(a_unit[AXIS_B] * cm.a[AXIS_B].junction_dev);
- a_delta += square(a_unit[AXIS_C] * cm.a[AXIS_C].junction_dev);
-
- float b_delta = square(b_unit[AXIS_X] * cm.a[AXIS_X].junction_dev);
- b_delta += square(b_unit[AXIS_Y] * cm.a[AXIS_Y].junction_dev);
- b_delta += square(b_unit[AXIS_Z] * cm.a[AXIS_Z].junction_dev);
- b_delta += square(b_unit[AXIS_A] * cm.a[AXIS_A].junction_dev);
- b_delta += square(b_unit[AXIS_B] * cm.a[AXIS_B].junction_dev);
- b_delta += square(b_unit[AXIS_C] * cm.a[AXIS_C].junction_dev);
-
- float delta = (sqrt(a_delta) + sqrt(b_delta)) / 2;
- float sintheta_over2 = sqrt((1 - costheta) / 2);
- float radius = delta * sintheta_over2 / (1 - sintheta_over2);
- float velocity = sqrt(radius * cm.junction_acceleration);
-
- return velocity;
-}
}
+/// Correct velocity in last segment for reporting purposes
+void mp_zero_segment_velocity() {mr.segment_velocity = 0;}
+
+
+/// Returns current velocity (aggregate)
+float mp_get_runtime_velocity() {return mr.segment_velocity;}
+
+
+/// Returns current axis position in machine coordinates
+float mp_get_runtime_absolute_position(uint8_t axis) {return mr.position[axis];}
+
+
+/// Set offsets in the MR struct
+void mp_set_runtime_work_offset(float offset[]) {
+ copy_vector(mr.gm.work_offset, offset);
+}
+
+
+/// Returns current axis position in work coordinates
+/// that were in effect at move planning time
+float mp_get_runtime_work_position(uint8_t axis) {
+ return mr.position[axis] - mr.gm.work_offset[axis];
+}
+
+
+/// Return TRUE if motion control busy (i.e. robot is moving)
+/// Use this function to sync to the queue. If you wait until it returns
+/// FALSE you know the queue is empty and the motors have stopped.
+uint8_t mp_get_runtime_busy() {
+ return st_runtime_isbusy() || mr.move_state == MOVE_RUN;
+}
void mp_set_planner_position(uint8_t axis, const float position);
void mp_set_runtime_position(uint8_t axis, const float position);
void mp_set_steps_to_runtime_position();
-
-// line.c functions
float mp_get_runtime_velocity();
float mp_get_runtime_work_position(uint8_t axis);
float mp_get_runtime_absolute_position(uint8_t axis);
void mp_set_runtime_work_offset(float offset[]);
void mp_zero_segment_velocity();
uint8_t mp_get_runtime_busy();
-float* mp_get_planner_position_vector();
+
+// line.c functions
void mp_plan_block_list(mpBuf_t *bf, uint8_t *mr_flag);
stat_t mp_aline(GCodeState_t *gm_in);
stat_t mp_dwell(const float seconds);
// command.c functions
-typedef void (*cm_exec_t)(float[], float[]);
-void mp_queue_command(cm_exec_t, float *value, float *flag);
+void mp_queue_command(cm_exec_t cm_exec, float *value, float *flag);
void mp_runtime_command(mpBuf_t *bf);
#pragma once
-/************************************************************************************
- * STATUS CODES
+/* Status codes
*
- * Status codes are divided into ranges for clarity and extensibility. At some point
- * this may break down and the whole thing will get messy(er), but it's advised not
- * to change the values of existing status codes once they are in distribution.
+ * Status codes are divided into ranges for clarity and extensibility. At some
+ * point this may break down and the whole thing will get messy(er), but it's
+ * advised not to change the values of existing status codes once they are in
+ * distribution.
*
* Ranges are:
*
- * 0 - 19 OS, communications and low-level status
+ * 0 - 19 OS, communications and low-level status
*
- * 20 - 99 Generic internal and application errors. Internal errors start at 20 and work up,
- * Assertion failures start at 99 and work down.
+ * 20 - 99 Generic internal and application errors. Internal errors start at
+ * 20 and work up, Assertion failures start at 99 and work down.
*
- * 100 - 129 Generic data and input errors - not specific to Gcode or TinyG
+ * 100 - 129 Generic data and input errors - not specific to Gcode or TinyG
*
- * 130 - Gcode and TinyG application errors and warnings
+ * 130 - Gcode and TinyG application errors and warnings
*
- * See main.c for associated message strings. Any changes to the codes may also require
- * changing the message strings and string array in main.c
+ * See main.c for associated message strings. Any changes to the codes may also
+ * require changing the message strings and string array in main.c
*
- * Most of the status codes (except STAT_OK) below are errors which would fail the command,
- * and are returned by the failed command and reported back via JSON or text.
- * Some status codes are warnings do not fail the command. These can be used to generate
- * an exception report. These are labeled as WARNING
+ * Most of the status codes (except STAT_OK) below are errors which would fail
+ * the command, and are returned by the failed command and reported back via
+ * JSON or text. Some status codes are warnings do not fail the command. These
+ * can be used to generate an exception report. These are labeled as WARNING
*/
#include <stdint.h>
// OS, communications and low-level status
#define STAT_OK 0 // function completed OK
#define STAT_ERROR 1 // generic error return EPERM
-#define STAT_EAGAIN 2 // function would block here (call again)
+#define STAT_EAGAIN 2 // function would block (call again)
#define STAT_NOOP 3 // function had no-operation
#define STAT_COMPLETE 4 // operation is complete
#define STAT_TERMINATE 5 // operation terminated (gracefully)
#define STAT_BUFFER_FULL 13
#define STAT_BUFFER_FULL_FATAL 14
#define STAT_INITIALIZING 15 // initializing - not ready for use
-#define STAT_ENTERING_BOOT_LOADER 16 // this code actually emitted from boot loader, not TinyG
+#define STAT_ENTERING_BOOT_LOADER 16 // emitted from boot loader
#define STAT_FUNCTION_IS_STUBBED 17
// Internal errors and startup messages
#define STAT_INTERNAL_ERROR 20 // unrecoverable internal error
-#define STAT_INTERNAL_RANGE_ERROR 21 // number range other than by user input
+#define STAT_INTERNAL_RANGE_ERROR 21 // other than by user input
#define STAT_FLOATING_POINT_ERROR 22 // number conversion error
#define STAT_DIVIDE_BY_ZERO 23
#define STAT_INVALID_ADDRESS 24
#define STAT_PERSISTENCE_ERROR 34
#define STAT_BAD_STATUS_REPORT_SETTING 35
-// Assertion failures - build down from 99 until they meet the system internal errors
+// Assertion failures - down from 99 until they meet system internal errors
#define STAT_CONFIG_ASSERTION_FAILURE 90
#define STAT_ENCODER_ASSERTION_FAILURE 92
#define STAT_STEPPER_ASSERTION_FAILURE 93
#define STAT_CANONICAL_MACHINE_ASSERTION_FAILURE 95
#define STAT_CONTROLLER_ASSERTION_FAILURE 96
#define STAT_STACK_OVERFLOW 97
-#define STAT_MEMORY_FAULT 98 // generic memory corruption
-#define STAT_GENERIC_ASSERTION_FAILURE 99 // generic assertion failure - unclassified
+#define STAT_MEMORY_FAULT 98
+#define STAT_GENERIC_ASSERTION_FAILURE 99
// Application and data input errors
// Generic data input errors
-#define STAT_UNRECOGNIZED_NAME 100 // parser didn't recognize the name
-#define STAT_INVALID_OR_MALFORMED_COMMAND 101 // malformed line to parser
-#define STAT_BAD_NUMBER_FORMAT 102 // number format error
-#define STAT_UNSUPPORTED_TYPE 103 // An otherwise valid number or JSON type is not supported
-#define STAT_PARAMETER_IS_READ_ONLY 104 // input error: parameter cannot be set
-#define STAT_PARAMETER_CANNOT_BE_READ 105 // input error: parameter cannot be set
-#define STAT_COMMAND_NOT_ACCEPTED 106 // command cannot be accepted at this time
-#define STAT_INPUT_EXCEEDS_MAX_LENGTH 107 // input string is too long
-#define STAT_INPUT_LESS_THAN_MIN_VALUE 108 // input error: value is under minimum
-#define STAT_INPUT_EXCEEDS_MAX_VALUE 109 // input error: value is over maximum
-
-#define STAT_INPUT_VALUE_RANGE_ERROR 110 // input error: value is out-of-range
-#define STAT_JSON_SYNTAX_ERROR 111 // JSON input string is not well formed
-#define STAT_JSON_TOO_MANY_PAIRS 112 // JSON input string has too many JSON pairs
-#define STAT_JSON_TOO_LONG 113 // JSON input or output exceeds buffer size
+#define STAT_UNRECOGNIZED_NAME 100
+#define STAT_INVALID_OR_MALFORMED_COMMAND 101
+#define STAT_BAD_NUMBER_FORMAT 102
+#define STAT_UNSUPPORTED_TYPE 103
+#define STAT_PARAMETER_IS_READ_ONLY 104
+#define STAT_PARAMETER_CANNOT_BE_READ 105
+#define STAT_COMMAND_NOT_ACCEPTED 106
+#define STAT_INPUT_EXCEEDS_MAX_LENGTH 107
+#define STAT_INPUT_LESS_THAN_MIN_VALUE 108
+#define STAT_INPUT_EXCEEDS_MAX_VALUE 109
+
+#define STAT_INPUT_VALUE_RANGE_ERROR 110
+#define STAT_JSON_SYNTAX_ERROR 111
+#define STAT_JSON_TOO_MANY_PAIRS 112
+#define STAT_JSON_TOO_LONG 113
// Gcode errors and warnings (Most originate from NIST - by concept, not number)
// Fascinating: http://www.cncalarms.com/
-#define STAT_GCODE_GENERIC_INPUT_ERROR 130 // generic error for gcode input
-#define STAT_GCODE_COMMAND_UNSUPPORTED 131 // G command is not supported
-#define STAT_MCODE_COMMAND_UNSUPPORTED 132 // M command is not supported
-#define STAT_GCODE_MODAL_GROUP_VIOLATION 133 // gcode modal group error
-#define STAT_GCODE_AXIS_IS_MISSING 134 // command requires at least one axis present
-#define STAT_GCODE_AXIS_CANNOT_BE_PRESENT 135 // error if G80 has axis words
-#define STAT_GCODE_AXIS_IS_INVALID 136 // an axis is specified that is illegal for the command
-#define STAT_GCODE_AXIS_IS_NOT_CONFIGURED 137 // WARNING: attempt to program an axis that is disabled
-#define STAT_GCODE_AXIS_NUMBER_IS_MISSING 138 // axis word is missing its value
-#define STAT_GCODE_AXIS_NUMBER_IS_INVALID 139 // axis word value is illegal
-
-#define STAT_GCODE_ACTIVE_PLANE_IS_MISSING 140 // active plane is not programmed
-#define STAT_GCODE_ACTIVE_PLANE_IS_INVALID 141 // active plane selected is not valid for this command
-#define STAT_GCODE_FEEDRATE_NOT_SPECIFIED 142 // move has no feedrate
-#define STAT_GCODE_INVERSE_TIME_MODE_CANNOT_BE_USED 143 // G38.2 and some canned cycles cannot accept inverse time mode
-#define STAT_GCODE_ROTARY_AXIS_CANNOT_BE_USED 144 // G38.2 and some other commands cannot have rotary axes
-#define STAT_GCODE_G53_WITHOUT_G0_OR_G1 145 // G0 or G1 must be active for G53
+#define STAT_GCODE_GENERIC_INPUT_ERROR 130
+#define STAT_GCODE_COMMAND_UNSUPPORTED 131
+#define STAT_MCODE_COMMAND_UNSUPPORTED 132
+#define STAT_GCODE_MODAL_GROUP_VIOLATION 133
+#define STAT_GCODE_AXIS_IS_MISSING 134
+#define STAT_GCODE_AXIS_CANNOT_BE_PRESENT 135
+#define STAT_GCODE_AXIS_IS_INVALID 136
+#define STAT_GCODE_AXIS_IS_NOT_CONFIGURED 137
+#define STAT_GCODE_AXIS_NUMBER_IS_MISSING 138
+#define STAT_GCODE_AXIS_NUMBER_IS_INVALID 139
+
+#define STAT_GCODE_ACTIVE_PLANE_IS_MISSING 140
+#define STAT_GCODE_ACTIVE_PLANE_IS_INVALID 141
+#define STAT_GCODE_FEEDRATE_NOT_SPECIFIED 142
+#define STAT_GCODE_INVERSE_TIME_MODE_CANNOT_BE_USED 143
+#define STAT_GCODE_ROTARY_AXIS_CANNOT_BE_USED 144
+#define STAT_GCODE_G53_WITHOUT_G0_OR_G1 145
#define STAT_REQUESTED_VELOCITY_EXCEEDS_LIMITS 146
#define STAT_CUTTER_COMPENSATION_CANNOT_BE_ENABLED 147
#define STAT_PROGRAMMED_POINT_SAME_AS_CURRENT_POINT 148
#define STAT_S_WORD_IS_MISSING 151
#define STAT_S_WORD_IS_INVALID 152
#define STAT_SPINDLE_MUST_BE_OFF 153
-#define STAT_SPINDLE_MUST_BE_TURNING 154 // some canned cycles require spindle to be turning when called
-#define STAT_ARC_SPECIFICATION_ERROR 155 // generic arc specification error
-#define STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE 156 // arc is missing axis (axes) required by selected plane
-#define STAT_ARC_OFFSETS_MISSING_FOR_SELECTED_PLANE 157 // one or both offsets are not specified
-#define STAT_ARC_RADIUS_OUT_OF_TOLERANCE 158 // WARNING - radius arc is too small or too large - accuracy in question
+#define STAT_SPINDLE_MUST_BE_TURNING 154
+#define STAT_ARC_SPECIFICATION_ERROR 155
+#define STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE 156
+#define STAT_ARC_OFFSETS_MISSING_FOR_SELECTED_PLANE 157
+#define STAT_ARC_RADIUS_OUT_OF_TOLERANCE 158
#define STAT_ARC_ENDPOINT_IS_STARTING_POINT 159
-#define STAT_P_WORD_IS_MISSING 160 // P must be present for dwells and other functions
-#define STAT_P_WORD_IS_INVALID 161 // generic P value error
+#define STAT_P_WORD_IS_MISSING 160
+#define STAT_P_WORD_IS_INVALID 161
#define STAT_P_WORD_IS_ZERO 162
-#define STAT_P_WORD_IS_NEGATIVE 163 // dwells require positive P values
-#define STAT_P_WORD_IS_NOT_AN_INTEGER 164 // G10s and other commands require integer P numbers
+#define STAT_P_WORD_IS_NEGATIVE 163
+#define STAT_P_WORD_IS_NOT_AN_INTEGER 164
#define STAT_P_WORD_IS_NOT_VALID_TOOL_NUMBER 165
#define STAT_D_WORD_IS_MISSING 166
#define STAT_D_WORD_IS_INVALID 167
// TinyG errors and warnings
#define STAT_GENERIC_ERROR 200
-#define STAT_MINIMUM_LENGTH_MOVE 201 // move is less than minimum length
-#define STAT_MINIMUM_TIME_MOVE 202 // move is less than minimum time
-#define STAT_MACHINE_ALARMED 203 // machine is alarmed. Command not processed
-#define STAT_LIMIT_SWITCH_HIT 204 // a limit switch was hit causing shutdown
-#define STAT_PLANNER_FAILED_TO_CONVERGE 205 // trapezoid generator can through this exception
-
-#define STAT_SOFT_LIMIT_EXCEEDED 220 // soft limit error - axis unspecified
-#define STAT_SOFT_LIMIT_EXCEEDED_XMIN 221 // soft limit error - X minimum
-#define STAT_SOFT_LIMIT_EXCEEDED_XMAX 222 // soft limit error - X maximum
-#define STAT_SOFT_LIMIT_EXCEEDED_YMIN 223 // soft limit error - Y minimum
-#define STAT_SOFT_LIMIT_EXCEEDED_YMAX 224 // soft limit error - Y maximum
-#define STAT_SOFT_LIMIT_EXCEEDED_ZMIN 225 // soft limit error - Z minimum
-#define STAT_SOFT_LIMIT_EXCEEDED_ZMAX 226 // soft limit error - Z maximum
-#define STAT_SOFT_LIMIT_EXCEEDED_AMIN 227 // soft limit error - A minimum
-#define STAT_SOFT_LIMIT_EXCEEDED_AMAX 228 // soft limit error - A maximum
-#define STAT_SOFT_LIMIT_EXCEEDED_BMIN 229 // soft limit error - B minimum
-
-#define STAT_SOFT_LIMIT_EXCEEDED_BMAX 220 // soft limit error - B maximum
-#define STAT_SOFT_LIMIT_EXCEEDED_CMIN 231 // soft limit error - C minimum
-#define STAT_SOFT_LIMIT_EXCEEDED_CMAX 232 // soft limit error - C maximum
-
-#define STAT_HOMING_CYCLE_FAILED 240 // homing cycle did not complete
+#define STAT_MINIMUM_LENGTH_MOVE 201 // move is less than minimum length
+#define STAT_MINIMUM_TIME_MOVE 202 // move is less than minimum time
+#define STAT_MACHINE_ALARMED 203 // machine is alarmed
+#define STAT_LIMIT_SWITCH_HIT 204 // a limit switch was hit
+#define STAT_PLANNER_FAILED_TO_CONVERGE 205 // trapezoid generator throws this
+
+#define STAT_SOFT_LIMIT_EXCEEDED 220 // axis unspecified
+#define STAT_SOFT_LIMIT_EXCEEDED_XMIN 221
+#define STAT_SOFT_LIMIT_EXCEEDED_XMAX 222
+#define STAT_SOFT_LIMIT_EXCEEDED_YMIN 223
+#define STAT_SOFT_LIMIT_EXCEEDED_YMAX 224
+#define STAT_SOFT_LIMIT_EXCEEDED_ZMIN 225
+#define STAT_SOFT_LIMIT_EXCEEDED_ZMAX 226
+#define STAT_SOFT_LIMIT_EXCEEDED_AMIN 227
+#define STAT_SOFT_LIMIT_EXCEEDED_AMAX 228
+#define STAT_SOFT_LIMIT_EXCEEDED_BMIN 229
+#define STAT_SOFT_LIMIT_EXCEEDED_BMAX 220
+#define STAT_SOFT_LIMIT_EXCEEDED_CMIN 231
+#define STAT_SOFT_LIMIT_EXCEEDED_CMAX 232
+
+#define STAT_HOMING_CYCLE_FAILED 240 // homing cycle did not complete
#define STAT_HOMING_ERROR_BAD_OR_NO_AXIS 241
#define STAT_HOMING_ERROR_ZERO_SEARCH_VELOCITY 242
#define STAT_HOMING_ERROR_ZERO_LATCH_VELOCITY 243
#define STAT_HOMING_ERROR_NEGATIVE_LATCH_BACKOFF 245
#define STAT_HOMING_ERROR_SWITCH_MISCONFIGURATION 246
-#define STAT_PROBE_CYCLE_FAILED 250 // probing cycle did not complete
+#define STAT_PROBE_CYCLE_FAILED 250 // probing cycle did not complete
#define STAT_PROBE_ENDPOINT_IS_STARTING_POINT 251
-#define STAT_JOGGING_CYCLE_FAILED 252 // jogging cycle did not complete
-// !!! Do not exceed 255 without also changing stat_t typedef
+// Do not exceed 255 without also changing stat_t typedef
#include <stdio.h>
+#define DDA_PERIOD (F_CPU / STEP_CLOCK_FREQ)
+
+enum {MOTOR_1, MOTOR_2, MOTOR_3, MOTOR_4};
+
+
stConfig_t st_cfg;
stPrepSingleton_t st_pre;
static stRunSingleton_t st_run;
+
static void _load_move();
static void _request_load_move();
static void _update_steps_per_unit(int motor);
-#define _f_to_period(f) (uint16_t)((float)F_CPU / (float)f)
-
-enum {
- MOTOR_1,
- MOTOR_2,
- MOTOR_3,
- MOTOR_4,
- MOTOR_5,
- MOTOR_6,
-};
-
-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:
void stepper_init() {
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;
-
- // setup ports and data structures
+ // Setup ports
for (uint8_t i = 0; i < MOTORS; i++) {
- // 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
}
- // setup DDA timer
+ // Setup DDA timer
TIMER_DDA.CTRLA = STEP_TIMER_DISABLE; // turn timer off
TIMER_DDA.CTRLB = STEP_TIMER_WGMODE; // waveform mode
TIMER_DDA.INTCTRLA = TIMER_DDA_INTLVL; // interrupt mode
st_pre.buffer_state = PREP_BUFFER_OWNED_BY_EXEC;
- // defaults
+ // Defaults
st_cfg.motor_power_timeout = MOTOR_IDLE_TIMEOUT;
st_cfg.mot[MOTOR_1].motor_map = M1_MOTOR_MAP;
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 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].microsteps = M2_MICROSTEPS;
st_cfg.mot[MOTOR_2].polarity = M2_POLARITY;
st_cfg.mot[MOTOR_2].power_mode = M2_POWER_MODE;
-#endif
-#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].microsteps = M3_MICROSTEPS;
st_cfg.mot[MOTOR_3].polarity = M3_POLARITY;
st_cfg.mot[MOTOR_3].power_mode = M3_POWER_MODE;
-#endif
-#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].microsteps = M4_MICROSTEPS;
st_cfg.mot[MOTOR_4].polarity = M4_POLARITY;
st_cfg.mot[MOTOR_4].power_mode = M4_POWER_MODE;
-#endif
// Init steps per unit
for (int m = 0; m < MOTORS; m++) _update_steps_per_unit(m);
}
+/// returns 1 if motor is enabled (motor is actually active low)
static uint8_t _motor_is_enabled(uint8_t motor) {
- uint8_t port = 0xff;
- if (motor < 4) port = vports[motor]->OUT;
- // returns 1 if motor is enabled (motor is actually active low)
+ uint8_t port = motor < MOTORS ? hw.st_port[motor]->OUT : 0xff;
return port & MOTOR_ENABLE_BIT_bm ? 0 : 1;
}
/// Remove power from a motor
static void _deenergize_motor(const uint8_t motor) {
- if (motor < 4) vports[motor]->OUT |= MOTOR_ENABLE_BIT_bm;
+ if (motor < MOTORS) hw.st_port[motor]->OUTSET = 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;
- }
+ if (st_cfg.mot[motor].power_mode == MOTOR_DISABLED) _deenergize_motor(motor);
- if (motor < 4) vports[motor]->OUT &= ~MOTOR_ENABLE_BIT_bm;
- st_run.mot[motor].power_state = MOTOR_POWER_TIMEOUT_START;
+ else {
+ if (motor < MOTORS) hw.st_port[motor]->OUTCLR = 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++) {
+ for (uint8_t motor = 0; motor < MOTORS; motor++) {
_energize_motor(motor);
st_run.mot[motor].power_state = MOTOR_POWER_TIMEOUT_START;
}
/// Remove power from all motors
void st_deenergize_motors() {
- for (uint8_t motor = MOTOR_1; motor < MOTORS; motor++)
+ for (uint8_t motor = 0; motor < MOTORS; motor++)
_deenergize_motor(motor);
}
/// 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
- for (uint8_t m = 0; m < MOTORS; m++) {
- // 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
- if (st_cfg.mot[m].power_mode == MOTOR_ALWAYS_POWERED) {
- if (!_motor_is_enabled(m)) _energize_motor(m);
- continue;
- }
-
- // 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;
+ for (int motor = 0; motor < MOTORS; motor++)
+ switch (st_cfg.mot[motor].power_mode) {
+ case MOTOR_ALWAYS_POWERED:
+ if (!_motor_is_enabled(motor)) _energize_motor(motor);
+ break;
+
+ case MOTOR_POWERED_IN_CYCLE: case MOTOR_POWERED_ONLY_WHEN_MOVING:
+ if (st_run.mot[motor].power_state == MOTOR_POWER_TIMEOUT_START) {
+ // Start a countdown
+ st_run.mot[motor].power_state = MOTOR_POWER_TIMEOUT_COUNTDOWN;
+ st_run.mot[motor].power_systick = rtc_get_time() +
+ (st_cfg.motor_power_timeout * 1000);
+ }
- // 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;
- _deenergize_motor(m);
+ // Run the countdown if not in feedhold and in a countdown
+ if (cm_get_combined_state() != COMBINED_HOLD &&
+ st_run.mot[motor].power_state == MOTOR_POWER_TIMEOUT_COUNTDOWN &&
+ rtc_get_time() > st_run.mot[motor].power_systick) {
+ st_run.mot[motor].power_state = MOTOR_IDLE;
+ _deenergize_motor(motor);
report_request(); // request a status report when motors shut down
}
+ break;
+
+ default: _deenergize_motor(motor); break; // Motor disabled
}
- }
return STAT_OK;
}
st_run.mot[motor].substep_accumulator += st_run.mot[motor].substep_increment;
if (0 < st_run.mot[motor].substep_accumulator) {
- vports[motor]->OUT ^= STEP_BIT_bm; // toggle step line
+ hw.st_port[motor]->OUTTGL = STEP_BIT_bm; // toggle step line
st_run.mot[motor].substep_accumulator -= st_run.dda_ticks_X_substeps;
INCREMENT_ENCODER(motor);
}
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);
+ -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;
+ hw.st_port[motor]->OUTCLR = DIRECTION_BIT_bm;
+ else hw.st_port[motor]->OUTSET = DIRECTION_BIT_bm;
}
SET_ENCODER_STEP_SIGN(motor, prep_mot->step_sign);
// Enable the stepper and start motor power management
if ((run_mot->substep_increment && cfg_mot->power_mode != MOTOR_DISABLED) ||
cfg_mot->power_mode == MOTOR_POWERED_IN_CYCLE) {
- vports[motor]->OUT &= ~MOTOR_ENABLE_BIT_bm; // energize motor
+ hw.st_port[motor]->OUTCLR = MOTOR_ENABLE_BIT_bm; // energize motor
run_mot->power_state = MOTOR_POWER_TIMEOUT_START; // start power management
}
// 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; // no more moves to load
+ if (st_runtime_isbusy() || st_pre.buffer_state != PREP_BUFFER_OWNED_BY_LOADER)
+ return; // exit if the runtime is busy or there are no more moves
st_run.move_type = st_pre.move_type;
- // Handle aline loads first (most common case)
- if (st_pre.move_type == MOVE_TYPE_ALINE) {
- // Setup the new segment
+ switch (st_pre.move_type) {
+ case 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;
_load_motor_move(MOTOR_4);
// do this last
- TIMER_DDA.PER = st_pre.dda_period;
+ TIMER_DDA.PER = DDA_PERIOD;
TIMER_DDA.CTRLA = STEP_TIMER_ENABLE; // enable the DDA timer
+ break;
- } else if (st_pre.move_type == MOVE_TYPE_DWELL) {
- // handle dwells
+ case MOVE_TYPE_DWELL: // handle dwells
st_run.dda_ticks_downcount = st_pre.dda_ticks;
- TIMER_DDA.PER = st_pre.dda_period; // load dwell timer period
+ TIMER_DDA.PER = DDA_PERIOD; // load dwell timer period
TIMER_DDA.CTRLA = STEP_TIMER_ENABLE; // enable the dwell timer
+ break;
- } else if (st_pre.move_type == MOVE_TYPE_COMMAND)
- // handle synchronous commands
+ case MOVE_TYPE_COMMAND: // handle synchronous commands
mp_runtime_command(st_pre.bf);
+ // Fall through
- // all other cases drop to here (e.g. Null moves after Mcodes skip to here)
+ default:
+ TIMER_DDA.CTRLA = STEP_TIMER_DISABLE;
+ break;
+ }
+
+ // all other cases skip to here (e.g. Null moves after Mcodes skip to here)
st_pre.move_type = MOVE_TYPE_0;
+
// 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
// 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);
- // converts minutes to seconds
- st_pre.dda_ticks = (int32_t)(segment_time * 60 * FREQUENCY_DDA);
+ // convert minutes to seconds
+ st_pre.dda_ticks = (int32_t)(segment_time * 60 * STEP_CLOCK_FREQ);
st_pre.dda_ticks_X_substeps = st_pre.dda_ticks * DDA_SUBSTEPS;
// setup motor parameters
/// Add a dwell to the move buffer
void st_prep_dwell(float microseconds) {
st_pre.move_type = MOVE_TYPE_DWELL;
- st_pre.dda_period = _f_to_period(FREQUENCY_DDA);
- st_pre.dda_ticks = microseconds / 1000000 * FREQUENCY_DDA;
+ st_pre.dda_ticks = microseconds / 1000000 * STEP_CLOCK_FREQ;
st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // signal prep buffer ready
}
};
+enum {
+ MOTOR_POLARITY_NORMAL,
+ MOTOR_POLARITY_REVERSED
+};
+
+
/// Min/Max timeouts allowed for motor disable. Allow for inertial stop.
/// Must be non-zero
#define MOTOR_TIMEOUT_SECONDS_MIN (float)0.1
*
* MAX_LONG 2^31, maximum signed long (depth of accumulator.
* values are negative)
- * FREQUENCY_DDA DDA clock rate in Hz.
+ * STEP_CLOCK_FREQ 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
* millisecond segments
*/
#define DDA_SUBSTEPS \
- ((MAX_LONG * 0.90) / (FREQUENCY_DDA * (NOM_SEGMENT_TIME * 60)))
+ ((MAX_LONG * 0.90) / (STEP_CLOCK_FREQ * (NOM_SEGMENT_TIME * 60)))
/* Step correction settings
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
continue;
}
- if (sw.count[i] == 0) { // trigger point
+ if (!sw.count[i]) { // trigger point
sw.sw_num_thrown = i; // record number of thrown switch
sw.debounce[i] = SW_LOCKOUT;
/// read a switch directly with no interrupts or deglitching
uint8_t read_switch(uint8_t sw_num) {
- if ((sw_num < 0) || (sw_num >= NUM_SWITCHES)) return SW_DISABLED;
+ if (sw_num < 0 || sw_num >= NUM_SWITCHES) return SW_DISABLED;
uint8_t read = 0;
switch (sw_num) {
// A NO switch drives the pin LO when thrown
if (sw.switch_type == SW_TYPE_NORMALLY_OPEN)
- sw.state[sw_num] = (read == 0) ? SW_CLOSED : SW_OPEN;
- else sw.state[sw_num] = (read != 0) ? SW_CLOSED : SW_OPEN;
+ sw.state[sw_num] = read ? SW_OPEN : SW_CLOSED;
+ else sw.state[sw_num] = read ? SW_CLOSED : SW_OPEN;
return sw.state[sw_num];
}
#define SW_DEGLITCH_TICKS 3 // 3=30ms
// switch modes
-#define SW_HOMING_BIT 0x01
-#define SW_LIMIT_BIT 0x02
+#define SW_HOMING_BIT 1
+#define SW_LIMIT_BIT 2
#define SW_MODE_DISABLED 0 // disabled for all operations
#define SW_MODE_HOMING SW_HOMING_BIT // enable switch for homing only
#define SW_MODE_LIMIT SW_LIMIT_BIT // enable switch for limits only
/// Test if vectors are equal
uint8_t vector_equal(const float a[], const float b[]) {
- if ((fp_EQ(a[AXIS_X], b[AXIS_X])) &&
- (fp_EQ(a[AXIS_Y], b[AXIS_Y])) &&
- (fp_EQ(a[AXIS_Z], b[AXIS_Z])) &&
- (fp_EQ(a[AXIS_A], b[AXIS_A])) &&
- (fp_EQ(a[AXIS_B], b[AXIS_B])) &&
- (fp_EQ(a[AXIS_C], b[AXIS_C])))
- return true;
-
-
- return false;
+ return
+ fp_EQ(a[AXIS_X], b[AXIS_X]) && fp_EQ(a[AXIS_Y], b[AXIS_Y]) &&
+ fp_EQ(a[AXIS_Z], b[AXIS_Z]) && fp_EQ(a[AXIS_A], b[AXIS_A]) &&
+ fp_EQ(a[AXIS_B], b[AXIS_B]) && fp_EQ(a[AXIS_C], b[AXIS_C]);
}
vector[AXIS_A] = a;
vector[AXIS_B] = b;
vector[AXIS_C] = c;
+
return vector;
}
/// load a single value into a zero vector
float *set_vector_by_axis(float value, uint8_t axis) {
clear_vector(vector);
+
switch (axis) {
- case (AXIS_X): vector[AXIS_X] = value; break;
- case (AXIS_Y): vector[AXIS_Y] = value; break;
- case (AXIS_Z): vector[AXIS_Z] = value; break;
- case (AXIS_A): vector[AXIS_A] = value; break;
- case (AXIS_B): vector[AXIS_B] = value; break;
- case (AXIS_C): vector[AXIS_C] = value;
+ case AXIS_X: vector[AXIS_X] = value; break;
+ case AXIS_Y: vector[AXIS_Y] = value; break;
+ case AXIS_Z: vector[AXIS_Z] = value; break;
+ case AXIS_A: vector[AXIS_A] = value; break;
+ case AXIS_B: vector[AXIS_B] = value; break;
+ case AXIS_C: vector[AXIS_C] = value; break;
}
+
return vector;
}
uint32_t h = 0;
uint16_t len = strlen(string);
- if (length != 0) len = min(len, length);
+ if (length) len = min(len, length);
- for (uint16_t i=0; i<len; i++)
+ for (uint16_t i = 0; i < len; i++)
h = 31 * h + string[i];
return h % HASHMASK;
projects / bbctrl-firmware / commitdiff