From d448557e329ca57a0b7cec9a7eff8abdc304935c Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Sat, 12 Mar 2016 18:07:27 -0800 Subject: [PATCH] Cleanup and optimizations, 80-columns! --- src/canonical_machine.c | 815 ++++++++++++++++++++-------------------- src/canonical_machine.h | 632 ++++++++++++++++--------------- src/config.h | 125 +++--- src/cycle_homing.c | 291 +++++++------- src/cycle_probing.c | 221 ++++++----- src/gcode_parser.c | 19 +- src/gpio.c | 41 +- src/gpio.h | 4 +- src/hardware.c | 9 +- src/hardware.h | 4 +- src/plan/arc.c | 270 +++++++------ src/plan/buffer.c | 4 +- src/plan/command.c | 3 +- src/plan/exec.c | 427 +++++++++++---------- src/plan/feedhold.c | 5 +- src/plan/jog.c | 8 +- src/plan/kinematics.c | 19 +- src/plan/line.c | 448 ++++++++++------------ src/plan/planner.c | 31 ++ src/plan/planner.h | 8 +- src/status.h | 174 +++++---- src/stepper.c | 169 ++++----- src/stepper.h | 11 +- src/switch.c | 8 +- src/switch.h | 4 +- src/util.c | 33 +- 26 files changed, 1884 insertions(+), 1899 deletions(-) diff --git a/src/canonical_machine.c b/src/canonical_machine.c index c62d371..9e69b94 100644 --- a/src/canonical_machine.c +++ b/src/canonical_machine.c @@ -32,76 +32,74 @@ * 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" @@ -124,9 +122,11 @@ #include #include -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); @@ -135,17 +135,9 @@ static void _exec_flood_coolant_control(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; @@ -155,7 +147,8 @@ uint8_t cm_get_combined_state() { 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; } @@ -167,6 +160,7 @@ uint8_t cm_get_hold_state() {return cm.hold_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; @@ -178,26 +172,63 @@ void cm_set_motion_state(uint8_t 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) { @@ -233,71 +264,72 @@ void cm_set_model_linenum(uint32_t linenum) { } -/*********************************************************************************** - * 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++) @@ -305,15 +337,13 @@ void cm_set_work_offsets(GCodeState_t *gcode_state) { } -/* - * 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]; @@ -321,17 +351,16 @@ float cm_get_absolute_position(GCodeState_t *gcode_state, uint8_t 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) */ @@ -348,21 +377,22 @@ float cm_get_work_position(GCodeState_t *gcode_state, uint8_t axis) { } -/*********************************************************************************** - * 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 @@ -381,13 +411,13 @@ void cm_update_model_position_from_runtime() { } -/* - * 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 @@ -395,17 +425,20 @@ void cm_update_model_position_from_runtime() { * 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); @@ -421,9 +454,11 @@ void cm_set_model_target(float target[], float flag[]) { 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]); } } @@ -435,21 +470,23 @@ void cm_set_model_target(float target[], float flag[]) { 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) @@ -458,10 +495,12 @@ stat_t cm_test_soft_limits(float target[]) { 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; } @@ -469,27 +508,25 @@ stat_t cm_test_soft_limits(float target[]) { } -/************************************************************************* - * 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; @@ -679,40 +716,33 @@ stat_t cm_hard_alarm(stat_t status) { 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. @@ -723,7 +753,7 @@ stat_t cm_set_distance_mode(uint8_t mode) { */ 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++) @@ -736,10 +766,8 @@ stat_t cm_set_coord_offsets(uint8_t coord_system, float offset[], // 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; @@ -752,33 +780,34 @@ stat_t cm_set_coord_system(uint8_t 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 @@ -790,18 +819,19 @@ void cm_set_position(uint8_t axis, float position) { } -/*** 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]; @@ -824,22 +854,18 @@ static void _exec_absolute_origin(float *value, float *flag) { 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; @@ -848,47 +874,50 @@ stat_t cm_set_origin_offsets(float offset[], float flag[]) { 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[]) { @@ -900,74 +929,77 @@ 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; @@ -982,12 +1014,7 @@ stat_t cm_set_path_control(uint8_t 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) @@ -1000,7 +1027,7 @@ stat_t cm_dwell(float 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; @@ -1020,26 +1047,17 @@ stat_t cm_straight_feed(float target[], float flags[]) { } -/***************************** - * 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; } @@ -1050,8 +1068,9 @@ static void _exec_select_tool(float *value, float *flag) { } +/// 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; } @@ -1062,13 +1081,8 @@ static void _exec_change_tool(float *value, float *flag) { } -/*********************************** - * 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); @@ -1079,13 +1093,14 @@ stat_t cm_mist_coolant_control(uint8_t mist_coolant) { 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; } @@ -1094,28 +1109,22 @@ stat_t cm_flood_coolant_control(uint8_t flood_coolant) { 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; @@ -1123,7 +1132,8 @@ stat_t cm_override_enables(uint8_t flag) { // M48, M49 } -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; @@ -1132,14 +1142,16 @@ stat_t cm_feed_rate_override_enable(uint8_t flag) { // M50 } -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; @@ -1148,14 +1160,16 @@ stat_t cm_traverse_override_enable(uint8_t flag) { // M50.2 } -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; @@ -1164,7 +1178,8 @@ stat_t cm_spindle_override_enable(uint8_t flag) { // M51.1 } -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; @@ -1172,27 +1187,14 @@ stat_t cm_spindle_override_factor(uint8_t flag) { // M50.1 } -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. @@ -1203,42 +1205,41 @@ void cm_message(char *message) { * 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 } @@ -1246,21 +1247,22 @@ stat_t cm_feedhold_sequencing_callback() { 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(); @@ -1272,7 +1274,7 @@ stat_t cm_feedhold_sequencing_callback() { 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 @@ -1280,28 +1282,22 @@ stat_t cm_queue_flush() { // 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) @@ -1312,9 +1308,11 @@ stat_t cm_queue_flush() { * 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 @@ -1328,17 +1326,17 @@ static void _exec_program_finalize(float *value, float *flag) { 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 @@ -1348,6 +1346,7 @@ static void _exec_program_finalize(float *value, float *flag) { } +/// Do a cycle start right now void cm_cycle_start() { cm.machine_state = MACHINE_CYCLE; @@ -1356,28 +1355,33 @@ void cm_cycle_start() { } +/// 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); } @@ -1385,33 +1389,28 @@ void cm_program_end() { /// 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); diff --git a/src/canonical_machine.h b/src/canonical_machine.h index f4b13f1..f3422c3 100644 --- a/src/canonical_machine.h +++ b/src/canonical_machine.h @@ -6,26 +6,32 @@ * * 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 . * - * 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 + * . + * + * 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 @@ -36,180 +42,203 @@ #include -#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 @@ -219,14 +248,17 @@ typedef struct cmSingleton { // struct to manage cm globals and cycles uint8_t distance_mode; // G90,G91 reset default // coordinate systems and offsets - float offset[COORDS + 1][AXES]; // persistent coordinate offsets: absolute (G53) + G54,G55,G56,G57,G58,G59 + // 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 @@ -239,9 +271,11 @@ typedef struct cmSingleton { // struct to manage cm globals and cycles 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 @@ -254,100 +288,97 @@ typedef struct cmSingleton { // struct to manage cm globals and cycles 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 }; @@ -355,11 +386,12 @@ enum cmProbeState { // applies to cm.probe_state * 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 @@ -375,7 +407,7 @@ enum cmNextAction { // these are in order to optimized CASE st 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 @@ -393,46 +425,47 @@ enum cmMotionMode { // G Modal Group 1 }; -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 @@ -443,65 +476,72 @@ enum cmCoordSystem { #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 @@ -580,7 +620,8 @@ void cm_set_motion_mode(GCodeState_t *gcode_state, uint8_t motion_mode); 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 @@ -597,96 +638,99 @@ stat_t cm_deferred_write_callback(); 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(); diff --git a/src/config.h b/src/config.h index e51993b..fc8e3bb 100644 --- a/src/config.h +++ b/src/config.h @@ -17,16 +17,11 @@ #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) @@ -35,7 +30,7 @@ #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 @@ -43,47 +38,29 @@ #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 @@ -97,9 +74,11 @@ #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 @@ -113,7 +92,7 @@ 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 @@ -213,8 +192,7 @@ // 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 @@ -233,51 +211,37 @@ #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 @@ -308,16 +272,10 @@ enum cfgPortBits { // motor control port bit positions #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) @@ -335,23 +293,24 @@ enum cfgPortBits { // motor control port bit positions */ // 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 diff --git a/src/cycle_homing.c b/src/cycle_homing.c index a96b640..47e2849 100644 --- a/src/cycle_homing.c +++ b/src/cycle_homing.c @@ -45,28 +45,38 @@ #include -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 @@ -79,7 +89,7 @@ struct hmHomingSingleton { // persistent homing runtime variables 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); @@ -92,62 +102,61 @@ static stat_t _homing_error_exit(int8_t axis, stat_t status); 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); @@ -159,7 +168,7 @@ stat_t cm_homing_cycle_start() { // 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; @@ -173,41 +182,31 @@ stat_t cm_homing_cycle_start() { 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); @@ -216,7 +215,7 @@ static stat_t _homing_axis_start(int8_t axis) { 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 @@ -229,7 +228,8 @@ static stat_t _homing_axis_start(int8_t axis) { // 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); @@ -238,50 +238,56 @@ static stat_t _homing_axis_start(int8_t axis) { 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); @@ -289,15 +295,18 @@ static stat_t _homing_axis_clear(int8_t axis) { // first clear mov } -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) @@ -309,35 +318,37 @@ static stat_t _homing_axis_latch(int8_t axis) { // latch to switch } -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)); @@ -347,31 +358,33 @@ static stat_t _homing_axis_move(int8_t axis, float target, float velocity) { /// 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); @@ -384,44 +397,49 @@ static stat_t _homing_finalize_exit(int8_t axis) { // third part of return to ho } -/* - * 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; @@ -430,30 +448,35 @@ static int8_t _get_next_axis(int8_t axis) { 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 diff --git a/src/cycle_probing.c b/src/cycle_probing.c index 32f1ade..3544c49 100644 --- a/src/cycle_probing.c +++ b/src/cycle_probing.c @@ -1,28 +1,33 @@ -/* - * 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 . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #include "canonical_machine.h" @@ -39,31 +44,31 @@ #include #include -/**** 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(); @@ -72,101 +77,97 @@ static stat_t _probing_finalize_exit(); 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 - - -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) { diff --git a/src/gpio.h b/src/gpio.h index 06f21d6..221e8fa 100644 --- a/src/gpio.h +++ b/src/gpio.h @@ -33,9 +33,11 @@ #pragma once - #include + +enum {SPINDLE_LED, SPINDLE_DIR_LED, SPINDLE_PWM_LED, COOLANT_LED}; + void indicator_led_set(); void indicator_led_clear(); void indicator_led_toggle(); diff --git a/src/hardware.c b/src/hardware.c index 6dda9b8..fd1ea15 100644 --- a/src/hardware.c +++ b/src/hardware.c @@ -43,6 +43,9 @@ #include +hwSingleton_t hw; + + /// Bind XMEGA ports to hardware static void _port_bindings() { hw.st_port[0] = &PORT_MOTOR_1; @@ -70,10 +73,10 @@ void hardware_init() { } -/* - * 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 @@ -122,7 +125,7 @@ void hw_request_hard_reset() {cs.hard_reset_requested = true;} /// 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 } diff --git a/src/hardware.h b/src/hardware.h index 61eb2ee..5251016 100644 --- a/src/hardware.h +++ b/src/hardware.h @@ -36,7 +36,6 @@ #pragma once - #include "status.h" #include "config.h" @@ -55,7 +54,8 @@ typedef struct { 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); diff --git a/src/plan/arc.c b/src/plan/arc.c index e581951..2200373 100644 --- a/src/plan/arc.c +++ b/src/plan/arc.c @@ -22,7 +22,7 @@ */ /* 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. */ @@ -47,39 +47,41 @@ static void _estimate_arc_time(); 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; @@ -110,11 +112,13 @@ stat_t cm_arc_feed(float target[], float flags[], // arc endpoints 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 @@ -125,21 +129,22 @@ stat_t cm_arc_feed(float target[], float flags[], // arc endpoints // 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()); @@ -148,15 +153,14 @@ stat_t cm_arc_feed(float target[], float flags[], // arc endpoints 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. @@ -165,14 +169,15 @@ stat_t cm_arc_feed(float target[], float flags[], // arc endpoints */ stat_t cm_arc_callback() { if (arc.run_state == MOVE_OFF) return STAT_NOOP; - if (mp_get_planner_buffers_available() < PLANNER_BUFFER_HEADROOM) return STAT_EAGAIN; + 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; @@ -189,69 +194,84 @@ void cm_abort_arc() { } -/* - * 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; } } @@ -261,16 +281,21 @@ static stat_t _compute_arc() { 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; @@ -280,35 +305,37 @@ static stat_t _compute_arc() { 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 @@ -330,26 +357,29 @@ static stat_t _compute_arc() { * 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 * @@ -377,18 +407,19 @@ static stat_t _compute_arc_offsets_from_radius() { // 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 @@ -407,27 +438,34 @@ static stat_t _compute_arc_offsets_from_radius() { } -/* - * 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)); } diff --git a/src/plan/buffer.c b/src/plan/buffer.c index c4dc99c..ee6a02a 100644 --- a/src/plan/buffer.c +++ b/src/plan/buffer.c @@ -143,8 +143,8 @@ void mp_commit_write_buffer(const uint8_t move_type) { */ 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 diff --git a/src/plan/command.c b/src/plan/command.c index dd8fe15..1815a3b 100644 --- a/src/plan/command.c +++ b/src/plan/command.c @@ -70,8 +70,7 @@ static stat_t _exec_command(mpBuf_t *bf) { /// 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 diff --git a/src/plan/exec.c b/src/plan/exec.c index a3fd8f9..1db438d 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -80,12 +80,6 @@ stat_t mp_exec_move() { * * 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 @@ -126,10 +120,10 @@ stat_t mp_exec_move() { * * 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 @@ -145,14 +139,11 @@ stat_t mp_exec_move() { * 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; @@ -255,7 +246,161 @@ stat_t mp_exec_aline(mpBuf_t *bf) { 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 @@ -266,47 +411,47 @@ stat_t mp_exec_aline(mpBuf_t *bf) { * * 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 @@ -331,7 +476,8 @@ stat_t mp_exec_aline(mpBuf_t *bf) { * 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 * @@ -341,20 +487,23 @@ stat_t mp_exec_aline(mpBuf_t *bf) { * 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 @@ -362,16 +511,14 @@ stat_t mp_exec_aline(mpBuf_t *bf) { * 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; @@ -407,75 +554,9 @@ static void _init_forward_diffs(float Vi, float 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)) { @@ -503,7 +584,7 @@ static stat_t _exec_aline_head() { // 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; @@ -514,7 +595,7 @@ static stat_t _exec_aline_head() { 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; @@ -527,7 +608,7 @@ static stat_t _exec_aline_head() { #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; @@ -565,99 +646,11 @@ static stat_t _exec_aline_head() { 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 @@ -669,13 +662,15 @@ static stat_t _exec_aline_tail() { 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 @@ -690,7 +685,7 @@ static stat_t _exec_aline_tail() { 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; @@ -735,7 +730,7 @@ static stat_t _exec_aline_tail() { return STAT_EAGAIN; } -#endif // __JERK_EXEC +#endif // !__JERK_EXEC /* Segment runner helper @@ -767,7 +762,6 @@ static stat_t _exec_aline_segment() { // 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]); @@ -786,7 +780,6 @@ static stat_t _exec_aline_segment() { // 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]; diff --git a/src/plan/feedhold.c b/src/plan/feedhold.c index a8fdedb..2af8cf4 100644 --- a/src/plan/feedhold.c +++ b/src/plan/feedhold.c @@ -26,7 +26,7 @@ * 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. */ @@ -125,8 +125,7 @@ stat_t mp_plan_hold_callback() { 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 diff --git a/src/plan/jog.c b/src/plan/jog.c index 087f02f..f3658a1 100644 --- a/src/plan/jog.c +++ b/src/plan/jog.c @@ -56,12 +56,10 @@ static stat_t _exec_jog(mpBuf_t *bf) { // 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 diff --git a/src/plan/kinematics.c b/src/plan/kinematics.c index a89040d..ff7c375 100644 --- a/src/plan/kinematics.c +++ b/src/plan/kinematics.c @@ -51,24 +51,19 @@ * 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; } } diff --git a/src/plan/line.c b/src/plan/line.c index 895aa63..4995656 100644 --- a/src/plan/line.c +++ b/src/plan/line.c @@ -45,41 +45,222 @@ #include -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); } @@ -290,122 +471,6 @@ stat_t mp_aline(GCodeState_t *gm_in) { } -/* - * Compute optimal and minimum move times into the gcode_state - * - * "Minimum time" is the fastest the move can be performed given - * the velocity constraints on each participating axis - regardless - * of the feed rate requested. The minimum time is the time limited - * by the rate-limiting axis. The minimum time is needed to compute - * the optimal time and is recorded for possible feed override - * computation. - * - * "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 @@ -498,7 +563,7 @@ void mp_plan_block_list(mpBuf_t *bf, uint8_t *mr_flag) { // forward planning pass - recomputes trapezoids in the list from the first // block to the bf block. while ((bp = mp_get_next_buffer(bp)) != bf) { - if (bp->pv == bf || *mr_flag == true) { + if (bp->pv == bf || *mr_flag) { bp->entry_velocity = bp->entry_vmax; // first block in the list *mr_flag = false; @@ -527,106 +592,3 @@ void mp_plan_block_list(mpBuf_t *bf, uint8_t *mr_flag) { 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; -} diff --git a/src/plan/planner.c b/src/plan/planner.c index 507c9ea..4cf39a0 100644 --- a/src/plan/planner.c +++ b/src/plan/planner.c @@ -158,3 +158,34 @@ void mp_set_steps_to_runtime_position() { } +/// 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; +} diff --git a/src/plan/planner.h b/src/plan/planner.h index a18b88f..0099717 100644 --- a/src/plan/planner.h +++ b/src/plan/planner.h @@ -269,15 +269,14 @@ void mp_flush_planner(); 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); @@ -315,8 +314,7 @@ void mp_copy_buffer(mpBuf_t *bf, const mpBuf_t *bp); 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); diff --git a/src/status.h b/src/status.h index dade7a8..95c6444 100644 --- a/src/status.h +++ b/src/status.h @@ -32,31 +32,31 @@ #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 @@ -73,7 +73,7 @@ void print_status_message(const char *msg, stat_t status); // 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) @@ -87,12 +87,12 @@ void print_status_message(const char *msg, stat_t status); #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 @@ -109,7 +109,7 @@ void print_status_message(const char *msg, stat_t status); #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 @@ -117,47 +117,47 @@ void print_status_message(const char *msg, stat_t status); #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 @@ -167,18 +167,18 @@ void print_status_message(const char *msg, stat_t status); #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 @@ -198,28 +198,27 @@ void print_status_message(const char *msg, stat_t status); // 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 @@ -227,10 +226,9 @@ void print_status_message(const char *msg, stat_t status); #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 diff --git a/src/stepper.c b/src/stepper.c index d7344d7..8188790 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -53,34 +53,21 @@ #include +#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: @@ -93,27 +80,20 @@ static VPORT_t *vports[] = { 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; @@ -122,30 +102,27 @@ void stepper_init() { st_cfg.mot[MOTOR_1].microsteps = M1_MICROSTEPS; st_cfg.mot[MOTOR_1].polarity = M1_POLARITY; st_cfg.mot[MOTOR_1].power_mode = M1_POWER_MODE; -#if 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); @@ -171,36 +148,34 @@ void st_reset() { } +/// 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; } @@ -209,7 +184,7 @@ void st_energize_motors() { /// 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); } @@ -219,39 +194,32 @@ void st_deenergize_motors() { /// 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; } @@ -261,7 +229,7 @@ static inline void _step_motor(int motor) { 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); } @@ -355,11 +323,11 @@ static inline void _load_motor_move(int 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); @@ -368,7 +336,7 @@ static inline void _load_motor_move(int motor) { // 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 } @@ -394,15 +362,13 @@ static void _load_move() { // Be aware that dda_ticks_downcount must equal zero for the loader to run. // So the initial load must also have this set to zero as part of // initialization - if (st_runtime_isbusy()) return; // exit if the runtime is busy - if (st_pre.buffer_state != PREP_BUFFER_OWNED_BY_LOADER) - return; // 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; @@ -412,21 +378,28 @@ static void _load_move() { _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 @@ -477,9 +450,8 @@ stat_t st_prep_line(float travel_steps[], float following_error[], // 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 @@ -573,8 +545,7 @@ void st_prep_command(void *bf) { /// 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 } diff --git a/src/stepper.h b/src/stepper.h index 8ca3682..46cd8ce 100644 --- a/src/stepper.h +++ b/src/stepper.h @@ -320,6 +320,12 @@ enum cmMotorPowerMode { }; +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 @@ -346,7 +352,7 @@ enum cmMotorPowerMode { * * 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 @@ -355,7 +361,7 @@ enum cmMotorPowerMode { * 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 @@ -460,7 +466,6 @@ typedef struct stPrepSingleton { 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 diff --git a/src/switch.c b/src/switch.c index abbf828..d0eb517 100644 --- a/src/switch.c +++ b/src/switch.c @@ -165,7 +165,7 @@ void switch_rtc_callback() { 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; @@ -209,7 +209,7 @@ void reset_switches() { /// 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) { @@ -225,8 +225,8 @@ uint8_t read_switch(uint8_t 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]; } diff --git a/src/switch.h b/src/switch.h index a850ba1..8fd98bd 100644 --- a/src/switch.h +++ b/src/switch.h @@ -55,8 +55,8 @@ #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 diff --git a/src/util.c b/src/util.c index f98ce10..4f883c3 100644 --- a/src/util.c +++ b/src/util.c @@ -51,16 +51,10 @@ float vector[AXES]; // statically allocated global for vector utilities /// 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]); } @@ -83,6 +77,7 @@ float *set_vector(float x, float y, float z, float a, float b, float c) { vector[AXIS_A] = a; vector[AXIS_B] = b; vector[AXIS_C] = c; + return vector; } @@ -90,14 +85,16 @@ float *set_vector(float x, float y, float z, float a, float b, float c) { /// 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; } @@ -219,9 +216,9 @@ uint16_t compute_checksum(char const *string, const uint16_t length) { 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